齐次坐标变换的理解以及在无人机相机定位坐标系转换中的应用

一、平移齐次坐标变换

4*4矩阵的右边三个数表示平移，如果原来的向量u的w=0，那么就是u+（ai+bj+ck）

二、旋转齐次变换矩阵

对应xyz三个轴的循环变换，注意负号的位置

三、物体的变换

用描述空间一点的变换方法来描述物体在空间的位置和方向。

先变换的矩阵乘在右边。

四、齐次变化的逆

五、平移坐标变换

$${}^{A}p={}^{B}p+{}^A{p}_{B_o}$$

从$B$到$A$的平移坐标变换。

问题一

解：

CM先饶$z$轴旋转180°，在沿$x$轴反方向平移0.13m，得到M。
$$T_{CM}^M=Trans\left(-0.13,0,0\right)Rot\left(z,180^{°}\right)=\left[\begin{array}{cccc}1& 0& 0& -0.13\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}-1& 0& 0& 0\\ 0& -1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

$$T_{CM}^C=\left[\begin{array}{cc}{R}_{CM}^C& P_{CM}^C\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}\cos y& \sin y& 0& dx\\ -\sin y& \cos y& 0& dy\\ 0& 0& 1& dz\\ 0& 0& 0& 1\end{array}\right]$$

齐次矩阵的逆：

$$T_{CM}^M{T}_C^{CM}=\left[\begin{array}{cccc}-\cos y& \sin y& 0& dx\cos y-dy\sin y-0.13\\ -\sin y& -\cos y& 0& dx\sin y+dy\cos y\\ 0& 0& 1& -dz\\ 0& 0& 0& 1\end{array}\right]$$
把上式代入，得到：
$$[\begin{array}{c}{x}_{MCM}^M\\ y_{MCM}^M\\ z_{MCM}^M\\ 1\end{array}]+T_{CM}^M\left[\begin{array}{c}{x}_{CMC}^{CM}\\ y_{CMC}^{CM}\\ z_{CMC}^{CM}\\ 1\end{array}\right]+T_{CM}^M{T}_C^{CM}\left[\begin{array}{c}{x}_{CB}^C\\ y_{CB}^C\\ z_{CB}^C\\ 1\end{array}\right]$$

$$=\left[\begin{array}{l}0.13\\ 0\\ 0\\ 1\end{array}\right]+\left[\begin{array}{cccc}-1& 0& 0& -0.13\\ 0& -1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{l}x\\ y\\ z\\ 1\end{array}\right]+\left[\begin{array}{cccc}-\cos y& \sin y& 0& dx\cos y-dy\sin y-0.13\\ -\sin y& -\cos y& 0& dx\sin y+dy\cos y\\ 0& 0& 1& -dz\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}-0.13\\ 0\\ 0\\ 1\end{array}\right]$$

$$=\left[\begin{array}{c}-x+dx\cos y-dy\sin y+0.13\cos y-0.13\\ -y+dx\sin y+dy\cos y+0.13\sin y\\ z+dz\\ 1\end{array}\right]$$

注意到，在$dt$时间内，$dx,dy$是小量，可以略去，剩下的部分就是如下代码：

(r,p,y)=tf.transformations.euler_from_quaternion([data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z,data.pose.pose.orientation.w])
local_pose.pose.position.x = -data.pose.pose.position.x - rtt_ctr_b * (1-math.cos(y))
local_pose.pose.position.y = - data.pose.pose.position.y + rtt_ctr_b * math.sin(y) #推导y
local_pose.pose.position.z = data.pose.pose.position.z            #相机高度作为无人机的高度
local_pose.pose.orientation =  data.pose.pose.orientation