SLAM中的坐标系旋转变换以及旋转矩阵的左乘和右乘问题
对于一个变换矩阵 T T T，具体的定义因人而异，例如一个变换 T w c T_{wc} Twc，代表了从相机坐标系到世界坐标系的变换，但是其基准（base）坐标系有时候定义为 w w w，也可以定义为 c c c，基准坐标系的不同，直接导致变换相反。
例如对于下图中的三个简单的坐标系，分别为相机坐标系 c 1 ， c 2 c1，c2 c1，c2，世界坐标系 w w w，其中点 P P P 在坐标系 c 1 c1 c1 下。接下来定义变换矩阵，为了简化计算，其中三个坐标系没有旋转，只需要考虑平移。
-
对于变换矩阵 T c w T_{cw} Tcw，理解为
world到camera的变换，如果以camera为基准坐标系，所以如果想将点 P P P 旋转到 c 2 c2 c2 坐标系下，那么变换矩阵的定义：
T c 1 w = [ 1 0 0 0 0 1 0 − 1 0 0 1 0 0 0 0 1 ] T c 2 w = [ 1 0 0 − 1 0 1 0 0 0 0 1 0 0 0 0 1 ] T c 2 c 1 = [ 1 0 0 − 1 0 1 0 1 0 0 1 0 0 0 0 1 ] T_{c_1w} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] T_{c_2w} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] T_{c_2c_1} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] Tc1w=⎣⎢⎢⎡1000010000100−101⎦⎥⎥⎤Tc2w=⎣⎢⎢⎡100001000010−1001⎦⎥⎥⎤Tc2c1=⎣⎢⎢⎡100001000010−1101⎦⎥⎥⎤
那么将一个点从 c 1 c_1 c1 坐标系转换到 c 2 c_2 c2 坐标系，需要先转换到世界坐标系。那么如下：
c 2 P = T c 2 w T w c 1 c 1 P = T c 2 w T c 1 w − 1 c 1 P = [ 1 0 0 − 1 0 1 0 0 0 0 1 0 0 0 0 1 ] [ 1 0 0 0 0 1 0 1 0 0 1 0 0 0 0 1 ] [ 1 0 0 1 ] ^{c_2}P=T_{c_2w}T_{wc_1} {^{c_1}P} = T_{c_2w} T^{-1}_{c_1w}{^{c_1}P} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] \left[ \begin{array}{cc} 1 \\ 0 \\ 0 \\ \hline 1 \\ \end{array} \right] c2P=Tc2wTwc1c1P=Tc2wTc1w−1c1P=⎣⎢⎢⎡100001000010−1001⎦⎥⎥⎤⎣⎢⎢⎡1000010000100101⎦⎥⎥⎤⎣⎢⎢⎡1001⎦⎥⎥⎤ = [ 1 0 0 − 1 0 1 0 1 0 0 1 0 0 0 0 1 ] [ 1 0 0 1 ] = [ 0 1 0 1 ] = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] \left[ \begin{array}{cc} 1 \\ 0 \\ 0 \\ \hline 1 \\ \end{array} \right] =\left[ \begin{array}{cc} 0 \\ 1 \\ 0 \\ \hline 1 \\ \end{array} \right] =⎣⎢⎢⎡100001000010−1101⎦⎥⎥⎤⎣⎢⎢⎡1001⎦⎥⎥⎤=⎣⎢⎢⎡0101⎦⎥⎥⎤ -
那么对于变换矩阵 T c w T_{cw} Tcw，理解为
world到camera的变换，如果以world为基准坐标系，所以如果想将点 P P P 旋转到 c 2 c2 c2 坐标系下，那么变换矩阵的定义：
T c 1 w = [ 1 0 0 0 0 1 0 1 0 0 1 0 0 0 0 1 ] T c 2 w = [ 1 0 0 1 0 1 0 0 0 0 1 0 0 0 0 1 ] T c 2 c 1 = [ 1 0 0 1 0 1 0 − 1 0 0 1 0 0 0 0 1 ] T_{c_1w} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] T_{c_2w} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] T_{c_2c_1} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & -1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] Tc1w=⎣⎢⎢⎡1000010000100101⎦⎥⎥⎤Tc2w=⎣⎢⎢⎡1000010000101001⎦⎥⎥⎤Tc2c1=⎣⎢⎢⎡1000010000101−101⎦⎥⎥⎤
对于点P，假设其姿态与c1坐标系一致，则
T p c 1 = [ 1 0 0 1 0 1 0 0 0 0 1 0 0 0 0 1 ] T_{pc_1} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] Tpc1=⎣⎢⎢⎡1000010000101001⎦⎥⎥⎤
那么可以得到，从c2到P的变换，等于从c2到c1的变换乘c1到P的变换：
T p c 2 = T c 1 c 2 T p c 1 = [ 1 0 0 − 1 0 1 0 1 0 0 1 0 0 0 0 1 ] [ 1 0 0 1 0 1 0 0 0 0 1 0 0 0 0 1 ] = [ 1 0 0 0 0 1 0 1 0 0 1 0 0 0 0 1 ] T_{pc_2} = T_{c_1c_2}T_{pc_1} = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] = \left[ \begin{array}{ccc|c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1& 0 \\ \hline 0 & 0 & 0 & 1 \\ \end{array} \right] Tpc2=Tc1c2Tpc1=⎣⎢⎢⎡100001000010−1101⎦⎥⎥⎤⎣⎢⎢⎡1000010000101001⎦⎥⎥⎤=⎣⎢⎢⎡1000010000100101⎦⎥⎥⎤所以在c2坐标系下，P点的坐标为 ( 0 , 1 , 0 ) ( 0 , 1 ,0) (0,1,0)
对于上面两种定义方式，都可以求得正确的结果，但是定义不同，计算方式就不同。
旋转的左乘与右乘未完待更…