网站自己做还是用程序,四川学校网站建设公,小程序制作平台开发,电商网站平台有哪些功能1. 变换点或者变换向量
1.1左乘
矩阵左乘通常是指对”目标点“进行左乘#xff0c;即: A ′ R ∗ A AR*A A′R∗A 其中#xff0c;A为原始3维点#xff0c;表示一个3*1的列向量#xff0c;R为33的旋转矩阵#xff0c;A‘为变换后的点 B ′ T ∗ B BT*B B′T∗B 其中…1. 变换点或者变换向量
1.1左乘
矩阵左乘通常是指对”目标点“进行左乘即: A ′ R ∗ A AR*A A′R∗A 其中A为原始3维点表示一个3*1的列向量R为33的旋转矩阵A‘为变换后的点 B ′ T ∗ B BT*B B′T∗B 其中B为原始点3维点表示一个4*1的齐次化列向量T为44的旋转矩阵R|tB‘为变换后的点 以此类推 如果是点云 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∣XsrcA1,A2……An}A表示一个3*1的列向量 此时 X s r c X_{src} Xsrc为一个3*n的矩阵那么变换可以表示为 X A ′ R ∗ X A X_AR*X_A XA′R∗XA X B ′ T ∗ X B X_BT*X_B XB′T∗XB
1.1.1矩阵与旋转角
上面为3维点的变换为了方便画图解释下面以2维点进行描述 P A R ∗ P B P_A R*P_B PAR∗PB 将矩阵乘法展开可以写为 [ 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 ] \begin{bmatrix} P_{xA}\\P_{yA}\\P_{zA} \end{bmatrix} \begin{bmatrix} cos(\alpha) -sin(\alpha) 0 \\ sin(\alpha) cos(\alpha) 0 \\ 0 0 1 \end{bmatrix}*\begin{bmatrix} P_{xB}\\P_{yB}\\P_{zB} \end{bmatrix} PxAPyAPzA cos(α)sin(α)0−sin(α)cos(α)0001 ∗ PxBPyBPzB
上面图片表示的是一个矩阵的左乘其中旋转矩阵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 ] \begin{bmatrix} cos(\alpha) -sin(\alpha) 0 \\ sin(\alpha) cos(\alpha) 0 \\ 0 0 1 \end{bmatrix} cos(α)sin(α)0−sin(α)cos(α)0001
Z轴顺时针旋转点的R矩阵Z轴逆时针旋转坐标系的R矩阵 [ c o s ( α ) s i n ( α ) 0 − s i n ( α ) c o s ( α ) 0 0 0 1 ] \begin{bmatrix} cos(\alpha) sin(\alpha) 0 \\ -sin(\alpha) cos(\alpha) 0 \\ 0 0 1 \end{bmatrix} cos(α)−sin(α)0sin(α)cos(α)0001
1.2右乘
没有具体例子
1.3 左乘右乘同时存在的场景
针对transform增量例如A坐标系下的R|t变换坐标系的场景
求点云 X A w o r l d X_A^{world} XAworld与点云 X B w o r l d X_B^{world} XBworld进行icp匹配得到A点云到B点云的相对位姿 T A − B w o r l d T_{A-B}^{world} TA−Bworld 已知 lidar是一个在移动车辆上的sensor输出A时刻相对lidar坐标系的点云为 X A l i d a r X_A^{lidar} XAlidar输出B时刻相对lidar坐标系的点云为 X B l i d a r X_B^{lidar} XBlidar imu是一个在移动车辆上的sensor输出结果为mct坐标系下的结果把mct坐标系下第一帧的时刻定义为固定坐标系世界坐标系 P P i m u A w o r l d PP_{imu_A}^{world} PPimuAworldimu在A时刻的世界坐标系下位姿矩阵PPa P P i m u B w o r l d PP_{imu_B}^{world} PPimuBworldimu在B时刻的世界坐标系下位姿矩阵PPb imu到lidar的外参可以表述为 T i m u l i d a r T m c t l i d a r T_{imu}^{lidar}T_{mct}^{lidar} TimulidarTmctlidar因为imu输出结果是在mct系下所以外参可以看作是mct坐标系到lidar坐标系的变换 求解 lidar的A时刻在世界坐标系mct系下的位姿矩阵为 P P l i d a r A w o r l d P P i m u A w o r l d ∗ T i m u l i d a r P P i m u A w o r l d ∗ T m c t l i d a r PP_{lidar_A}^{world}PP_{imu_A}^{world}*T_{imu}^{lidar}PP_{imu_A}^{world}*T_{mct}^{lidar} PPlidarAworldPPimuAworld∗TimulidarPPimuAworld∗Tmctlidar lidar的B时刻在世界坐标系mct系下的位姿矩阵为 P P l i d a r B w o r l d P P i m u B w o r l d ∗ T i m u l i d a r P P i m u B w o r l d ∗ T m c t l i d a r PP_{lidar_B}^{world}PP_{imu_B}^{world}*T_{imu}^{lidar}PP_{imu_B}^{world}*T_{mct}^{lidar} PPlidarBworldPPimuBworld∗TimulidarPPimuBworld∗Tmctlidar A时刻相对世界坐标系mct系的点云为 X A w o r l d P P l i d a r A w o r l d ∗ X A l i d a r X_A^{world} PP_{lidar_A}^{world}*X_A^{lidar} XAworldPPlidarAworld∗XAlidar, 可展开为 X A w o r l d P P i m u A w o r l d ∗ T m c t l i d a r ∗ X A l i d a r X_A^{world} PP_{imu_A}^{world}*T_{mct}^{lidar}*X_A^{lidar} XAworldPPimuAworld∗Tmctlidar∗XAlidar B时刻相对世界坐标系mct系的点云为 X B w o r l d P P l i d a r B w o r l d ∗ X B l i d a r X_B^{world} PP_{lidar_B}^{world}*X_B^{lidar} XBworldPPlidarBworld∗XBlidar 可展开为 X B w o r l d P P i m u B w o r l d ∗ T m c t l i d a r ∗ X B l i d a r X_B^{world} PP_{imu_B}^{world}*T_{mct}^{lidar}*X_B^{lidar} XBworldPPimuBworld∗Tmctlidar∗XBlidar 经过icp匹配 X A w o r l d X_A^{world} XAworld和 X B w o r l d X_B^{world} XBworld可以得到 T A − B w o r l d T_{A-B}^{world} TA−Bworld
但是因为imu和lidar时间戳不同步因此对应时刻imu的位姿矩阵不可信因此只能得到mct系下AB时刻的点云 X A m c t X_A^{mct} XAmct和 X B m c t X_B^{mct} XBmct 经过icp匹配 X A l i d a r X_A^{lidar} XAlidar和 X B l i d a r X_B^{lidar} XBlidar可以得到变换矩阵 T A − B l i d a r T_{A-B}^{lidar} TA−Blidar 经过icp匹配 X A m c t X_A^{mct} XAmct和 X B m c t X_B^{mct} XBmct可以得到变换矩阵 T A − B m c t T_{A-B}^{mct} TA−Bmct 其中 X A m c t T m c t l i d a r ∗ X A l i d a r X_A^{mct}T_{mct}^{lidar}*X_A^{lidar} XAmctTmctlidar∗XAlidar X B m c t T m c t l i d a r ∗ X B l i d a r X_B^{mct}T_{mct}^{lidar}*X_B^{lidar} XBmctTmctlidar∗XBlidar 那么这个 T A − B l i d a r T_{A-B}^{lidar} TA−Blidar和 T A − B m c t T_{A-B}^{mct} TA−Bmct表示的是在不同坐标系下的同一个位姿变换矩阵位姿变换增量矩阵TR|t 这二者之间存在一个固定关系 X A l i d a r X_A^{lidar} XAlidar和 X B l i d a r X_B^{lidar} XBlidar内同一个特征点的坐标为 x A l i d a r x_A^{lidar} xAlidar和 x B l i d a r x_B^{lidar} xBlidar x A m c t T l i d a r m c t ∗ x A l i d a r x_A^{mct}T_{lidar}^{mct}*x_A^{lidar} xAmctTlidarmct∗xAlidar x B m c t T l i d a r m c t ∗ x B l i d a r x_B^{mct}T_{lidar}^{mct}*x_B^{lidar} xBmctTlidarmct∗xBlidar x B m c t T A − B m c t ∗ x A m c t x_B^{mct} T_{A-B}^{mct}*x_A^{mct} xBmctTA−Bmct∗xAmct T l i d a r m c t ∗ x B l i d a r T A − B m c t ∗ T l i d a r m c t ∗ x A l i d a r T_{lidar}^{mct}*x_B^{lidar} T_{A-B}^{mct}*T_{lidar}^{mct}*x_A^{lidar} Tlidarmct∗xBlidarTA−Bmct∗Tlidarmct∗xAlidar x B l i d a r T A − B l i d a r ∗ x A l i d a r x_B^{lidar} T_{A-B}^{lidar}*x_A^{lidar} xBlidarTA−Blidar∗xAlidar T l i d a r m c t ∗ T A − B l i d a r ∗ x A l i d a r T A − B m c t ∗ T l i d a r m c t ∗ x A l i d a r T_{lidar}^{mct}*T_{A-B}^{lidar}*x_A^{lidar} T_{A-B}^{mct}*T_{lidar}^{mct}*x_A^{lidar} Tlidarmct∗TA−Blidar∗xAlidarTA−Bmct∗Tlidarmct∗xAlidar T l i d a r m c t ∗ T A − B l i d a r T A − B m c t ∗ T l i d a r m c t T_{lidar}^{mct}*T_{A-B}^{lidar} T_{A-B}^{mct}*T_{lidar}^{mct} Tlidarmct∗TA−BlidarTA−Bmct∗Tlidarmct T A − B l i d a r ( T l i d a r m c t ) − 1 ∗ T A − B m c t ∗ T l i d a r m c t T_{A-B}^{lidar} (T_{lidar}^{mct})^{-1}*T_{A-B}^{mct}*T_{lidar}^{mct} TA−Blidar(Tlidarmct)−1∗TA−Bmct∗Tlidarmct 对应代码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))运行结果如下
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.00000e00 0.00000e00 0.00000e00 1.00000e00]]
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.00000000e00 0.00000000e00 0.00000000e00 1.00000000e00]]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° 也就是绕固定坐标系旋转还是绕自身坐标系旋转 此时有个口诀
左乘旋转矩阵绕固定坐标系旋转右乘旋转矩阵绕自身坐标系旋转在泊车项目中一般都是按照平面处理的也就是旋转的变化都是绕Z轴因为无论是固定为初始坐标系还是自身坐标系Z轴都是不变的因此左乘右乘都可以。 但是上述仅仅是理想情况。 一般情况下都不是纯z轴变化此时就要区分是左乘还是右乘。
2.1右乘
2.1.1例如求取外参【imu到lidar的外参】
已知 前后左右上下分别表示车体的前后左右上下 imu坐标系X朝前Y朝右Z朝下 lidar坐标系X朝右前45度Y朝左前45度Z朝上 lidar坐标系中心到imu坐标系中心在车体系下的相对位置关系为: t X l i d a r − i m u c a r 1.59 tX_{lidar-imu}^{car}1.59 tXlidar−imucar1.59 t Y l i d a r − i m u c a r − 0.5 tY_{lidar-imu}^{car}-0.5 tYlidar−imucar−0.5 t Z l i d a r − i m u c a r 0.66 tZ_{lidar-imu}^{car}0.66 tZlidar−imucar0.66
从imu系到lidar系的变化可以归结为以下几步
绕x轴顺时针旋转180度 R1绕z轴顺时针旋转45度 R2计算旋转矩阵R3R2*R1 此处计算R1*R2的结果是不一样的虽然绕x轴顺时针旋转180度z轴与原始z轴重合但是这两个z轴朝向不同因此在顺逆时针的旋转向量表达也会不同计算车体系下的偏移量T{tX,tY,tZ}到lidar系的分量 t X l i d a r − i m u l i d a r 1.47 tX_{lidar-imu}^{lidar}1.47 tXlidar−imulidar1.47 t Y l i d a r − i m u l i d a r 0.77 tY_{lidar-imu}^{lidar}0.77 tYlidar−imulidar0.77 t Z l i d a r − i m u l i d a r 0.66 tZ_{lidar-imu}^{lidar}0.66 tZlidar−imulidar0.66将旋转R3和t结合得到T 过程如下 R 3 R 2 ∗ R 1 R_3R_2*R_1 R3R2∗R1 R 3 [ c o s ( 45 ∗ π / 180 ) − s i n ( 45 ∗ π / 180 ) 0 s i n ( 45 ∗ π / 180 ) c o s ( 45 ∗ π / 180 ) 0 0 0 1 ] [ 1 0 0 0 c o s ( 180 ∗ π / 180 ) − s i n ( 180 ∗ π / 180 ) 0 s i n ( 180 ∗ π / 180 ) c o s ( 180 ∗ π / 180 ) ] R_3\begin{bmatrix} cos(45*\pi/180) -sin(45*\pi/180) 0 \\ sin(45*\pi/180) cos(45*\pi/180) 0 \\ 0 0 1 \end{bmatrix}\begin{bmatrix} 1 0 0 \\ 0 cos(180*\pi/180) -sin(180*\pi/180) \\ 0 sin(180*\pi/180) cos(180*\pi/180) \end{bmatrix} R3 cos(45∗π/180)sin(45∗π/180)0−sin(45∗π/180)cos(45∗π/180)0001 1000cos(180∗π/180)sin(180∗π/180)0−sin(180∗π/180)cos(180∗π/180) R 3 [ ( 2 ) / 2 − ( 2 ) / 2 0 ( 2 ) / 2 ( 2 ) / 2 0 0 0 1 ] [ 1 0 0 0 − 1 0 0 0 − 1 ] R_3\begin{bmatrix} \sqrt(2)/2 -\sqrt(2)/2 0 \\ \sqrt(2)/2 \sqrt(2)/2 0 \\ 0 0 1 \end{bmatrix}\begin{bmatrix} 1 0 0 \\ 0 -1 0 \\ 0 0 -1 \end{bmatrix} R3 ( 2)/2( 2)/20−( 2)/2( 2)/20001 1000−1000−1 R 3 [ ( 2 ) / 2 ( 2 ) / 2 0 ( 2 ) / 2 − ( 2 ) / 2 0 0 0 1 ] R_3\begin{bmatrix} \sqrt(2)/2 \sqrt(2)/2 0 \\ \sqrt(2)/2 -\sqrt(2)/2 0 \\ 0 0 1 \end{bmatrix} R3 ( 2)/2( 2)/20( 2)/2−( 2)/20001
PS有的时候是从imugnss得到的结果此时结果为经纬度高程航向角的结果如果是按照经纬度是无法直接使用的需要转到mct墨卡托坐标系下此时外参就不是从imu到lidar了而是从mct到lidar。因为从gnss的坐标系到mct坐标系已经经历过一轮变换了此时需要注意的是gnss坐标系输出的航向角heading是顺时针还是逆时针这个headingyaw角需要与mct坐标系下的heading保持一致。
2.1.2例如 累计 δ P o s e \delta Pose δPose得到每一个时刻的Pose T 1 T 0 ∗ δ T 0 1 T_1T_0*\delta T_0^1 T1T0∗δT01 T 2 T 1 ∗ δ T 1 2 T_2T_1*\delta T_1^2 T2T1∗δT12 T n T n − 1 ∗ δ T n − 1 n T_nT_{n-1}*\delta T_{n-1}^n TnTn−1∗δTn−1n T n T 0 ∗ δ T 0 1 ∗ δ T 1 2 … … ∗ δ T n − 1 n T_nT_0*\delta T_0^1*\delta T_1^2……*\delta T_{n-1}^n TnT0∗δT01∗δT12……∗δTn−1n
2.1.3例如 从imuPPposepostion得到lidarPPposepostion
已知外参 E x t r i n s i c T i m u l i d a r ExtrinsicT_{imu}^{lidar} ExtrinsicTimulidar P P l i d a r P P i m u ∗ T i m u l i d a r PP_{lidar}PP_{imu}*T_{imu}^{lidar} PPlidarPPimu∗Timulidar PS一般情况下imu的位姿矩阵会被理解为用多个空间3维点组成的轨迹但其实不然这个PP是有方向的所以不能当成点/点云处理而是当作变换矩阵处理当前PP与世界初始坐标系原点的变换矩阵。 又因为外参是相对自己坐标系的变换而不是相对世界初始坐标系固定坐标系所以外参也是用的右乘。
PS 虽然此处的imupose可以看作是一个旋转位移增量矩阵但是此处不是坐标系发生变换与1.3的例子不同。此处是同一个坐标系下imu轨迹和lidar轨迹存在一个刚性变换关系。同一个mct坐标系下imupose和lidarpose的变换关系。
2.2左乘
暂时没有例子
