//odometry計算的轉移矩陣(實時高頻量)
float transformSum[6] = {0};
//平移增量
float transformIncre[6] = {0};
//經過mapping矯正過後的最終的世界座標系下的位姿
float transformMapped[6] = {0};
//mapping傳遞過來的優化前的位姿
float transformBefMapped[6] = {0};
//mapping傳遞過來的優化後的位姿
float transformAftMapped[6] = {0};

//ros釋出、座標系、里程計、位姿轉換
ros::Publisher *pubLaserOdometry2Pointer = NULL;
tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
nav_msgs::Odometry laserOdometry2;
tf::StampedTransform laserOdometryTrans2;

二、位姿轉換 transformAssociateToMap

位姿轉換主要是將odometry的運動估計和mapping矯正量融合，主要步驟如下

步驟1：計算兩次鐳射里程計的平移增量transformIncre。由於是基於勻速運動模型的假設，所以運動增量為transformBefMapped[3] - transformSum[3]。由於這兩個陣列中的位姿態都是基於世界座標系（/camera_init）下的，所以需要將點雲從世界座標系轉換到當前時刻的imu座標系下，變換矩陣為 $R^{T}=R(z)^{T}R(x)^{T}R(y)^{T}$

$\begin{bmatrix} \Delta x_{Inc} \\ \Delta y_{Inc} \\ \Delta z_{Inc} \end{bmatrix} = \begin{bmatrix} cos\gamma & sin\gamma & 0 \\ -sin\gamma & cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\alpha & sin\alpha\\ 0 & -sin\alpha & cos\alpha \end{bmatrix} \begin{bmatrix} cos\beta & 0 & -sin\beta \\ 0 & 1 & 0 \\ sin\beta & 0 & cos\beta \end{bmatrix} \begin{bmatrix} x_{bef}-x_{sum}\\ y_{bef}-y_{sum}\\ z_{bef}-z_{sum} \end{bmatrix}$

步驟2：計算地圖map與世界座標系（/camera_init）的矩陣

$(T_{k}^{m})_{w-BefMap}= (T_{k-1}^{m})_{w-AftMap} \cdot ((T_{k-1}^{o})_{w-Odom})^T \cdot (T_{k}^{o})_{w-Odom}$

最終的旋轉矩陣 $R=R(y)^{ }R(x)^{ }R(z)^{ }$

$\begin{bmatrix} cosey \cdot cosez + siney \cdot sinex \cdot sinez & cosez \cdot siney \cdot sinex - cosey \cdot sinez & cosex \cdot siney \\ cosex \cdot sinez & cosex \cdot cosez & -sinex \\ cosey \cdot sinex \cdot sinez - cosez \cdot siney & siney \cdot sinez + sinex \cdot cosey \cdot cosez & -cosey \cdot cosex \end{bmatrix}$

記為

$R= \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{bmatrix}$

則上述旋轉矩陣轉換成尤拉角的計算公式為

$\beta _{y} = \theta _{y} = -asin(R_{23})$

$\alpha _{x} = \psi _{x} = atan2 \left ( \frac{R_{13}}{cos\theta _{y}}, \frac{R_{33}}{cos\theta _{y}} \right )$

$\gamma _{z} = \phi _{x} = atan2 \left ( \frac{R_{21}}{cos\theta _{y}}, \frac{R_{22}}{cos\theta _{y}} \right )$

程式碼中的的對應關係如下

srx對應 $R_{23}$
srycrx對應 $R_{13}$
crycrx對應 $R_{33}$
srzcrx對應 $R_{21}$
crzcrx對應 $R_{22}$

步驟3：在得到imu里程計預測的map在世界座標系（/camera_init）的位姿transformMapped（經過mapping矯正過後的最終的世界座標系下的位姿），還需要加上步驟1中計算的世界座標系下的平移增量，完成位姿更新。由於平移增量transformIncre是在當前時刻的imu座標系下，需要轉換到世界座標系，變換矩陣為 R=R(y)R(x)R(z) 。

