數(shù)學(xué)概念

1.矩陣乘法
   01.兩個(gè)矩陣A和B相乘，需要滿足A的列數(shù)等于B的行數(shù)
   02.單位矩陣 如果A是n×n矩陣，I是單位矩陣，則AI= A, IA = A
   03.逆矩陣  矩陣A的逆矩陣    A^-1， A A^-1=A^-1A= I，I是單位矩陣 
          當(dāng)一個(gè)矩陣沒(méi)有逆矩陣的時(shí)候，稱該矩陣為奇異矩陣
   04. 轉(zhuǎn)置矩陣 矩陣的轉(zhuǎn)置就是行列互換，用A^T表示A的轉(zhuǎn)置矩陣
   05. 對(duì)稱矩陣： 如果一個(gè)矩陣轉(zhuǎn)置后等于原矩陣，那么這個(gè)矩陣稱為對(duì)稱矩陣。由定義可知，對(duì)稱矩陣一定是方陣	  
   06. 若矩陣為方陣且其逆矩陣存在時(shí),矩陣的逆的轉(zhuǎn)置 等于 矩陣的轉(zhuǎn)置的逆   設(shè)A為可逆方陣 (A^{-1})^T = (A^T)^{-1}),
   07.? 矩陣的逆等于矩陣的轉(zhuǎn)置的矩陣稱為正交矩陣?，滿足 ( Q^{-1} = Q^T ) 或等價(jià)地 ( Q^T Q = I )（其中 ( I ) 為單位矩陣
2.矩陣乘法結(jié)合律
    遵循矩陣乘法結(jié)合律 最終變換結(jié)果相同
    數(shù)學(xué)上等價(jià)：Tx × P^T = (P × Tx^T)^T
	左乘:點(diǎn)云需要轉(zhuǎn)置 
3.	矩陣分塊運(yùn)算--分塊上下三角矩陣
    P2 =R*P1 + T1  （先旋轉(zhuǎn)再平移）變換矩陣的方式
       T1= R*T2 故 T2 = R^T *T1
    (先平移再旋轉(zhuǎn))   
    P2 = R(P1 + T2) =  R*P1 + R*T2
	分塊矩陣的計(jì)算 分塊矩陣 上三角矩陣的乘法
     [ R,T     [ P,   = [RP+T,		 
	   0,1]  *   1]        1] 

物理概念

計(jì)算機(jī)實(shí)現(xiàn)

 points_h = np.hstack([points, np.ones((points.shape[0], 1),dtypes=np.float32)])
 # 左乘：T × P^T
 transformed = (transform_matrix @ points_h.T).T
 ## points_h[:,:3] = np.dot(TX_L_V, points_h.T).T[:.:3]
 
 ###左乘  外側(cè)相同，內(nèi)測(cè)從右往左
   P_imu = T_vehicle_2_imu * T_lidar_2_vehicle * P_lidar

## 矩陣乘法的順序是從右向左  從右向左應(yīng)用變換：先應(yīng)用 T_lidar_to_vehicle，再應(yīng)用 T_vehicle_to_imu
     相當(dāng)于：點(diǎn)_imu = T_vehicle_to_imu * (T_lidar_to_vehicle × 點(diǎn)_lidar)
	         點(diǎn)_imu = T_vehicle_to_imu *  T_lidar_to_vehicle *  點(diǎn)_lidar= T__lidar_2_imu *  點(diǎn)_lidar

        T_lidar_to_imu = T_vehicle_to_imu × T_lidar_to_vehicle				 
### 注意：這里使用的是左乘，即變換矩陣依次左乘點(diǎn)坐標(biāo)
 使用 np.dot() 或 @ 運(yùn)算符進(jìn)行矩陣乘法

  注意矩陣乘法的順序（從右向左應(yīng)用變換）