debug过程中，矩阵左乘右乘相关概念梳理

1. 变换点或者变换向量

1.1左乘

矩阵左乘通常是指对”目标点“进行左乘，即:
$A^{\prime }=R\ast A$
其中，A为原始3维点，表示一个3*1的列向量，R为33的旋转矩阵，A‘为变换后的点
$B^{\prime }=T\ast B$
其中，B为原始点3维点，表示一个4*1的齐次化列向量，T为44的旋转矩阵R|t，B‘为变换后的点
以此类推，
如果是点云 c l o u d s r c = { X s r c ∣ X s r c = A 1 , A 2 … … A n } cloud_{src}=\{X_{src}|X_{src}=A_1,A_2……A_n\} cloudsrc​={Xsrc​∣Xsrc​=A1​,A2​……An​}，A表示一个3*1的列向量
此时$X_{src}$为一个3*n的矩阵，那么变换可以表示为
$X_A^{\prime }=R\ast X_A$
$X_B^{\prime }=T\ast X_B$

1.1.1矩阵与旋转角

上面为3维点的变换，为了方便画图解释下面以2维点进行描述：

$P_A=R\ast P_B$
将矩阵乘法展开可以写为：
[ P x A P y A P z A ] = [ c o s ( α ) − s i n ( α ) 0 s i n ( α ) c o s ( α ) 0 0 0 1 ] ∗ [ P x B P y B P z B ] [PxAPyAPzA]

= [cos(α)−sin(α)0sin(α)cos(α)0001]*[PxBPyBPzB] ​PxA​PyA​PzA​​ ​= ​cos(α)sin(α)0​−sin(α)cos(α)0​001​ ​∗ ​PxB​PyB​PzB​​ ​

上面图片表示的是一个矩阵的左乘，其中旋转矩阵R表达的是B点绕z轴逆时针旋转$\alpha$度，得到A点。
如果是旋转一个坐标系的话，那么上面的矩阵表示的就是坐标B系绕Z轴顺时针旋转$\alpha$度，得到A坐标系。

PS：此处顺逆时针都是Z轴朝上的，如果Z轴朝下，表达方式会有所不同。
这两个的表达是一个意思，矩阵表达也是一样的。（原因在于：虽然都是左乘，顺逆时针虽然相反，但是旋转矩阵R的选择是等价的，因此，从方程表达上是一样的。）
Z轴逆时针旋转点的R矩阵<==>Z轴顺时针旋转坐标系的R矩阵
[ c o s ( α ) − s i n ( α ) 0 s i n ( α ) c o s ( α ) 0 0 0 1 ] [cos(α)−sin(α)0sin(α)cos(α)0001]

​cos(α)sin(α)0​−sin(α)cos(α)0​001​ ​

Z轴顺时针旋转点的R矩阵<==>Z轴逆时针旋转坐标系的R矩阵
[ c o s ( α ) s i n ( α ) 0 − s i n ( α ) c o s ( α ) 0 0 0 1 ] [cos(α)sin(α)0−sin(α)cos(α)0001]

​cos(α)−sin(α)0​sin(α)cos(α)0​001​ ​

1.2右乘

没有具体例子：

1.3 左乘右乘同时存在的场景

针对transform增量（例如A坐标系下的R|t），变换坐标系的场景

