0. 前言

最近無事，在想着做一些工作。正好碰巧看到了yuanguobin01作者寫的Lego-Loam的改進思路系列文章，這部分看完後遺憾于作者僅僅提供了一些初步的設想，而沒有系統的學習代碼，為此本文打算從作者提出的幾個改進點來給出自己實作的政策思路。

LEGO-LOAM改進思路以及代碼0. 前言1. 二維輪式裡程計+IMU = 三維裡程計替換原本3D雷射前端裡程計2. 增加GPS子產品3. Scan-Context加入

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系列中回環檢測主要存在有四種方法

傳統的領域距離搜尋+ICP比對
基于scan context系列的粗比對+ICP精準比對的回環檢測
基于scan context的回環檢測
基于Intensity scan context+ICP的回環檢測

LEGO-LOAM改進思路以及代碼0. 前言1. 二維輪式裡程計+IMU = 三維裡程計替換原本3D雷射前端裡程計2. 增加GPS子產品3. Scan-Context加入

0. 前言

1. 二維輪式裡程計+IMU = 三維裡程計替換原本3D雷射前端裡程計

2. 增加GPS子產品

3. Scan-Context加入

…詳情請參照古月居

繼續閱讀

PCB基本設計流程詳解PCB基本設計流程詳解

布線工程師談PCB設計！

電源完整性與地彈噪聲的高速PCB仿真

變種LZ77資料無損壓縮算法

推薦一些VB的學習交流網站

重構，開始第一個案例前言 1 重構，開始第一個案例

dos指令(轉東轉西)

Cocos、Lua遊戲記憶體釋放之我見

手機遊戲優化技巧

Lua遊戲記憶體洩漏檢查

視覺傳感器在無人駕駛領域取得技術突破

GNU科學函數庫[參考手冊][v0.1 Build 090129 Beta][GNU Scientific Library]

對于0-1分數規劃的Dinkelbach算法的分析

Sql優化一：sql語句優化

SLAM主要步驟

Git學習筆記5 merge沖突時二選一

LEGO-LOAM改進思路以及代碼0. 前言1. 二維輪式裡程計+IMU = 三維裡程計 替換 原本3D雷射前端裡程計2. 增加GPS子產品3. Scan-Context加入

0. 前言

1. 二維輪式裡程計+IMU = 三維裡程計 替換 原本3D雷射前端裡程計

2. 增加GPS子產品

3. Scan-Context加入

…詳情請參照古月居

繼續閱讀

LEGO-LOAM改進思路以及代碼0. 前言1. 二維輪式裡程計+IMU = 三維裡程計替換原本3D雷射前端裡程計2. 增加GPS子產品3. Scan-Context加入

1. 二維輪式裡程計+IMU = 三維裡程計替換原本3D雷射前端裡程計