aloam中增加回环模块——流程分析

aloam没有回环模块，参考lego-loam增加回环模块。

一、lego loam mapping模块分析

1、首先要明确几个重要转换矩阵的含义：

    float transformLast[6];  
    float transformSum[6];
    float transformIncre[6];
    float transformTobeMapped[6];
    float transformBefMapped[6];
    float transformAftMapped[6];

• transformLast[6]与transformAftMapped[6]一样，记录的是某个点云帧到地图坐标系的转换关系；

• transformTobeMapped：在LM优化中，优化的是这个transformTobeMapped，优化完后，会在 transformUpdate()函数中transformTobeMapped赋给transformAftMapped，同时也会把transformSum赋给transformBefMapped。

• transformSum、transformBefMapped记录的是lidar到里程计坐标系的转换关系，Sum记录的是当前得到的lidar到里程计的转换关系，而transformBefMapped记录的是上一帧的。

2、数据的转换关系：

• mapping模块的回调函数中，接收laserOdometry模块发出/laser_odom_to_init，将其记录在transformSum变量中。
• 在主循环中，第一步做的就是调用transformAssociateToMap函数，这个函数的基本思路就是利用前一帧lidar到里程计坐标系的转换关系（记录在transformBefMapped）以及当前到里程计坐标系的转换关系（记录在transformSum中），计算出相对变化量，然后利用上一次的lidar到map的转换关系（记录在transformAftMapped），结合这个变化量，估计出当前的lidar到地图的转换关系（transformTobeMapped）。
  估计方式：
  T_(tobe)=T_(wAft) * T_(wBef).inverse * T_(wsum)
  R=R(y)R(x)R(z)
• 估计了当前帧对地图的转换关系后，就开始抽取附近帧的数据。抽取的时候，在不打开loopdetect时，用的是currentRobotPosPoint这个变量作为原点来搜索，这个变量在这个时候记录的其实是上一时刻的姿态。
  抽取附近关键帧的方法是kdtree在所有pose3d中搜索，找到附近50米的pose。把最终整理到的pose对应的点云加在一起：

for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
                *laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
                *laserCloudSurfFromMap   += *surroundingSurfCloudKeyFrames[i];
                *laserCloudSurfFromMap   += *surroundingOutlierCloudKeyFrames[i];
            }

• 下一步是对当前得到的surf、corner点进行下采样
• 接着调用scan2MapOptimization执行优化了，这个是关键：

void scan2MapOptimization(){

        if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {

            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

            for (int iterCount = 0; iterCount < 10; iterCount++) {

                laserCloudOri->clear();
                coeffSel->clear();

                cornerOptimization(iterCount);
                surfOptimization(iterCount);

                if (LMOptimization(iterCount) == true)
                    break;              
            }

            transformUpdate();
        }
    }

其实就是利用得到的周围的点云（分了corner、surf的）和当前的点云，进行优化计算，这个过程可以和aloam一样，用ceres进行。
得到了优化后的lidar到map的转换关系transformTobeMapped，然后在调用transformUpdate，对transformBefMapped和transformAftMapped进行赋值。
这个transformUpdate方法用到了接收到的imu的信息。

• 优化已经做完了，接着调用saveKeyFramesAndFactor方法保存key frame数据和factor因子。
  函数首先保存当前位置：

 currentRobotPosPoint.x = transformAftMapped[3];
 currentRobotPosPoint.y = transformAftMapped[4];
 currentRobotPosPoint.z = transformAftMapped[5];

然后判断当前帧是否作为一个关键帧处理，判断的条件是与上一个关键帧的位移大于0.3米，或者是不是第一帧，只有是第一帧或者判断为是关键帧，才往下处理，否则不再处理。
如果是，则进入下面的处理逻辑：
如果是第一帧，则把当前的位置（理论上就是无旋转、无平移）作为先验加入到graph中；
如果不是第一帧，则构建上一个关键帧与本关键帧的betwenFactor，加入到graph中；
然后进行isam的计算，这种计算会实时给出了整个图的姿态，当然包括当前的姿态，所以，会用最新估计的姿态赋值给transformAftMapped，这个就是图优化后的更新姿态。
当然也要把估计的最新姿态数据给到transformLast和transformTobeMapped。
有个问题：
这里为什么图优化是有效的？
像当前帧的姿态就是利用上一帧的姿态根据相对变化计算的，加入到这种图里，有用吗？
我的理解：
imu起的作用还未分析；
这个图构建的时候，不是每一帧都放入进去的，而是隔几帧才放入的，这就可以消除那种缓慢偏离。
如对于不同时刻：i=1,2,3,4,5,6,7,8
关键帧时刻选为1，5，8.
5的姿态是由4 <- 3 <- 2 <- 1 这样逐步推理得到的，如果在每一阶段都有一定的误差，毕竟不能保证观察是完全准确的，mapping优化刚好到位，就会积累误差。
那加入这个因子图，由于记录了1时刻的位姿，以及5时刻的位姿，8时刻的位姿，在没有回环的时候，给这些位姿之间加上一个不确定度，就可以进行估计了。（好像除了加了不确定度，还是不能很好的说明这个优化有用）
当遇到回环的时候，这个约束就很强了。后面再分析。