求：点云$X_A^{world}$与点云$X_B^{world}$进行icp匹配，得到A点云到B点云的相对位姿$T_{A->B}^{world}$
已知，
lidar是一个在移动车辆上的sensor，输出A时刻，相对lidar坐标系的点云为$X_A^{lidar}$；输出B时刻，相对lidar坐标系的点云为$X_B^{lidar}$
imu是一个在移动车辆上的sensor，输出结果为mct坐标系下的结果，把mct坐标系下第一帧的时刻定义为固定坐标系：世界坐标系
$P{P}_{im{u}_A}^{world}$imu在A时刻的世界坐标系下，位姿矩阵PPa
$P{P}_{im{u}_B}^{world}$imu在B时刻的世界坐标系下，位姿矩阵PPb
imu到lidar的外参可以表述为$T_{imu}^{lidar}=T_{mct}^{lidar}$，因为imu输出结果是在mct系下，所以外参可以看作是mct坐标系到lidar坐标系的变换
求解：
lidar的A时刻在世界坐标系（mct系）下的位姿矩阵为：$P{P}_{lida{r}_A}^{world}=P{P}_{im{u}_A}^{world}\ast T_{imu}^{lidar}=P{P}_{im{u}_A}^{world}\ast T_{mct}^{lidar}$
lidar的B时刻在世界坐标系（mct系）下的位姿矩阵为：$P{P}_{lida{r}_B}^{world}=P{P}_{im{u}_B}^{world}\ast T_{imu}^{lidar}=P{P}_{im{u}_B}^{world}\ast T_{mct}^{lidar}$
A时刻，相对世界坐标系（mct系）的点云为$X_A^{world}=P{P}_{lida{r}_A}^{world}\ast X_A^{lidar}$,
可展开为：$X_A^{world}=P{P}_{im{u}_A}^{world}\ast T_{mct}^{lidar}\ast X_A^{lidar}$
B时刻，相对世界坐标系（mct系）的点云为$X_B^{world}=P{P}_{lida{r}_B}^{world}\ast X_B^{lidar}$
可展开为：$X_B^{world}=P{P}_{im{u}_B}^{world}\ast T_{mct}^{lidar}\ast X_B^{lidar}$
经过icp匹配$X_A^{world}$和$X_B^{world}$可以得到$T_{A->B}^{world}$

但是因为imu和lidar时间戳不同步，因此对应时刻imu的位姿矩阵不可信，因此只能得到mct系下AB时刻的点云$X_A^{mct}$和$X_B^{mct}$
经过icp匹配$X_A^{lidar}$和$X_B^{lidar}$可以得到变换矩阵$T_{A->B}^{lidar}$
经过icp匹配$X_A^{mct}$和$X_B^{mct}$可以得到变换矩阵$T_{A->B}^{mct}$
其中，
$X_A^{mct}=T_{mct}^{lidar}\ast X_A^{lidar}$，
$X_B^{mct}=T_{mct}^{lidar}\ast X_B^{lidar}$
那么这个$T_{A->B}^{lidar}$和$T_{A->B}^{mct}$表示的是在不同坐标系下的同一个位姿变换矩阵（位姿变换增量矩阵）T=R|t，
这二者之间存在一个固定关系
$X_A^{lidar}$和$X_B^{lidar}$内同一个特征点的坐标为$x_A^{lidar}$和$x_B^{lidar}$
$x_A^{mct}=T_{lidar}^{mct}\ast x_A^{lidar}$，
$x_B^{mct}=T_{lidar}^{mct}\ast x_B^{lidar}$
$x_B^{mct}=T_{A->B}^{mct}\ast x_A^{mct}$
$T_{lidar}^{mct}\ast x_B^{lidar}=T_{A->B}^{mct}\ast T_{lidar}^{mct}\ast x_A^{lidar}$
$x_B^{lidar}=T_{A->B}^{lidar}\ast x_A^{lidar}$
$T_{lidar}^{mct}\ast T_{A->B}^{lidar}\ast x_A^{lidar}=T_{A->B}^{mct}\ast T_{lidar}^{mct}\ast x_A^{lidar}$
$T_{lidar}^{mct}\ast T_{A->B}^{lidar}=T_{A->B}^{mct}\ast T_{lidar}^{mct}$
$T_{A->B}^{lidar}=(T_{lidar}^{mct}{)}^{-1}\ast T_{A->B}^{mct}\ast T_{lidar}^{mct}$
对应代码：np.dot(np.dot(np.linalg.inv(lidar2mct),delta_mat_mct),lidar2mct)

