0. 前言

最近无事，在想着做一些工作。正好碰巧看到了yuanguobin01作者写的Lego-Loam的改进思路系列文章，这部分看完后遗憾于作者仅仅提供了一些初步的设想，而没有系统的学习代码，为此本文打算从作者提出的几个改进点来给出自己实现的策略思路。

1. 二维轮式里程计+IMU = 三维里程计 替换 原本3D激光前端里程计

这部分作者说通过二维里程计提供位移 + IMU航姿模块提供三向角度 投影成三维轮式IMU里程计 算率很低，实现很方便。很适合三维轮式里程计的操作。为此本文直接给出geometry_msgs::TwistStamped部分的操作，个人感觉使用IMU中值滤波。能够使整个结构更加紧凑。

void wheelHandler(const geometry_msgs::TwistStampedConstPtr &wheel_msg)
{ 
   
    using Eigen::Vector3d;

    if (wheel_msg->header.stamp.toSec() <= last_wheel_t)
    { 
   
        ROS_WARN("wheel message in disorder!");
        return;
    }

    double t  = wheel_msg->header.stamp.toSec();
    last_wheel_t = t;
    double vx = wheel_msg->twist.linear.x;
    double vy = wheel_msg->twist.linear.y;
    double vz = wheel_msg->twist.linear.z;
    double rx = wheel_msg->twist.angular.x;
    double ry = wheel_msg->twist.angular.y;
    double rz = wheel_msg->twist.angular.z;
    Vector3d vel(vx, vy, vz);
    Vector3d gyr(rx, ry, rz);
    inputWheel(t, vel, gyr);

    if (init_wheel)
    { 
   
        latest_time = t;
        init_wheel = 0;
        return;
    }
    double dt = t - latest_time;
    latest_time = t;

    tmp_Q = tmp_Q * Utility::deltaQ(gyr * dt);
    tmp_P = tmp_P + tmp_Q.toRotationMatrix() * vel * dt;
    tmp_V = vel;

    nav_msgs::Odometry wheelOdometry;
    wheelOdometry.header.frame_id = "/camera_init";
    wheelOdometry.child_frame_id = "/laser_odom";
    wheelOdometry.header.stamp = ros::Time().fromSec(t);
    wheelOdometry.pose.pose.orientation.x = tmp_Q.x();
    wheelOdometry.pose.pose.orientation.y = tmp_Q.y();
    wheelOdometry.pose.pose.orientation.z = tmp_Q.z();
    wheelOdometry.pose.pose.orientation.w = tmp_Q.w();
    wheelOdometry.pose.pose.position.x = tmp_P.x();
    wheelOdometry.pose.pose.position.y = tmp_P.y();
    wheelOdometry.pose.pose.position.z = tmp_P.z();
    pubWheelOdometry.publish(wheelOdometry);

    geometry_msgs::PoseStamped wheelPose;
    wheelPose.header = wheelOdometry.header;
    wheelPose.pose = wheelOdometry.pose.pose;
    wheelPath.header.stamp = wheelOdometry.header.stamp;
    wheelPath.poses.push_back(wheelPose);
    wheelPath.header.frame_id = "/camera_init";
    pubWheelPath.publish(wheelPath);

    if (saveWheelOdo) { 
   
        std::ofstream founW("xxx/results/wheel_odo.txt",
                            std::ios::app);
        founW.setf(std::ios::fixed, std::ios::floatfield);
        founW.precision(5);
        founW << wheelOdometry.header.stamp.toSec() << " ";
        founW.precision(5);
        founW << wheelOdometry.pose.pose.position.x << " "
              << wheelOdometry.pose.pose.position.y << " "
              << wheelOdometry.pose.pose.position.z << " "
              << wheelOdometry.pose.pose.orientation.x << " "
              << wheelOdometry.pose.pose.orientation.y << " "
              << wheelOdometry.pose.pose.orientation.z << " "
              << wheelOdometry.pose.pose.orientation.w << std::endl;
        founW.close();
    }
    return;
}

2. 增加GPS模块

