aloam 几个坐标系的关系的梳理

本文链接：https://blog.csdn.net/brightming/article/details/117843756

1、坐标系关系类型
总共有：
lidar到里程计的坐标系的关系；
里程计到map的坐标系的关系；
lidar到map的坐标系关系；

2、laserOdomerty会发布一个消息：

ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);

内容是：

// 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;
}