#验证lidar系下的icp匹配结果与mct系下的icp匹配结果相同
# mct系下的icp匹配结果 表达向量
delta_mat_mct = np.array([[0.999725 , -0.023439 , -0.00130324 , -0.127499] , 
[0.0234409 , 0.999724 , 0.00193209 , 0.0205244] , 
[0.00125752 , -0.0019622 , 0.999999 , -0.00368067] , 
[0.0 , 0.0 , 0.0 , 1]])
# lidar系下的icp匹配结果 表达向量
delta_mat_lidar = np.array([[0.999726 , -0.0234405 , 0.00044465 , -0.0937921] , 
[0.0234395 , 0.999723 , 0.00228773 , 0.140559] , 
[-0.000498197 , -0.00227667 , 0.999998 , -0.00616882] , 
[0 , 0 , 0 , 1]])

mct2lidar = np.array([[0.70710678, 0.70710678, 0.0, -1.477],
          [-0.70710678, 0.70710678, 0.0, -0.77],
          [0.0, 0.0, 1.0, -0.66],
          [0.0, 0.0, 0.0, 1.0]])

lidar2mct = np.linalg.inv(mct2lidar)
print("mct2lidar : ")
print(mct2lidar)
print("lidar2mct : ")
print(lidar2mct)
print("delta_mat_mct:")
print(delta_mat_mct)
print("delta_mat_lidar -> delta_mat_mct:")
print(np.dot(np.dot(np.linalg.inv(mct2lidar),delta_mat_lidar),mct2lidar))
print("delta_mat_lidar")
print(delta_mat_lidar)
print("delta_mat_mct -> delta_mat_lidar")
#对应公式$T_{A->B}^{lidar} = (T_{lidar}^{mct})^{-1}*T_{A->B}^{mct}*T_{lidar}^{mct}$
print(np.dot(np.dot(np.linalg.inv(lidar2mct),delta_mat_mct),lidar2mct))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

运行结果如下：

mct2lidar :
[[ 0.70710678  0.70710678  0.         -1.477     ]
 [-0.70710678  0.70710678  0.         -0.77      ]
 [ 0.          0.          1.         -0.66      ]
 [ 0.          0.          0.          1.        ]]
lidar2mct :
[[ 0.70710678 -0.70710678  0.          0.4999245 ]
 [ 0.70710678  0.70710678  0.          1.58886894]
 [ 0.          0.          1.          0.66      ]
 [ 0.          0.          0.          1.        ]]
delta_mat_mct:
[[ 0.999725   -0.023439   -0.00130324 -0.127499  ]
 [ 0.0234409   0.999724    0.00193209  0.0205244 ]
 [ 0.00125752 -0.0019622   0.999999   -0.00368067]
 [ 0.          0.          0.          1.        ]]
delta_mat_lidar -> delta_mat_mct:
[[ 0.999725   -0.0234385  -0.00130325 -0.12747292]
 [ 0.0234415   0.999724    0.00193208  0.02051356]
 [ 0.00125757 -0.00196213  0.999998   -0.00367863]
 [ 0.          0.          0.          1.        ]]
delta_mat_lidar
[[ 9.99726e-01 -2.34405e-02  4.44650e-04 -9.37921e-02]
 [ 2.34395e-02  9.99723e-01  2.28773e-03  1.40559e-01]
 [-4.98197e-04 -2.27667e-03  9.99998e-01 -6.16882e-03]
 [ 0.00000e+00  0.00000e+00  0.00000e+00  1.00000e+00]]
