[update]:更新一半齐次坐标变换

docs/chapt7/7.3.2动手学坐标变换.md

@@ -5,20 +5,100 @@
 本节课我们就通过对应的函数和库实现齐次矩阵的生成，齐次矩阵的乘法和求逆。
 
 1.齐次矩阵的合成
-齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态的表示我们用了四元数、欧拉角、轴角、旋转矩阵四种方式
+齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态可以用四元数、欧拉角、轴角、旋转矩阵四种方式来表示。
 
+所以我们考虑先将对应的姿态转成旋转矩阵，然后使用numpy讲旋转矩阵和平移向量填写到齐次矩阵对应的位置即可。
 
+1.1旋转矩阵+平移向量
 
+```
+#导入库
+import numpy as np
+import transforms3d as tfs
+```
+
+```
+# 定义旋转矩阵R和平移向量T
+R = np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]])
+T = np.asarray([1,0,1])
+R,T
+```
+
+![image-20220104124708934](7.3.2动手学坐标变换/imgs/image-20220104124708934.png)
+
+1.1.1 使用numpy方法合成齐次变换矩阵
+
+```
+temp = np.hstack((R,T.reshape(3,1)))
+np.vstack((temp,[0,0,0,1]))
+```
+
+![image-20220104124655166](7.3.2动手学坐标变换/imgs/image-20220104124655166.png)
+
+1.1.2 使用tfs中的函数合成齐次变换矩阵
+
+```
+tfs.affines.compose(T,R,[1,1,1])
+```
+
+![image-20220104124645988](7.3.2动手学坐标变换/imgs/image-20220104124645988.png)
+
+1.2四元数+平移向量
+
+思路:先将四元数转换成旋转矩阵，然后再利用1.1合成齐次矩阵
+
+```
+R = tfs.quaternions.quat2mat([1,0,0,0])
+tfs.affines.compose(T,R,[1,1,1])
+```
+
+![image-20220104124632233](7.3.2动手学坐标变换/imgs/image-20220104124632233.png)
 
 2.齐次矩阵的乘法
+对应numpy中矩阵的乘法`np.dot`讲两个矩阵相乘即可，我们以一道例题来讲解这个问题。
+
+2.1 练习-眼在手外
+
+如图🔓示，已知：
+
+![手眼系统的坐标系关系](7.3.2动手学坐标变换/imgs/image-20211104102656102.png)
+
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
+
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度
+
+可以参考下图看题目
+
+![坐标系关系图](7.3.2动手学坐标变换/imgs/image-20211108213832470.png)
+
+求：
+
+{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵
+
+解体思路也很简单，我们只要得出
+
+- B到C的齐次变换矩阵$^B_CT$
+- C到P的齐次变换矩阵$^C_PT$
+
+得到之后将两者相乘即可得出：
+$$
+^B_PT=^B_CT^C_PT
+$$
+求出$^B_PT$我们再将其分解成位置矢量和旋转矩阵即可
+
+动手写代码：
+
+
+
+
 
 3.齐次矩阵求逆
+3.1练习-眼在手上
 
 
-4.练习:坐标转换
-4.1眼在手外
-4.2眼在手上
-4.3点云坐标系转换
+4.练习
+4.1 map坐标系转换
+4.2 机械臂运动学正解
 
 --------------