$\begin{bmatrix} x_{fMap-new} \\ y_{fMap-new} \\ z_{fMap-new} \end{bmatrix} = \begin{bmatrix} x_{fMap} \\ y_{fMap} \\ z_{fMap} \end{bmatrix} - \begin{bmatrix} cos\beta & 0 & sin\beta \\ 0 & 1 & 0 \\ -sin\beta & 0 & cos\beta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\alpha & -sin\alpha\\ 0 & sin\alpha & cos\alpha \end{bmatrix} \begin{bmatrix} cos\gamma & -sin\gamma & 0 \\ sin\gamma & cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \Delta x_{Inc} \\ \Delta y_{Inc} \\ \Delta z_{Inc} \end{bmatrix}$

//odometry的運動估計和mapping矯正量融合之後得到的最終的位姿transformMapped
void transformAssociateToMap()
{
  //平移後繞y軸旋轉（-transformSum[1]）
  float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
           - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
  float y1 = transformBefMapped[4] - transformSum[4];
  float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
           + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);

  //繞x軸旋轉（-transformSum[0]）
  float x2 = x1;
  float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
  float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;

  //繞z軸旋轉（-transformSum[2]）
  transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
  transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
  transformIncre[5] = z2;

  float sbcx = sin(transformSum[0]);
  float cbcx = cos(transformSum[0]);
  float sbcy = sin(transformSum[1]);
  float cbcy = cos(transformSum[1]);
  float sbcz = sin(transformSum[2]);
  float cbcz = cos(transformSum[2]);

  float sblx = sin(transformBefMapped[0]);
  float cblx = cos(transformBefMapped[0]);
  float sbly = sin(transformBefMapped[1]);
  float cbly = cos(transformBefMapped[1]);
  float sblz = sin(transformBefMapped[2]);
  float cblz = cos(transformBefMapped[2]);

  float salx = sin(transformAftMapped[0]);
  float calx = cos(transformAftMapped[0]);
  float saly = sin(transformAftMapped[1]);
  float caly = cos(transformAftMapped[1]);
  float salz = sin(transformAftMapped[2]);
  float calz = cos(transformAftMapped[2]);

  float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
            - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
            - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
            - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 
            - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
  transformMapped[0] = -asin(srx);

  float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
               - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
               - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
               + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
               + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
               + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
  float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
               - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
               + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
               + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
               - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
               + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
  transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), 
                             crycrx / cos(transformMapped[0]));
  
  float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
               - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
               - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
               - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
               + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
  float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
               - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
               - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
               - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
               + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
  transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), 
                             crzcrx / cos(transformMapped[0]));

  x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];
  y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];
  z1 = transformIncre[5];

  x2 = x1;
  y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;
  z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;

  transformMapped[3] = transformAftMapped[3] 
                     - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);
  transformMapped[4] = transformAftMapped[4] - y2;
  transformMapped[5] = transformAftMapped[5] 
                     - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
}

三、接收資訊 Handler

接收laserOdometry的資訊

//接收laserOdometry的資訊
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

  //得到旋轉平移矩陣
  transformSum[0] = -pitch;
  transformSum[1] = -yaw;
  transformSum[2] = roll;

  transformSum[3] = laserOdometry->pose.pose.position.x;
  transformSum[4] = laserOdometry->pose.pose.position.y;
  transformSum[5] = laserOdometry->pose.pose.position.z;

  transformAssociateToMap();

  geoQuat = tf::createQuaternionMsgFromRollPitchYaw
            (transformMapped[2], -transformMapped[0], -transformMapped[1]);

  laserOdometry2.header.stamp = laserOdometry->header.stamp;
  laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
  laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
  laserOdometry2.pose.pose.orientation.z = geoQuat.x;
  laserOdometry2.pose.pose.orientation.w = geoQuat.w;
  laserOdometry2.pose.pose.position.x = transformMapped[3];
  laserOdometry2.pose.pose.position.y = transformMapped[4];
  laserOdometry2.pose.pose.position.z = transformMapped[5];
  pubLaserOdometry2Pointer->publish(laserOdometry2);

  //傳送旋轉平移量
  laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
  laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
  laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
  tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}

接收laserMapping的轉換資訊

//接收laserMapping的轉換資訊
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

  transformAftMapped[0] = -pitch;
  transformAftMapped[1] = -yaw;
  transformAftMapped[2] = roll;

  transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
  transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
  transformAftMapped[5] = odomAftMapped->pose.pose.position.z;

  transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
  transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
  transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;

  transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
  transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
  transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}