该部分作者说GPS因子的添加着重提高建图精度和长时间的鲁棒性，也可以用做回环检测。这部分的确能够给LEGO-LOAM系统带来良好的收益。这部分我们可以先看一下坐标系转换

 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
 { 
   
     // ********************************* 激光里程计 --> 全局位姿 ***********************************
     // 0. 北斗星通的惯导系为北东地，大地坐标转换函数的坐标系为东北天，这里现将惯导系作处理，转为东北天
     // 将大地坐标的xyz和惯导输出的rpy统一到东北天坐标系下。
     // 1. 激光系 --> 车体系：一个平移，不考虑惯导安装，因为可以直接从第一帧的RTK消息中，得知第一帧的车体系到东北天坐标系下的变换
     // 和ros标准保持一致，默认将前进方向设置为x，向上为z
     // 2. 车体系 --> 东北天：坐标变换，利用第一帧的xyzrpy变换得到。
     // ************************************************************************************************
     Eigen::Isometry3d mLidar = Eigen::Isometry3d::Identity(); // 将当前的位姿赋值给mBody
     mLidar.rotate(Eigen::Quaterniond(
                         laserOdometry2.pose.pose.orientation.x,
                     laserOdometry2.pose.pose.orientation.y,
                 laserOdometry2.pose.pose.orientation.z,
             laserOdometry2.pose.pose.orientation.w
     ));
     mLidar.translate(Eigen::Vector3d(
                         laserOdometry2.pose.pose.position.x, // 直接将激光剪掉
                     laserOdometry2.pose.pose.position.y,
                 laserOdometry2.pose.pose.position.z
     ));

     // 1. mLidar --> mBody
     Eigen::Isometry3d tLidar2Body = Eigen::Isometry3d::Identity();//平移
     tLidar2Body.translate(Eigen::Vector3d(1.83,0,0));//旋转
     Eigen::Isometry3d mBody = tLidar2Body*mLidar;

     // 2. mBody --> ENU
     Eigen::Isometry3d mENU = tBody2ENU*mBody;

     // ********************************* 全局位姿 --> 激光里程计 ***********************************

     // Body到ENU原点，因此是负数
     mENU.translate(Eigen::Vector3d(RTK->x,RTK->y,RTK->Ati);
     // RTK的RPY测量的是车体相对于ENU坐标系的旋转
     mENU.rotate(Eigen::Quaterniond(Eigen::AngleAxisd(RTK->yaw*PI/180.0,Eigen::Vector3d::UnitZ())*
                     Eigen::AngleAxisd(RTK->pitch*PI/180.0,Eigen::Vector3d::UnitY())*
                     Eigen::AngleAxisd(RTK->roll*PI/180.0,Eigen::Vector3d::UnitX()));//欧拉角转四元数
     
     // 1. ENU --> mBody 
     Eigen::Isometry3d mBody = tBody2ENU.inverse()*mENU;

     // 2. mBody --> mLidar 
     Eigen::Isometry3d mLidar = tLidar2Body.inverse()*mBody;
}

 
 // 涉及到三个坐标系：gps,body,lidar
 // body和lidar差一个平移
 void rtkHandler(const bdxt::rtk::ConstPtr& RTK)
 { 
   
     if(!is_gps_init){ 
    // gps初始值记录
         if(RTK->navStatus == 4 && RTK->rtkStatus == 5){ 
   
             // qBody2ENU计算车体坐标系到gps(东北天)坐标系的旋转
             // 而可以获取的imu读数是惯导系下的值(北东地)，因此需要作变换：BODY-->北东地-->东北天
             // 涉及两个四元数：qBODY2NED,qNED2ENU

             Eigen::Vector3d vBody2ENU; // gps初始化位姿：x,y,z,pitch,yaw,roll，同时也是激光原点坐标系和大地坐标系的静态变换
             Eigen::Quaterniond qBody2ENU; // body到ENU的旋转变换

             // RTK的RPY测量的是车体相对于ENU坐标系的旋转
             Eigen::Quaterniond qBodyNED_; // Body相对于NED坐标系下的旋转，实际上是NED2BODY
             qBodyNED_ = Eigen::AngleAxisd(RTK->yaw*PI/180.0,Eigen::Vector3d::UnitZ())*
                         Eigen::AngleAxisd(RTK->pitch*PI/180.0,Eigen::Vector3d::UnitY())*
                         Eigen::AngleAxisd(RTK->roll*PI/180.0,Eigen::Vector3d::UnitX());//欧拉角转四元数
             Eigen::Quaterniond qBody2NED = qBodyNED_.conjugate();//反转四元数
             // 东北天 --> 北东地，先z轴旋转PI/2，再x轴旋转PI，注意顺序
             Eigen::Quaterniond qNED2ENU;
             qNED2ENU = Eigen::AngleAxisd(PI/2,Eigen::Vector3d::UnitZ())*
                     Eigen::AngleAxisd(PI,Eigen::Vector3d::UnitX());
             //注意顺序，从右到左变换
             qBody2ENU = qNED2ENU*qBody2NED;
             
             // Body到ENU原点，因此是负数
             vBody2ENU[0] = -RTK->x;  // 如果使用第一帧为坐标原点坐标转换，设置为0
             vBody2ENU[1] = -RTK->y;  // 如果使用第一帧为坐标原点坐标转换，设置为0
             vBody2ENU[2] = -RTK->Ati;  // 如果使用第一帧为坐标原点坐标转换，设置为0

             // 得到变换矩阵
             tBody2ENU.rotate(qBody2ENU);
             tBody2ENU.translate(vBody2ENU);

             is_gps_init = true;
         }
         else{ 
   
             ROS_INFO("bad RTK status\n");
         }
     }
 }

在转换后我们可以通过EKF或者Ceres带入进去，并在前端实现优化。而作者说的后端部分优化可以参考下面的部分文档，基本上就是在GTSAM中加入GPS因子

isamCurrentEstimate = isam->calculateEstimate();//这部分可以替换为RTK，不需要实时融合传入。如果认为RTK精度足够的话
double recentOptimizedX = lastOptimizedPose.translation().x();
double recentOptimizedY = lastOptimizedPose.translation().y();
double bigNoiseTolerentToXY = 1000000000.0; // 1e9
double gpsAltitudeNoiseScore = 250.0; // if height is misaligned after loop clsosing, use this value bigger
gtsam::Vector robustNoiseVector3(3); // gps factor has 3 elements (xyz)
robustNoiseVector3 << bigNoiseTolerentToXY, bigNoiseTolerentToXY, gpsAltitudeNoiseScore; // means only caring altitude here. (because LOAM-like-methods tends to be asymptotically flyging)
robustGPSNoise = gtsam::noiseModel::Robust::Create(
                gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good.
                gtsam::noiseModel::Diagonal::Variances(robustNoiseVector3) );

// 找到最合适的GPS信息，这里速度够慢可以不对齐
double eps = 0.1; // 在eps内找到一个GPS主题
while (!gpsBuf.empty()) { 
   
    auto thisGPS = gpsBuf.front();
    auto thisGPSTime = thisGPS->header.stamp.toSec();
    if( abs(thisGPSTime - timeLaserOdometry) < eps ) { 
   
        currGPS = thisGPS;
        hasGPSforThisKF = true; 
        break;
    } else { 
   
        hasGPSforThisKF = false;
    }
    gpsBuf.pop();
}
mBuf.unlock(); 

// gps factor 
const int curr_node_idx = keyframePoses.size() - 1; // 因为索引从0开始(实际上这个索引可以是任何数字，但为了简单的实现，我们遵循顺序索引)
if(hasGPSforThisKF) { 
   
    double curr_altitude_offseted = currGPS->altitude - gpsAltitudeInitOffset;
    mtxRecentPose.lock();
    gtsam::Point3 gpsConstraint(recentOptimizedX, recentOptimizedY, curr_altitude_offseted); // 在这个例子中，只调整高度(对于x和y，设置了很大的噪音)
    mtxRecentPose.unlock();
    gtSAMgraph.add(gtsam::GPSFactor(curr_node_idx, gpsConstraint, robustGPSNoise));
    cout << "GPS factor added at node " << curr_node_idx << endl;
}
initialEstimate.insert(curr_node_idx, poseTo);    

3. Scan-Context加入

这部分可以参考我之前写的系列文章。
文中也给出在LOAM系列中回环检测主要存在有四种方法

1. 传统的领域距离搜索+ICP匹配
2. 基于scan context系列的粗匹配+ICP精准匹配的回环检测
3. 基于scan context的回环检测
4. 基于Intensity scan context+ICP的回环检测

下面是Intensity scan context的部分操作

void mapOptimization::performLoopClosure(void) { 
   
     if (cloudKeyPoses3D->points.empty() == true)
         return;
 
     // try to find close key frame if there are any
     if (potentialLoopFlag == false) { 
   
         if (detectLoopClosure() == true) { 
   
             std::cout << std::endl;
             potentialLoopFlag = true; // find some key frames that is old enough or close enough for loop closure
             timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
         }
         if (potentialLoopFlag == false) { 
   
             return;
         }
     }
 
     // reset the flag first no matter icp successes or not
     potentialLoopFlag = false;
 
     // *****
     // Main
     // *****
     // make common variables at forward
     float x, y, z, roll, pitch, yaw;
     Eigen::Affine3f correctionCameraFrame;
     float noiseScore = 0.5; // constant is ok...
     gtsam::Vector Vector6(6);
     Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
     constraintNoise = noiseModel::Diagonal::Variances(Vector6);
     robustNoiseModel = gtsam::noiseModel::Robust::Create(
             gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure
             gtsam::noiseModel::Diagonal::Variances(Vector6)
     ); // - checked it works. but with robust kernel, map modification may be delayed (i.e,. requires more true-positive loop factors)
 
     bool isValidRSloopFactor = false;
     bool isValidSCloopFactor = false;
 
     /* * 1. RS loop factor (radius search) * 将RS检测到的历史帧和当前帧匹配，求transform, 作为约束边 */
     if (RSclosestHistoryFrameID != -1) { 
   
         pcl::IterativeClosestPoint<PointType, PointType> icp;
         icp.setMaxCorrespondenceDistance(100);
         icp.setMaximumIterations(100);
         icp.setTransformationEpsilon(1e-6);
         icp.setEuclideanFitnessEpsilon(1e-6);
         icp.setRANSACIterations(0);
 
         // Align clouds
         icp.setInputSource(RSlatestSurfKeyFrameCloud);
         icp.setInputTarget(RSnearHistorySurfKeyFrameCloudDS);
         pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
         icp.align(*unused_result);
 
         // 通过score阈值判定icp是否匹配成功
         std::cout << "[RS] ICP fit score: " << icp.getFitnessScore() << std::endl;
         if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) { 
   
             std::cout << "[RS] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")"
                       << std::endl;
             isValidRSloopFactor = false;
         } else { 
   
             std::cout << "[RS] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure
                       << " ] and RS nearest [ " << RSclosestHistoryFrameID << " ]" << std::endl;
             isValidRSloopFactor = true;
         }
 
         // 这里RS检测成功，加入约束边
         if (isValidRSloopFactor == true) { 
   
             correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame)
             pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
             Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
             // transform from world origin to wrong pose
             Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(
                     cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
             // transform from world origin to corrected pose
             Eigen::Affine3f tCorrect =
                     correctionLidarFrame * tWrong; // pre-multiplying -> successive rotation about a fixed frame
             pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
             gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
             gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[RSclosestHistoryFrameID]);
             gtsam::Vector Vector6(6);
 
             std::lock_guard<std::mutex> lock(mtx);
             gtSAMgraph.add(
                     BetweenFactor<Pose3>(latestFrameIDLoopCloure, RSclosestHistoryFrameID, poseFrom.between(poseTo),
                                          robustNoiseModel));
             isam->update(gtSAMgraph);
             isam->update();
             gtSAMgraph.resize(0);
         }
     }
 
     /* * 2. SC loop factor (scan context) * SC检测成功，进行icp匹配 */
     if (SCclosestHistoryFrameID != -1) { 
   
         pcl::IterativeClosestPoint<PointType, PointType> icp;
         icp.setMaxCorrespondenceDistance(100);
         icp.setMaximumIterations(100);
         icp.setTransformationEpsilon(1e-6);
         icp.setEuclideanFitnessEpsilon(1e-6);
         icp.setRANSACIterations(0);
 
         // Align clouds
         // Eigen::Affine3f icpInitialMatFoo = pcl::getTransformation(0, 0, 0, yawDiffRad, 0, 0); // because within cam coord: (z, x, y, yaw, roll, pitch)
         // Eigen::Matrix4f icpInitialMat = icpInitialMatFoo.matrix();
         icp.setInputSource(SClatestSurfKeyFrameCloud);
         icp.setInputTarget(SCnearHistorySurfKeyFrameCloudDS);
         pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
         icp.align(*unused_result);
         // icp.align(*unused_result, icpInitialMat); // PCL icp non-eye initial is bad ... don't use (LeGO LOAM author also said pcl transform is weird.)
 
         std::cout << "[SC] ICP fit score: " << icp.getFitnessScore() << std::endl;
         if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) { 
   
             std::cout << "[SC] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")"
                       << std::endl;
             isValidSCloopFactor = false;
         } else { 
   
             std::cout << "[SC] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure
                       << " ] and SC nearest [ " << SCclosestHistoryFrameID << " ]" << std::endl;
             isValidSCloopFactor = true;
         }
 
         // icp匹配成功也加入约束边
         if (isValidSCloopFactor == true) { 
   
             correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame)
             pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
             gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
             gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
 
             std::lock_guard<std::mutex> lock(mtx);
             // gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise)); // original
             gtSAMgraph.add(
                     BetweenFactor<Pose3>(latestFrameIDLoopCloure, SCclosestHistoryFrameID, poseFrom.between(poseTo),
                                          robustNoiseModel)); // giseop
             isam->update(gtSAMgraph);
             isam->update();
             gtSAMgraph.resize(0);
         }
     }
 
     // flagging
     aLoopIsClosed = true;
 
 } // performLoopClosure

4. 添加一段距离的闭环约束

…详情请参照古月居