delta_mat_mct -> delta_mat_lidar
[[ 9.99725450e-01 -2.34404500e-02  4.44664099e-04 -9.38036435e-02]
 [ 2.34394500e-02  9.99723550e-01  2.28772378e-03  1.40585449e-01]
 [-4.98284007e-04 -2.27668585e-03  9.99999000e-01 -6.17034358e-03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

2.变换矩阵左右乘/旋转矩阵左右乘

与变换某个目标不同，当一个坐标系发生连续变化时，如何描述这个坐标系的最终变换。
例如，先绕x轴顺时针转180度，然后绕z轴顺时针转45，最后绕y轴转30°
这个时候就会出现两种情况：
1.原始坐标系称为a0，先绕x轴（a0的x轴）顺时针转180度得到坐标系a1，然后绕z轴（这个z轴是a0的z轴）顺时针转45得到坐标系a2，最后绕y轴（这个y轴是a0的y轴）转30°
2.原始坐标系称为a0，先绕x轴（a0的x轴）顺时针转180度得到坐标系a1，然后绕z轴（这个z轴是a1的z轴）顺时针转45得到坐标系a2，最后绕y轴（这个y轴是a2的y轴）转30°
也就是，绕固定坐标系旋转还是绕自身坐标系旋转
此时有个口诀

！！！左乘旋转矩阵绕固定坐标系旋转，右乘旋转矩阵绕自身坐标系旋转！！！
1

在泊车项目中，一般都是按照平面处理的，也就是旋转的变化都是绕Z轴，因为无论是固定为初始坐标系还是自身坐标系Z轴都是不变的，因此，左乘右乘都可以。
但是上述仅仅是理想情况。
一般情况下都不是纯z轴变化，此时，就要区分是左乘还是右乘。

2.1右乘

2.1.1例如：求取外参【imu到lidar的外参】

已知：
前后左右上下，分别表示车体的前后左右上下
imu坐标系：X朝前，Y朝右，Z朝下
lidar坐标系：X朝右前45度，Y朝左前45度，Z朝上
lidar坐标系中心到imu坐标系中心在车体系下的相对位置关系为:
$t{X}_{lidar-imu}^{car}=1.59$
$t{Y}_{lidar-imu}^{car}=-0.5$
$t{Z}_{lidar-imu}^{car}=0.66$

从imu系到lidar系的变化可以归结为以下几步：

1. 绕x轴顺时针旋转180度 =R1
2. 绕z轴顺时针旋转45度 =R2
3. 计算旋转矩阵R3=R2*R1
   （此处计算R1*R2的结果是不一样的，虽然绕x轴顺时针旋转180度，z轴与原始z轴重合，但是这两个z轴朝向不同，因此在顺逆时针的旋转向量表达也会不同）
4. 计算车体系下的偏移量T{tX,tY,tZ}到lidar系的分量
   $t{X}_{lidar-imu}^{lidar}=1.47$
   $t{Y}_{lidar-imu}^{lidar}=0.77$
   $t{Z}_{lidar-imu}^{lidar}=0.66$
5. 将旋转R3和t结合得到T
   过程如下：
   $R_3=R_2\ast R_1$
   $R_3=\left[\begin{array}{ccc}cos(45\ast \pi /180)& -sin(45\ast \pi /180)& 0\\ sin(45\ast \pi /180)& cos(45\ast \pi /180)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(180\ast \pi /180)& -sin(180\ast \pi /180)\\ 0& sin(180\ast \pi /180)& cos(180\ast \pi /180)\end{array}\right]$
   $R_3=\left[\begin{array}{ccc}\sqrt{(}2)/2& -\sqrt{(}2)/2& 0\\ \sqrt{(}2)/2& \sqrt{(}2)/2& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$
   $R_3=\left[\begin{array}{ccc}\sqrt{(}2)/2& \sqrt{(}2)/2& 0\\ \sqrt{(}2)/2& -\sqrt{(}2)/2& 0\\ 0& 0& 1\end{array}\right]$

PS：有的时候是从imu+gnss得到的结果，此时结果为经纬度+高程+航向角的结果，如果是按照经纬度是无法直接使用的需要转到mct（墨卡托）坐标系下，此时外参就不是从imu到lidar了，而是从mct到lidar。因为从gnss的坐标系到mct坐标系已经经历过一轮变换了，此时需要注意的是gnss坐标系输出的航向角heading是顺时针还是逆时针，这个heading（yaw）角需要与mct坐标系下的heading保持一致。

2.1.2例如： 累计$\delta Pose$得到每一个时刻的Pose

$T_1=T_0\ast \delta T_0^1$
$T_2=T_1\ast \delta T_1^2$
$T_n=T_{n-1}\ast \delta T_{n-1}^n$
$T_n=T_0\ast \delta T_0^1\ast \delta T_1^2\dots \dots \ast \delta T_{n-1}^n$

2.1.3例如： 从imuPP（pose&postion）得到lidarPP（pose&postion）

已知外参：
$Extrinsic=T_{imu}^{lidar}$
$P{P}_{lidar}=P{P}_{imu}\ast T_{imu}^{lidar}$
PS：一般情况下imu的位姿矩阵会被理解为用多个空间3维点组成的轨迹，但其实不然，这个PP是有方向的，所以不能当成点/点云处理，而是当作变换矩阵处理（当前PP与世界初始坐标系原点的变换矩阵）。
又因为外参是相对自己坐标系的变换而不是相对世界初始坐标系（固定坐标系），所以外参也是用的右乘。

PS：
虽然此处的imupose可以看作是一个旋转位移增量矩阵，但是！！此处不是坐标系发生变换，与1.3的例子不同。此处是同一个坐标系下，imu轨迹和lidar轨迹存在一个刚性变换关系。同一个mct坐标系下，imupose和lidarpose的变换关系。

2.2左乘

根据roll，pithc，yaw角计算旋转矩阵的方式：
使用左乘，因为左乘是根据固定坐标系旋转。R1R2R3.相当于先转R3，再转R2，最后R1.这三个旋转过程中的旋转轴都是使用的一开始的坐标轴。也相当于先转R1，再转R2，最后R3.这三个旋转过程中的旋转轴都是每次旋转坐标系后的新轴。

                //interpolation rpy
                double roll = (imuDatas[i][3] + calculateAngleDifference(imuDatas[i+1][3],imuDatas[i][3]) * ratio) * M_PI / 180.0;
                double pitch = (imuDatas[i][4] + calculateAngleDifference(imuDatas[i+1][4],imuDatas[i][4]) * ratio) * M_PI / 180.0;
                double yaw = (imuDatas[i][5] + calculateAngleDifference(imuDatas[i+1][5],imuDatas[i][5]) * ratio) * M_PI / 180.0;
                std::cout << "X  = " << imuDatas[i][3] << " " << imuDatas[i+1][3] << std::endl;
                std::cout << "Y  = " << imuDatas[i][4] << " " << imuDatas[i+1][4] << std::endl;
                std::cout << "Z  = " << imuDatas[i][5] << " " << imuDatas[i+1][5] << std::endl;
                //rpy 2 mat
                // 将roll、pitch、yaw转换为旋转矩阵
                Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX()));
                Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY()));
                Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ()));
                
                Eigen::Matrix3d rotation_matrix;
                rotation_matrix=yawAngle*pitchAngle*rollAngle;

                auto angle = rotation_matrix.eulerAngles(0, 1, 2) * 180 / M_PI;
                auto angleypr = rotation_matrix.eulerAngles(2, 1, 0) * 180 / M_PI;
                std::cout << "X  = " << angle(0) << std::endl;
                std::cout << "Y  = " << angle(1) << std::endl;
                std::cout << "Z  = " << angle(2) << std::endl;
            
                std::cout << "yaw  = " << angleypr(0) << std::endl;
                std::cout << "pitch  = " << angleypr(1) << std::endl;
                std::cout << "roll  = " << angleypr(2) << std::endl;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25