四、主函式 main

main函式的作用如下

transformMaintenance節點的初始化
訂閱laserOdometry節點發布的/laser_odom_to_init訊息（Lidar里程計估計位姿到初始座標系的變換）；訂閱laserMapping節點發布的/aft_mapped_to_init訊息（laserMapping節點優化後的位姿到初始座標系的變換）
釋出/integrated_to_init訊息

//主函式
int main(int argc, char** argv)
{
  //ros節點初始化
  ros::init(argc, argv, "transformMaintenance");
  ros::NodeHandle nh;

  //ros訂閱資訊
  ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
                                     ("/laser_odom_to_init", 5, laserOdometryHandler);

  ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> 
                                     ("/aft_mapped_to_init", 5, odomAftMappedHandler);

  //ros釋出資訊
  ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);
  pubLaserOdometry2Pointer = &pubLaserOdometry2;
  laserOdometry2.header.frame_id = "/camera_init";
  laserOdometry2.child_frame_id = "/camera";

  tf::TransformBroadcaster tfBroadcaster2;
  tfBroadcaster2Pointer = &tfBroadcaster2;
  laserOdometryTrans2.frame_id_ = "/camera_init";
  laserOdometryTrans2.child_frame_id_ = "/camera";

  ros::spin();

  return 0;
}

結語

至此，已經把transformMaintenance.cpp的內容解析完了意味著LOAM程式碼解析的已經結束。

上述內容還有幾處不太理解的，如果有人能夠解答，就請給我留言吧，十分感謝。

~~如果你看到這裡，說明你已經下定決心要學習loam了，學習新知識的過程總是痛苦的，與君共勉吧！~~

一些碎碎念

在程式碼解析過程中，作為一個從未接觸過鐳射雷達的我來說，真的是需要學習非常多新的知識，論文中的難點就在於座標系的轉換，座標系轉換經常會把我整得暈乎乎的，我也有好幾處地方沒有完全弄懂。

雖然這件事情非常之難，沒有什麼大師指導，沒有什麼資金支援，但這件事情我沒有感到絲毫後悔，因為我確實在這學習過程中得到許多，不僅僅是知識，更多的是對於一件事情、一個專案的處理。

只有不斷學習新知識，才能是自己成長。

LOAM學習-程式碼解析（七）融合資訊 transformMaintenance

前言

一、初始化

二、位姿轉換 transformAssociateToMap

三、接收資訊 Handler

四、主函式 main

結語

一些碎碎念

LOAM學習-程式碼解析（七）融合資訊 transformMaintenance

LOAM學習-程式碼解析（三）特徵點運動估計 laserOdometry

LOAM學習-程式碼解析（五）地圖構建 laserMapping

Dubbo原始碼解析（七）註冊中心——zookeeper

影象分割套件PaddleSeg全面解析（七）損失函式解析

深度學習程式碼入門（二）：tf下的Keras介面，非自定義網路初步實現

“基於醫療知識圖譜的問答系統”程式碼解析（三）

“基於醫療知識圖譜的問答系統”程式碼解析（四）

JavaScript實現簡易輪播圖最全程式碼解析（ES5）

JavaScript實現簡易放大鏡最全程式碼解析（ES5）

PHPExcel 表格匯入資料程式碼解析（二）

基於C++程式碼的UE4學習（七）—— 自定義事件及觸發Custom Event

《第一行程式碼：Android篇》學習筆記（七）

OpenGL學習（七）-- 基礎變化綜合練習實踐總結

MySQL學習（七）：Innodb儲存引擎索引的實現原理詳解

Docker學習（七）實戰 - 構建REDIS映象

深度學習論文翻譯解析（八）：Rich feature hierarchies for accurate object detection and semantic segmentation

深度學習論文翻譯解析（九）：Spatial Pyramid Pooling in Deep Convolutional Networks for Visual Recognition

深度學習論文翻譯解析（十）：Visualizing and Understanding Convolutional Networks

Python學習————drf（七）

LOAM學習-程式碼解析（七）融合資訊 transformMaintenance

前言

一、初始化

二、位姿轉換 transformAssociateToMap

三、接收資訊 Handler

四、主函式 main

結語

一些碎碎念

相關推薦