LEGO-LOAM改进思路以及代码

青旅半醒 2022-09-11 05:24 181阅读 0赞

# 0. 前言 #

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

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

这部分作者说通过二维里程计提供位移 + IMU航姿模块提供三向角度 投影成三维轮式IMU里程计 算率很低，实现很方便。很适合三维轮式里程计的操作。为此本文直接给出[`geometry_msgs::TwistStamped`][geometry_msgs_TwistStamped]部分的操作，个人感觉使用[IMU中值滤波][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系统带来良好的收益。这部分我们可以先看一下[坐标系转换][Link 1]

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加入 #

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

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

## …详情请参照[古月居][Link 3] ##

[yuanguobin01]: https://blog.csdn.net/qq_34672671
[20210526113411804.png_x-oss-process_image_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70_pic_center]: /images/20220828/5f61cfa9e5a842fe8e2eda07e187bc56.png
[geometry_msgs_TwistStamped]: http://docs.ros.org/en/jade/api/geometry_msgs/html/msg/TwistStamped.html
[IMU]: https://blog.csdn.net/weixin_36084613/article/details/112535166
[Link 1]: https://blog.csdn.net/ktigerhero3/article/details/77155199
[Link 2]: https://www.guyuehome.com/34085
[Link 3]: https://www.guyuehome.com/35398