aloam 幾個坐标系的關系的梳理

1、坐标系關系類型

總共有：

lidar到裡程計的坐标系的關系；

裡程計到map的坐标系的關系；

lidar到map的坐标系關系；

2、laserOdomerty會釋出一個消息：

内容是：

// publish odometry
            nav_msgs::Odometry laserOdometry;
            laserOdometry.header.frame_id = "/camera_init";
            laserOdometry.child_frame_id = "/laser_odom";
            laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
            laserOdometry.pose.pose.orientation.x = q_w_curr.x();
            laserOdometry.pose.pose.orientation.y = q_w_curr.y();
            laserOdometry.pose.pose.orientation.z = q_w_curr.z();
            laserOdometry.pose.pose.orientation.w = q_w_curr.w();
            laserOdometry.pose.pose.position.x = t_w_curr.x();
            laserOdometry.pose.pose.position.y = t_w_curr.y();
            laserOdometry.pose.pose.position.z = t_w_curr.z();
            pubLaserOdometry.publish(laserOdometry);
           

這裡雖然寫的是q_w_curr，實際上是lidar到裡程計坐标系的偏移。

3、lasermapping對/laser_odom_to_init的處理

在mapping節點中，對于收到的odom消息，會存放到lidar到裡程計坐标系的轉換關系中，然後利用這個轉換關系，更新lidar到map的轉換關系（這個是我們最終需要的資料，隻是需要由裡程計來中轉）。

// high frequence publish
	Eigen::Quaterniond q_wodom_curr;
	Eigen::Vector3d t_wodom_curr;
	q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
	q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
	q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
	q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
	t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
	t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
	t_wodom_curr.z() = laserOdometry->pose.pose.position.z;

	Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
	Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 
           

一開始的時候，認為裡程計坐标系和map坐标系是一緻的，即沒有旋轉，沒有位移，從初始化值可以看到：

// wmap_T_odom * odom_T_curr = wmap_T_curr;
// transformation between odom's world and map's world frame
//世界坐标系下目前裡程計坐标系的四元數與位移
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
 
//裡程計坐标系下某點的四元數和位移
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
           

那為何不能始終如一呢？

因為在裡程計中，用到的是lidar點雲與上一幀點雲的比對得到的相對變換關系，這個變換關系可能是不完全對的，因為參考的資訊不夠，隻有兩幀之間的點雲，久而久之，就慢慢發散了。

但是在mapping子產品中，是會根據點雲與曆史附近點雲做比對，比對的範圍更大了，理論上是會更準确的。就會得到兩個不同的轉換關系了。對于同樣一個真實的地點，在裡程計坐标系下可能是T1,在map下是T2。

如果裡程計子產品綜合了與mapping一樣多的資訊來計算，那麼兩者是一緻的，或者直接可以取消odometry子產品，也就沒有了這個中轉子產品，也是可以的。

回到aloam這裡，轉換關系的維護流程如下：

4、轉換關系的變化

上面說了收到了odometry子產品的odom消息後，放到了q_wodom_curr、t_wodom_curr變量，也用這個變量更新q_w_curr、t_w_curr變量。

這個時候，可以認為q_w_curr、t_w_curr是一個估計值，需要利用點雲與map點雲的比對（更大範圍）來更新q_w_curr、t_w_curr，更新完了，再反過來更新q_wmap_wodom、t_wmap_wodom，就形成一個閉環疊代了：

1）T_map_odom為對角陣，表示開始時刻兩者統一，

2）利用前後幀點雲比對得到當時的T_odom_curr，

3）釋出T_odom_curr給下遊，結合T_map_odom，計算出T_map_curr，作為優化的初值，

利用優化子產品，更新T_map_curr，

4）反過來更新了T_map_odom

整個流程回到第2）步不斷循環。

優化子產品的處理：

對于每個lidar點，進行：pointAssociateToMap，轉換到map坐标系下，

利用優化子產品，更新T_map_curr，

優化結束後，執行transformUpdate，更新odom到map的轉換關系。

void transformAssociateToMap()
{
	q_w_curr = q_wmap_wodom * q_wodom_curr;
	t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}


//求世界坐标系下目前裡程計坐标系的四元數與位移
void transformUpdate()
{
	q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
	t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
           

是以，總的來說，這些是一個不斷疊代的過程。odom與map兩個坐标系會不斷偏離，但是兩者始終維護了一個轉換矩陣，使得odometry子產品計算的位置能轉換到map坐标系下。

同時，odometry的軌迹是平滑的，map下可能會有跳變。

另外，odometry可以高頻釋出，可以結合imu等一起計算。

另外，odometry可以取消掉，直接就用mapping那一套來計算。