• correctPoses
  当执行了回环后，才进行：
  上面优化了所有时刻的位姿后，重新把isam里估计的姿态数据给记录的cloudKeyPoses3D和cloudKeyPoses6D

• publishTF
  发布当前的地图位姿，和tf数据。

• publishKeyPosesAndFrames
  发布相关的姿态和点云数据。

• clearCloud
  清理缓存的用于中间处理的点云数据。

• 整体的主流程代码：

if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {

                timeLastProcessing = timeLaserOdometry;
                //把点云坐标均转换到世界坐标系下
                //围绕transformTobeMapped数组进行的变换，这就是从雷达坐标系转换到世界坐标系下的变换。
                transformAssociateToMap();
                //由于帧数的频率大于建图的频率，因此需要提取关键帧进行匹配
                extractSurroundingKeyFrames();
                //降采样匹配以及增加地图点云，回环检测
                downsampleCurrentScan();

                scan2MapOptimization();

                saveKeyFramesAndFactor();

                correctPoses();

                publishTF();

                publishKeyPosesAndFrames();

                clearCloud();
            }

二、几个问题
1、imu在lego-loam的mapping起到什么作用？

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        // Get the matrix represented as roll pitch and yaw about fixed axes XYZ. 
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        imuPointerLast = (imuPointerLast + 1) % imuQueLength;
        imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
    }

imu的数据用到了pitch、roll，唯一用到的就是在：
transformUpdate这个函数里，这个函数的执行是在scan2MapOptimization函数中，优化执行完以后进行才调用的，具体代码：

void transformUpdate()
    {   //此时transformTobeMapped已经经过LM优化过.
		if (imuPointerLast >= 0) {
		    float imuRollLast = 0, imuPitchLast = 0;
            //寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
		    while (imuPointerFront != imuPointerLast) {
		        if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) {
		            break;
		        }
		        imuPointerFront = (imuPointerFront + 1) % imuQueLength;
		    }

		    if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) {
		        imuRollLast = imuRoll[imuPointerFront];
		        imuPitchLast = imuPitch[imuPointerFront];
		    } else {
		        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
		        float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) 
		                         / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
		        float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) 
		                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);

		        imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
		        imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
		    }

		    transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
		    transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;
		  }

		for (int i = 0; i < 6; i++) {
		    transformBefMapped[i] = transformSum[i];
		    transformAftMapped[i] = transformTobeMapped[i];//Lm优化是优化transformTobeMapped，将优化后的值赋给transformAftMapped
		}
    }

也就是说，上面利用LM方法优化得到的transformTobeMapped这个变量，在这里会根据imu之间的相对变化进行调整。
基本就是按照这一帧雷达的前面时刻的imu数据、后面时刻的imu数据，线性插值出当前帧时刻对应的imuRoll（记录在imuRollLast）、imuPitch（记录在imuPitchLast），然后按照固定权重微调里面的roll、pitch角度

transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;

这个权重设置是否与安装有关系？imu的坐标轴如果和lidar的不一致，imu的pitch不一定是lidar的pitch啊？
那没有imu其实也可以啊？
有了imu的这一步调整，对应后面的加入因子图就会有不同的约束了，因为当前帧的姿态数据不完全来源于点云的匹配（不管是前后帧还是帧与局部点云的匹配），就带来了更多的信息。

三、调整思路
1、地图保存方式调整
修改地图点云的保存为lego-loam的地图保存的方式，即采用每个pose关联对应该帧点云，不再像aloam那样，将不同时刻得到的点云保存在立方体中，分不出彼此，这样才能在姿态更新后，进行调整。

2、增加imu消息的接收

3、优化的方法还是保留原来的ceres的方式

4、增加因子图记录帧间的关系