當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）

發(fā)布時間：2023/12/14 编程问答 31 豆豆

生活随笔收集整理的這篇文章主要介紹了 SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

寫在前面

關(guān)于安裝配置，博客LIO_SAM實測運行，論文學(xué)習(xí)及代碼注釋[附對應(yīng)google driver數(shù)據(jù)] 我覺得已經(jīng)寫的比較完善了。但是我覺得在注釋方面，這位博主寫的還不夠完善，因此在學(xué)習(xí)以后，我想補充一些內(nèi)容。

關(guān)于本身論文的原理，參見我的專欄內(nèi)容：

SLAM學(xué)習(xí)筆記（十九）開源3D激光SLAM總結(jié)大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及區(qū)別

上面這篇文章內(nèi)容比較多，我放了目錄，可以只看對應(yīng)位置。

而現(xiàn)在這篇文章的主要目的是理清LIO-SAM的流程，對其中的數(shù)據(jù)處理的思路、程序設(shè)計的流程等各方面做一個總結(jié)。代碼的注釋有我自己寫的內(nèi)容，還有就是參考了一些別人的網(wǎng)絡(luò)博客的內(nèi)容?？傊诳偨Y(jié)的過程中，我發(fā)現(xiàn)有些東西其實最開始的時候自己理解的不夠深刻。

因此，總結(jié)此文，一來為自己對代碼更熟悉，便于之后回顧，二來也希望能幫到大家。如果這篇文章有幸被你讀到，我希望讀者可以自己總結(jié)一下流程，自己一行一行親手把代碼注釋寫進去。否則如果只是整篇復(fù)制下來讀一遍的話，說實話沒啥意義，如同過眼云煙，等過個兩天腦子里什么都沒了。

截至目前，我認為我這篇文章是對代碼梳理的最詳細的一篇文章。

寫在前面

utility.h

imageProjection.cpp

注釋

總結(jié)

featureExtraction.cpp

注釋

總結(jié)

imuPreintergration.cpp

注釋

總結(jié)

mapOptimization.cpp

注釋

總結(jié)

LIO-SAM說白了只有五個文件，一個頭文件utility.h，還有四個cpp文件。我們從頭文件開始介紹。

utility.h

頭文件里主要是放了一些通用的參數(shù)定義，比方說：

nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");

這個意思就是我launch文件里面有這個"lio_sam/pointCloudTopic"（前面這個）的參數(shù)值的賦值，那么就賦值給pointCloudTopic（后面這個），后面的"/points_raw"就會忽略。那假如launch文件里面沒有這個"lio_sam/pointCloudTopic"的定義，則就用"points_raw"。我們打開run.launch文件，可以看到是有加載參數(shù)的：

?那么相關(guān)的參數(shù)就放在params.yaml文件中。

關(guān)于這個params.yaml，里面的東西乍一看很多，其實就幾個比較重要：

第一：IMU Settings 來記錄IMU的bias和噪聲。當然IMU其實是加速度計和陀螺儀一共三個軸，這里卻不分軸，用了一個平均數(shù)。如果沒有轉(zhuǎn)臺之類的設(shè)備，就跑艾倫方差也可以標出各個軸的bias。作者的處理是：在imu預(yù)積分部分，三個軸都用了同樣的bias方差（也就是寫在配置文件里的這個），當然你要是有能力標的更準，那不妨這里把各個軸的bias都寫進去，然后在imupreintegration.cpp文件里面改一改。

第二，IMU和雷達之間的外參。我不知道為什么作者TiXiaoshan很騷包的在這里寫了一個Extrinsics (lidar -> IMU)，其實不能這樣寫，應(yīng)該寫成IMU->Lidar。因為它其實是，也就是說在IMU坐標系下的一個點，左乘，就可以變換到lidar坐標系下。而且作者用的IMU也不是正常IMU，我推測他用的應(yīng)該是左手系IMU。對于正常人使用的話，就普通的IMU就行，假如你就是那個機器人，x射向正前方，y射向左邊，z射向頭頂。雷達和IMU都是這樣擺放，所以extrinsicRot和extrinsicRPY全部弄成單位矩陣就行。差的不太遠的話，extrinsicTrans弄成[0，0，0]就行。

不過有精力的話還是標定了一下，就用浙大在2020開源的標定工具，lidar_IMU_calib ，（這個博客講怎么配置的，我用的是這個），我個人拿尺子量出來大致比一下，我覺得他們估的還挺準。順便提一嘴，ETH還有一個標定雷達和IMU的，那個準確來說標的是雷達和里程計的，原理也比較簡單，反正雷達運動算一個軌跡，IMU也來一個軌跡，兩邊用外參聯(lián)系起來，構(gòu)成一個環(huán)，來求解外參。但是這種是有問題的，因為IMU估計的軌跡本身也就不準，這樣估出來的外參也就不對，所以人家是標雷達和里程計的，不能單估雷達和IMU。浙大的這個lidar_IMU_calib的文章我大致看了看，大致是用樣條插值，相機去對齊IMU，通過這種方式來初始化，之后構(gòu)建surfelsMap，迭代優(yōu)化來精修。（細節(jié)以我的性格正常我會展開說的，但是我不喜歡他們這篇論文所以不講，主要是因為他們在論文里表示坐標系變換，非要寫成，像這種反人類的寫法我們應(yīng)該堅決抵制 ^ _ ^）

第三，z_tollerance，可以給z一個比較大的限制，如果用的是無人車，那就不可能在z軸變化過大，這里就可以限制它，防止它飄走。

第四，voxel filter paprams，這個是一些體素濾波的下采樣參數(shù)，注意室內(nèi)和室外是有區(qū)別的。

現(xiàn)在回到這個頭文件里，可以注意兩個內(nèi)容：

第一，imuConverter函數(shù)，這個函數(shù)之后會被頻繁調(diào)用。它主要的作用，是把IMU的信息，從IMU坐標系，轉(zhuǎn)換到雷達坐標系。（注意，這個函數(shù)只旋轉(zhuǎn)，沒有平移，和真正的雷達坐標系之間還是差了一個平移的。至于為什么沒有平移，先提前劇透一下，在imuPreintegration.cpp文件中，還有兩個imu2Lidar，lidar2imu變量，這倆變量只有平移，沒有旋轉(zhuǎn)。

事實上，作者后續(xù)是把imu數(shù)據(jù)先用imuConverter旋轉(zhuǎn)到雷達系下（但其實還差了個平移）。然后他把雷達數(shù)據(jù)又根據(jù)lidar2Imu反向平移了一下，和轉(zhuǎn)換以后差了個平移的imu數(shù)據(jù)在“中間系”對齊，之后算完又從中間系通過imu2Lidar挪回了雷達系進行publish。

第二，publishCloud函數(shù)，這個函數(shù)傳入句柄，然后發(fā)布形參里的內(nèi)容，在cpp文件涉及到話題發(fā)布的地方，都會調(diào)用它。

其他的函數(shù)都是一些數(shù)據(jù)轉(zhuǎn)換函數(shù)，沒什么可說的。

imageProjection.cpp

注釋

#include "utility.h" #include "lio_sam/cloud_info.h"struct VelodynePointXYZIRT {PCL_ADD_POINT4DPCL_ADD_INTENSITY;uint16_t ring;float time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW } EIGEN_ALIGN16; POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)(uint16_t, ring, ring) (float, time, time) )struct OusterPointXYZIRT {PCL_ADD_POINT4D;float intensity;uint32_t t;uint16_t reflectivity;uint8_t ring;uint16_t noise;uint32_t range;EIGEN_MAKE_ALIGNED_OPERATOR_NEW } EIGEN_ALIGN16; POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)(uint32_t, t, t) (uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range) )// Use the Velodyne point format as a common representation using PointXYZIRT = VelodynePointXYZIRT;const int queueLength = 2000;class ImageProjection : public ParamServer { private:std::mutex imuLock;std::mutex odoLock;ros::Subscriber subLaserCloud;ros::Publisher pubLaserCloud;ros::Publisher pubExtractedCloud;ros::Publisher pubLaserCloudInfo;ros::Subscriber subImu;std::deque<sensor_msgs::Imu> imuQueue;ros::Subscriber subOdom;std::deque<nav_msgs::Odometry> odomQueue;std::deque<sensor_msgs::PointCloud2> cloudQueue;sensor_msgs::PointCloud2 currentCloudMsg;double *imuTime = new double[queueLength];double *imuRotX = new double[queueLength];double *imuRotY = new double[queueLength];double *imuRotZ = new double[queueLength];int imuPointerCur;bool firstPointFlag;Eigen::Affine3f transStartInverse;pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;pcl::PointCloud<PointType>::Ptr fullCloud;pcl::PointCloud<PointType>::Ptr extractedCloud;int deskewFlag;cv::Mat rangeMat;bool odomDeskewFlag;float odomIncreX;float odomIncreY;float odomIncreZ;lio_sam::cloud_info cloudInfo;double timeScanCur;double timeScanEnd;std_msgs::Header cloudHeader;public:ImageProjection():deskewFlag(0){//訂閱話題進入回調(diào)函數(shù) imu數(shù)據(jù) 激光點云.// imuTopic:topic name; 2000:queue size; &ImageProjection::imuHandler:callback function// this: 調(diào)用這個class里的返回函數(shù)，可以使用第四個參數(shù)，例如有個對象叫l(wèi)istener，// 調(diào)用該對象內(nèi)部的回調(diào)函數(shù)，則傳入&listener，這里調(diào)用自己的，則傳入this指針// ros::TransportHints().tcpNoDelay() :被用于指定hints，確定傳輸層的作用話題的方式:無延遲的TCP傳輸方式subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());//訂閱話題進入回調(diào)函數(shù) //訂閱imu里程計: 來自IMUPreintegration(IMUPreintegration.cpp中的類IMUPreintegration)發(fā)布的里程計話題（增量式）subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());//訂閱話題進入回調(diào)函數(shù) 激光點云subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());//發(fā)布去畸變的點云,"lio_sam/deskew/cloud_deskewed":topic name; 1:queue_sizepubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);//發(fā)布激光點云信息這里建議看一下自定義的lio_sam::cloud_info的msg文件里面包含了較多信息pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);//分配內(nèi)存allocateMemory();//重置部分參數(shù)resetParameters();//setVerbosityLevel用于設(shè)置控制臺輸出的信息。(pcl::console::L_ALWAYS)不會輸出任何信息;L_DEBUG會輸出DEBUG信息;//L_ERROR會輸出ERROR信息pcl::console::setVerbosityLevel(pcl::console::L_ERROR);}void allocateMemory(){//根據(jù)params.yaml中給出的N_SCAN Horizon_SCAN參數(shù)值分配內(nèi)存//用智能指針的reset方法在構(gòu)造函數(shù)里面進行初始化laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());fullCloud.reset(new pcl::PointCloud<PointType>());extractedCloud.reset(new pcl::PointCloud<PointType>());fullCloud->points.resize(N_SCAN*Horizon_SCAN);//cloudinfo是msg文件下自定義的cloud_info消息，對其中的變量進行賦值操作//(int size, int value):size-要分配的值數(shù),value-要分配給向量名稱的值cloudInfo.startRingIndex.assign(N_SCAN, 0);cloudInfo.endRingIndex.assign(N_SCAN, 0);cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);resetParameters();}void resetParameters(){//清零操作laserCloudIn->clear();extractedCloud->clear();// reset range matrix for range image projection,//初始全部用FLT_MAX 填充，//因此后文函數(shù)projectPointCloud中有一句if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX) continue;rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));imuPointerCur = 0;firstPointFlag = true;odomDeskewFlag = false;for (int i = 0; i < queueLength; ++i){imuTime[i] = 0;imuRotX[i] = 0;imuRotY[i] = 0;imuRotZ[i] = 0;}}~ImageProjection(){}/*** 訂閱原始imu數(shù)據(jù)* 1、imu原始測量數(shù)據(jù)轉(zhuǎn)換到lidar系，加速度、角速度、RPY*/void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg){//imuConverter在頭文件utility.h中，作用是把imu數(shù)據(jù)轉(zhuǎn)換到lidar坐標系sensor_msgs::Imu thisImu = imuConverter(*imuMsg);// 上鎖，添加數(shù)據(jù)的時候隊列不可用std::lock_guard<std::mutex> lock1(imuLock);imuQueue.push_back(thisImu);//debug IMU datacout << std::setprecision(6);cout << "IMU acc: " << endl;cout << "x: " << thisImu.linear_acceleration.x << ", y: " << thisImu.linear_acceleration.y << ", z: " << thisImu.linear_acceleration.z << endl;cout << "IMU gyro: " << endl;cout << "x: " << thisImu.angular_velocity.x << ", y: " << thisImu.angular_velocity.y << ", z: " << thisImu.angular_velocity.z << endl;double imuRoll, imuPitch, imuYaw;tf::Quaternion orientation;tf::quaternionMsgToTF(thisImu.orientation, orientation);tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);cout << "IMU roll pitch yaw: " << endl;cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;}/*** 訂閱imu里程計，由imuPreintegration積分計算得到的每時刻imu位姿.(地圖優(yōu)化程序中發(fā)布的)*/void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg){std::lock_guard<std::mutex> lock2(odoLock);odomQueue.push_back(*odometryMsg);}void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){//添加一幀激光點云到隊列，取出最早一幀作為當前幀，計算起止時間戳，檢查數(shù)據(jù)有效性if (!cachePointCloud(laserCloudMsg))return;// 當前幀起止時刻對應(yīng)的imu數(shù)據(jù)、imu里程計數(shù)據(jù)處理if (!deskewInfo())return;// 當前幀激光點云運動畸變校正// 1、檢查激光點距離、掃描線是否合規(guī)// 2、激光運動畸變校正，保存激光點projectPointCloud();// 提取有效激光點，存extractedCloudcloudExtraction();// 發(fā)布當前幀校正后點云，有效點publishClouds();// 重置參數(shù)，接收每幀lidar數(shù)據(jù)都要重置這些參數(shù)resetParameters();}bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){// cache point cloudcloudQueue.push_back(*laserCloudMsg);if (cloudQueue.size() <= 2)return false;// convert cloud// 取出激光點云隊列中最早的一幀currentCloudMsg = std::move(cloudQueue.front());cloudQueue.pop_front();if (sensor == SensorType::VELODYNE){// 轉(zhuǎn)換成pcl點云格式形參: (in,out)pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);}else if (sensor == SensorType::OUSTER){// Convert to Velodyne formatpcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);laserCloudIn->points.resize(tmpOusterCloudIn->size());laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;for (size_t i = 0; i < tmpOusterCloudIn->size(); i++){auto &src = tmpOusterCloudIn->points[i];auto &dst = laserCloudIn->points[i];dst.x = src.x;dst.y = src.y;dst.z = src.z;dst.intensity = src.intensity;dst.ring = src.ring;dst.time = src.t * 1e-9f;}}else{ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));ros::shutdown();}// get timestampcloudHeader = currentCloudMsg.header;//這一點的時間被記錄下來，存入timeScanCur中，函數(shù)deskewPoint中會被加上laserCloudIn->points[i].timetimeScanCur = cloudHeader.stamp.toSec(); //可以看出lasercloudin中存儲的time是一幀中距離起始點的相對時間,timeScanEnd是該幀點云的結(jié)尾時間timeScanEnd = timeScanCur + laserCloudIn->points.back().time;// check dense flagif (laserCloudIn->is_dense == false){ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");ros::shutdown();}// check ring channel//由于static關(guān)鍵字，只檢查一次，檢查ring這個field是否存在. veloodyne和ouster都有;//ring代表線數(shù)，0是最下面那條static int ringFlag = 0;if (ringFlag == 0){ringFlag = -1;for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i){if (currentCloudMsg.fields[i].name == "ring"){ringFlag = 1;break;}}if (ringFlag == -1){ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");ros::shutdown();}}// check point time // 檢查是否存在time通道if (deskewFlag == 0){deskewFlag = -1;for (auto &field : currentCloudMsg.fields){if (field.name == "time" || field.name == "t"){deskewFlag = 1;break;}}if (deskewFlag == -1)ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");}return true;}bool deskewInfo(){std::lock_guard<std::mutex> lock1(imuLock);std::lock_guard<std::mutex> lock2(odoLock);// make sure IMU data available for the scan// 要求imu數(shù)據(jù)時間上包含激光數(shù)據(jù)，否則不往下處理了if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd){ROS_DEBUG("Waiting for IMU data ...");return false;}// 當前幀對應(yīng)imu數(shù)據(jù)處理// 1、遍歷當前激光幀起止時刻之間的imu數(shù)據(jù)，初始時刻對應(yīng)imu的姿態(tài)角RPY設(shè)為當前幀的初始姿態(tài)角// 2、用角速度、時間積分，計算每一時刻相對于初始時刻的旋轉(zhuǎn)量，初始時刻旋轉(zhuǎn)設(shè)為0// 注：imu數(shù)據(jù)都已經(jīng)轉(zhuǎn)換到lidar系下了//imu去畸變參數(shù)計算 imuDeskewInfo();// 當前幀對應(yīng)imu里程計處理// 1、遍歷當前激光幀起止時刻之間的imu里程計數(shù)據(jù)，初始時刻對應(yīng)imu里程計設(shè)為當前幀的初始位姿// 2、用起始、終止時刻對應(yīng)imu里程計，計算相對位姿變換，保存平移增量// 注：imu數(shù)據(jù)都已經(jīng)轉(zhuǎn)換到lidar系下了//里程計去畸變參數(shù)計算odomDeskewInfo();return true;}void imuDeskewInfo(){cloudInfo.imuAvailable = false;// 從imu隊列中刪除當前激光幀0.01s前面時刻的imu數(shù)據(jù)while (!imuQueue.empty()){if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)imuQueue.pop_front();elsebreak;}if (imuQueue.empty())return;imuPointerCur = 0;// 遍歷當前激光幀起止時刻（前后擴展0.01s）之間的imu數(shù)據(jù)for (int i = 0; i < (int)imuQueue.size(); ++i){sensor_msgs::Imu thisImuMsg = imuQueue[i];double currentImuTime = thisImuMsg.header.stamp.toSec();// get roll, pitch, and yaw estimation for this scan// 提取imu姿態(tài)角RPY，作為當前l(fā)idar幀初始姿態(tài)角if (currentImuTime <= timeScanCur)imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);// 超過當前激光幀結(jié)束時刻0.01s，結(jié)束if (currentImuTime > timeScanEnd + 0.01)break;// 第一幀imu旋轉(zhuǎn)角初始化if (imuPointerCur == 0){imuRotX[0] = 0;imuRotY[0] = 0;imuRotZ[0] = 0;imuTime[0] = currentImuTime;++imuPointerCur;continue;}// get angular velocity// 提取imu角速度double angular_x, angular_y, angular_z;imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);// integrate rotationdouble timeDiff = currentImuTime - imuTime[imuPointerCur-1];imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;imuTime[imuPointerCur] = currentImuTime;++imuPointerCur;}--imuPointerCur;// 沒有合規(guī)的imu數(shù)據(jù)if (imuPointerCur <= 0)return;cloudInfo.imuAvailable = true;}//初始pose信息保存在cloudInfo里void odomDeskewInfo(){cloudInfo.odomAvailable = false;// 從imu里程計隊列中刪除當前激光幀0.01s前面時刻的imu數(shù)據(jù)while (!odomQueue.empty()){if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)odomQueue.pop_front();elsebreak;}if (odomQueue.empty())return;// 要求必須有當前激光幀時刻之前的里程計數(shù)據(jù)if (odomQueue.front().header.stamp.toSec() > timeScanCur)return;// get start odometry at the beinning of the scan(地圖優(yōu)化程序中發(fā)布的)nav_msgs::Odometry startOdomMsg;for (int i = 0; i < (int)odomQueue.size(); ++i){startOdomMsg = odomQueue[i];// 在cloudHandler的cachePointCloud函數(shù)中，timeScanCur = cloudHeader.stamp.toSec();，即當前幀點云的初始時刻//找到第一個大于初始時刻的odomif (ROS_TIME(&startOdomMsg) < timeScanCur)continue;elsebreak;}// 提取imu里程計姿態(tài)角tf::Quaternion orientation;tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);double roll, pitch, yaw;tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);// Initial guess used in mapOptimization// 用當前激光幀起始時刻的imu里程計，初始化lidar位姿，后面用于mapOptmizationcloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;cloudInfo.initialGuessRoll = roll;cloudInfo.initialGuessPitch = pitch;cloudInfo.initialGuessYaw = yaw;cloudInfo.odomAvailable = true;// get end odometry at the end of the scanodomDeskewFlag = false;// 如果當前激光幀結(jié)束時刻之后沒有imu里程計數(shù)據(jù)，返回if (odomQueue.back().header.stamp.toSec() < timeScanEnd)return;nav_msgs::Odometry endOdomMsg;// 提取當前激光幀結(jié)束時刻的imu里程計for (int i = 0; i < (int)odomQueue.size(); ++i){endOdomMsg = odomQueue[i];// 在cloudHandler的cachePointCloud函數(shù)中， timeScanEnd = timeScanCur + laserCloudIn->points.back().time;// 找到第一個大于一幀激光結(jié)束時刻的odomif (ROS_TIME(&endOdomMsg) < timeScanEnd)continue;elsebreak;}// 如果起止時刻對應(yīng)imu里程計的方差不等，返回if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))return;//感覺之后計算的信息并沒有被用到Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);// 起止時刻imu里程計的相對變換Eigen::Affine3f transBt = transBegin.inverse() * transEnd;// 相對變換，提取增量平移、旋轉(zhuǎn)（歐拉角）float rollIncre, pitchIncre, yawIncre;// 給定的轉(zhuǎn)換中，提取XYZ以及歐拉角,通過tranBt 獲得增量值后續(xù)去畸變用到pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);odomDeskewFlag = true;}/*** 在當前激光幀起止時間范圍內(nèi)，計算某一時刻的旋轉(zhuǎn)（相對于起始時刻的旋轉(zhuǎn)增量）*/void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur){*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;// 查找當前時刻在imuTime下的索引int imuPointerFront = 0;//imuDeskewInfo中，對imuPointerCur進行計數(shù)(計數(shù)到超過當前激光幀結(jié)束時刻0.01s)while (imuPointerFront < imuPointerCur){//imuTime在imuDeskewInfo（deskewInfo中調(diào)用，deskewInfo在cloudHandler中調(diào)用）被賦值，從imuQueue中取值//pointTime為當前時刻，由此函數(shù)的函數(shù)形參傳入,要找到imu積分列表里第一個大于當前時間的索引if (pointTime < imuTime[imuPointerFront])break;++imuPointerFront;}// 設(shè)為離當前時刻最近的旋轉(zhuǎn)增量//如果計數(shù)為0或該次imu時間戳小于了當前時間戳(異常退出)if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0){//未找到大于當前時刻的imu積分索引//imuRotX等為之前積分出的內(nèi)容.(imuDeskewInfo中)*rotXCur = imuRotX[imuPointerFront];*rotYCur = imuRotY[imuPointerFront];*rotZCur = imuRotZ[imuPointerFront];} else {//// 前后時刻插值計算當前時刻的旋轉(zhuǎn)增量//此時front的時間是大于當前pointTime時間，back=front-1剛好小于當前pointTime時間，前后時刻插值計算int imuPointerBack = imuPointerFront - 1;//算一下該點時間戳在本組imu中的位置double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);//這三項作為函數(shù)返回值，以形參指針的方式返回//按前后百分比賦予旋轉(zhuǎn)量*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;}}void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur){// // 如果傳感器移動速度較慢，例如人行走的速度，那么可以認為激光在一幀時間范圍內(nèi)，平移量小到可以忽略不計*posXCur = 0; *posYCur = 0; *posZCur = 0;// If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.// if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)// return;// float ratio = relTime / (timeScanEnd - timeScanCur);// *posXCur = ratio * odomIncreX;// *posYCur = ratio * odomIncreY;// *posZCur = ratio * odomIncreZ;}/*** 激光運動畸變校正* 利用當前幀起止時刻之間的imu數(shù)據(jù)計算旋轉(zhuǎn)增量，imu里程計數(shù)據(jù)計算平移增量，進而將每一時刻激光點位置變換到第一個激光點坐標系下，進行運動補償*///relTime:laserCloudIn->points[i].timePointType deskewPoint(PointType *point, double relTime){//這個來源于上文的時間戳通道和imu可用判斷，沒有或是不可用則返回點if (deskewFlag == -1 || cloudInfo.imuAvailable == false)return *point;//點的時間等于scan時間加relTime（后文的laserCloudIn->points[i].time）//lasercloudin中存儲的time是一幀中距離起始點的相對時間// 在cloudHandler的cachePointCloud函數(shù)中，timeScanCur = cloudHeader.stamp.toSec();，即當前幀點云的初始時刻//二者相加即可得到當前點的準確時刻double pointTime = timeScanCur + relTime;//根據(jù)時間戳插值獲取imu計算的旋轉(zhuǎn)量與位置量（注意imu計算的相對于起始時刻的旋轉(zhuǎn)增量）float rotXCur, rotYCur, rotZCur;findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);float posXCur, posYCur, posZCur;findPosition(relTime, &posXCur, &posYCur, &posZCur);//這里的firstPointFlag來源于resetParameters函數(shù)，而resetParameters函數(shù)每次ros調(diào)用cloudHandler都會啟動第一個點的位姿增量（0），求逆if (firstPointFlag == true){transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();firstPointFlag = false;//改成false以后，同一幀激光數(shù)據(jù)的下一次就不執(zhí)行了}// transform points to start//掃描當前點時lidar的世界坐標系下變換矩陣Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);//掃描該點相對掃描本次scan第一個點的lidar變換矩陣=//第一個點時lidar世界坐標系下變換矩陣的逆×當前點時lidar世界坐標系下變換矩陣//Tij=Twi^-1 * Twj//注：這里準確的來說，不是世界坐標系,//根據(jù)代碼來看，是把imu積分://從imuDeskewInfo函數(shù)中，在當前激光幀開始的前0.01秒的imu數(shù)據(jù)開始積分，//把它作為原點，然后獲取當前激光幀第一個點時刻的位姿增量transStartInverse，//和當前點時刻的位姿增量transFinal，根據(jù)逆矩陣計算二者變換transBt。//因此相對的不是“世界坐標系”，//而是“當前激光幀開始前的0.01秒的雷達坐標系（在imucallback函數(shù)中已經(jīng)把imu轉(zhuǎn)換到了雷達坐標系了）Eigen::Affine3f transBt = transStartInverse * transFinal;PointType newPoint;//根據(jù)lidar位姿變換 Tij，修正點云位置: Tij * PjnewPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);newPoint.intensity = point->intensity;return newPoint;}void projectPointCloud(){int cloudSize = laserCloudIn->points.size(); //點云數(shù)據(jù)量用于下面一個個點投影// range image projectionfor (int i = 0; i < cloudSize; ++i){PointType thisPoint;//laserCloudIn就是原始的點云話題中的數(shù)據(jù)thisPoint.x = laserCloudIn->points[i].x;thisPoint.y = laserCloudIn->points[i].y;thisPoint.z = laserCloudIn->points[i].z;thisPoint.intensity = laserCloudIn->points[i].intensity;float range = pointDistance(thisPoint);if (range < lidarMinRange || range > lidarMaxRange)continue;//距離圖像的行與點云中ring對應(yīng),//rowIdn計算出該點激光雷達是水平方向上第幾線的。從下往上計數(shù)，-15度記為初始線，第0線，一共16線(N_SCAN=16int rowIdn = laserCloudIn->points[i].ring;if (rowIdn < 0 || rowIdn >= N_SCAN)continue;if (rowIdn % downsampleRate != 0)continue;//水平角分辨率float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;//Horizon_SCAN=1800,每格0.2度static float ang_res_x = 360.0/float(Horizon_SCAN);//horizonAngle 為[-180,180],horizonAngle -90 為[-270,90],-round 為[-90,270], /ang_res_x 為[-450,1350]//+Horizon_SCAN/2為[450,2250]// 即把horizonAngle從[-180,180]映射到[450,2250]int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;//大于1800，則減去1800，相當于把1801～2250映射到1～450//先把columnIdn從horizonAngle:(-PI,PI]轉(zhuǎn)換到columnIdn:[H/4,5H/4],//然后判斷columnIdn大小，把H到5H/4的部分切下來，補到0～H/4的部分。//將它的范圍轉(zhuǎn)換到了[0,H] (H:Horizon_SCAN)。//這樣就把掃描開始的地方角度為0與角度為360的連在了一起，非常巧妙。//如果前方是x，左側(cè)是y，那么正后方左邊是180，右邊是-180。這里的操作就是，把它展開成一幅圖:// 0// 90 -90// 180 || (-180)// (-180) ----- (-90) ------ 0 ------ 90 -------180//變?yōu)? 90 ----180(-180) ---- (-90) ----- (0) ----- 90if (columnIdn >= Horizon_SCAN)columnIdn -= Horizon_SCAN;if (columnIdn < 0 || columnIdn >= Horizon_SCAN)continue;if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)continue;//去畸變運動補償這里需要用到雷達信息中的time 這個fieldthisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);//圖像中填入歐幾里得深度rangeMat.at<float>(rowIdn, columnIdn) = range;// 轉(zhuǎn)換成一維索引，存校正之后的激光點int index = columnIdn + rowIdn * Horizon_SCAN;fullCloud->points[index] = thisPoint;}}void cloudExtraction(){// 有效激光點數(shù)量int count = 0;// extract segmented cloud for lidar odometryfor (int i = 0; i < N_SCAN; ++i){//提取特征的時候，每一行的前5個和最后5個不考慮//記錄每根掃描線起始第5個激光點在一維數(shù)組中的索引//cloudInfo為自定義的msg// 記錄每根掃描線起始第5個激光點在一維數(shù)組中的索引cloudInfo.startRingIndex[i] = count - 1 + 5;///Horizon_SCAN=1800for (int j = 0; j < Horizon_SCAN; ++j){if (rangeMat.at<float>(i,j) != FLT_MAX){// mark the points' column index for marking occlusion later// 記錄激光點對應(yīng)的Horizon_SCAN方向上的索引cloudInfo.pointColInd[count] = j;// save range info激光點距離cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);// save extracted cloud// 加入有效激光點extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);// size of extracted cloud++count;}}// 記錄每根掃描線倒數(shù)第5個激光點在一維數(shù)組中的索引cloudInfo.endRingIndex[i] = count -1 - 5;}}/*** 發(fā)布當前幀校正后點云，有效點*/void publishClouds(){cloudInfo.header = cloudHeader;//publishCloud在utility.h頭文件中,需要傳入發(fā)布句柄pubExtractedCloud，提取出的有效點云，該幀時間戳，//pubExtractedCloud定義在構(gòu)造函數(shù)中，用來發(fā)布去畸變的點云.//extractedCloud主要在cloudExtraction中被提取，點云被去除了畸變，//另外每行頭五個和后五個不要(（仍然被保存，但是之后在提取特征時不要,因為要根據(jù)前后五個點算曲率）//cloudHeader.stamp 來源于currentCloudMsg,cloudHeader在cachePointCloud中被賦值currentCloudMsg.header//而currentCloudMsg是點云隊列cloudQueue中提取的//lidarFrame:在utility.h中被賦為base_link,//在publishCloud函數(shù)中，tempCloud.header.frame_id="base_link"(lidarFrame)//之后用發(fā)布句柄pubExtractedCloud來發(fā)布去畸變的點云cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);//發(fā)布自定義cloud_info信息pubLaserCloudInfo.publish(cloudInfo);//pubExtractedCloud發(fā)布的只有點云信息，而pubLaserCloudInfo發(fā)布的為自定義的很多信息} };int main(int argc, char** argv) {ros::init(argc, argv, "lio_sam");ImageProjection IP;ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");//對于一些只訂閱一個話題的簡單節(jié)點來說，我們使用ros::spin()進入接收循環(huán)，//每當有訂閱的話題發(fā)布時，進入回調(diào)函數(shù)接收和處理消息數(shù)據(jù)。//但是更多的時候，一個節(jié)點往往要接收和處理不同來源的數(shù)據(jù)，并且這些數(shù)據(jù)的產(chǎn)生頻率也各不相同，//當我們在一個回調(diào)函數(shù)里耗費太多時間時，會導(dǎo)致其他回調(diào)函數(shù)被阻塞，導(dǎo)致數(shù)據(jù)丟失。//這種場合需要給一個節(jié)點開辟多個線程，保證數(shù)據(jù)流的暢通。ros::MultiThreadedSpinner spinner(3);spinner.spin();return 0; }

總結(jié)

我會依次把下面的內(nèi)容標號，請注意我的標號。

和大多數(shù)的SLAM算法一樣，程序就在回調(diào)函數(shù)里處理算法。

1：imu原始數(shù)據(jù)送入imuhandle，放入imuqueue中；

2.odometry/imu_incremental送入odomhandle，放入odomqueue中。

這里要說一下，注意兩點：

第一，這個話題是imu里程計，是來自IMUPreintegration(imuPreintegration.cpp中的類IMUPreintegration)發(fā)布的里程計話題。既然叫里程計，就不是兩幀之間的預(yù)積分數(shù)據(jù)。不要被它名字里的incremental誤導(dǎo)了，以為它好像是一個增加的量，以為是一段中間時刻內(nèi)的數(shù)據(jù)。這種理解是不對的。它就是一個里程計數(shù)據(jù)，通過imu預(yù)積分計算優(yōu)化來得到的任意時刻在世界坐標系下的位姿。

第二，mapOptimization.cpp文件還會發(fā)布一個叫"lio_sam/mapping/odometry_incremental"，它代表的是激光里程計，不要和這里的odometry/imu_incremental混起來。

3.原始點云數(shù)據(jù)送入cloudhandle：

3.1 點云加入cloudQueue，檢查數(shù)據(jù)有效性（格式上，主要因為雷達款式不同）

3.2 deskewinfo：

?3.2.1 imuDeskewInfo：遍歷激光幀，從imuqueue中找當前激光幀前0.01s開始，當前幀后0.01s結(jié)束，找到最近的一個imu數(shù)據(jù)，把它的原始角度數(shù)據(jù)，作為cloudInfo.imuRollInit等變量。cloudInfo是自定義的一個消息類型，用來之后發(fā)布去畸變的點云，具體可以看cloud_info.msg里面的定義。與此同時，要根據(jù)角速度信息做一個積分，保存到imuRotX等數(shù)據(jù)結(jié)構(gòu)中，之后用來去畸變。

3.2.2 odomDeskewInfo：遍歷imu里程計的odomqueue隊列，剔除當前0.01s之前的數(shù)據(jù)，找到第一個大于當前激光幀的數(shù)據(jù)，然后用里程計數(shù)據(jù)來初始化雷達，也是填充角度，不過這次填充的是cloudInfo.initialGuessRoll等變量，位置也會被填充到initialGuessX里。

注意：cloudInfo.initialGuessRoll和cloudInfo.imuRollInit，這些東西都是角度數(shù)據(jù)，帶Guess的為imu里程計提供的數(shù)據(jù)，imuRollInit這種為imu原始數(shù)據(jù)。如果有合適的，分別會填充cloudInfo.odomAvailable和cloudInfo.imuAvailable變量為true，代表這塊的數(shù)據(jù)可用。這些數(shù)據(jù)到底用在哪里呢？

這也需要劇透：會被用在mapOptimization.cpp的updateInitGuess函數(shù)，給激光里程計做一個初始化，然后在這個初始化的基礎(chǔ)上進行非線性優(yōu)化。

?3.3 projectPointCloud：

3.3.1 獲取點云中的每一個點，投影圖像。

3.3.2 去畸變。假設(shè)一幀激光數(shù)據(jù)，是在一個很小的時間段內(nèi)獲取的，但是由于這段時間內(nèi)，激光雷達可能處于運動狀態(tài)，那么就可能導(dǎo)致運動畸變。這里就要用到了3.2.1中提到的imuRotX等玩意，根據(jù)imu的運動信息，把這段時間內(nèi)依次接收的每個點統(tǒng)一投影到初始時刻，去除運動畸變。

3.4 CloudExtraction：

這個是想把特征依次裝到一個一維數(shù)組中，然后發(fā)布到別的進程里處理。

既然是一維數(shù)組，16線的點都弄到一維數(shù)組里，那就比較麻煩。雖然說這里要弄一個深度投影圖像，但是其實進程之間傳輸?shù)牟⒉皇沁@個深度投影圖像，而是自定義的cloudinfo形式的數(shù)據(jù)。

實際上是想把有效的數(shù)據(jù)記錄進來，例如空值不要，每根線前5個和后5個也不要……那這樣16線的數(shù)據(jù)混到一維組里就分不清了，所以要記錄下每根掃瞄線起始點在1維數(shù)組中的索引startRingIndex，結(jié)束點的索引endRingIndex，在原始圖像中的列數(shù)pointColInd，距離值pointRange。然后把有效激光點，放到extractedCloud中。

3.5 publishClouds：

把extractedCloud發(fā)布出去，/lio_sam/deskew/cloud_deskewed，這個只有點云信息。

然后再把這些點，賦值到cloudInfo.cloud_deskewed中，把整個的cloudInfo發(fā)布出去，名字就叫“l(fā)io_sam/deskew/cloud_info"，這個就是作者自己定義的特殊的msg。

featureExtraction.cpp

注釋

#include "utility.h" #include "lio_sam/cloud_info.h"struct smoothness_t{ float value; // 曲率值size_t ind; // 激光點一維索引 };/*** 曲率比較函數(shù)，從小到大排序 */ struct by_value{ bool operator()(smoothness_t const &left, smoothness_t const &right) { return left.value < right.value;} };class FeatureExtraction : public ParamServer {public:ros::Subscriber subLaserCloudInfo;// 發(fā)布當前激光幀提取特征之后的點云信息ros::Publisher pubLaserCloudInfo;// 發(fā)布當前激光幀提取的角點點云ros::Publisher pubCornerPoints;// 發(fā)布當前激光幀提取的平面點點云ros::Publisher pubSurfacePoints;// 當前激光幀運動畸變校正后的有效點云pcl::PointCloud<PointType>::Ptr extractedCloud;// 當前激光幀角點點云集合pcl::PointCloud<PointType>::Ptr cornerCloud;// 當前激光幀平面點點云集合pcl::PointCloud<PointType>::Ptr surfaceCloud;pcl::VoxelGrid<PointType> downSizeFilter; // 當前激光幀點云信息，包括的歷史數(shù)據(jù)有：運動畸變校正，點云數(shù)據(jù)，初始位姿，姿態(tài)角，有效點云數(shù)據(jù)，角點點云，平面點點云等lio_sam::cloud_info cloudInfo;std_msgs::Header cloudHeader;std::vector<smoothness_t> cloudSmoothness;//用來做曲率計算的中間變量float *cloudCurvature;// 特征提取標記，1表示遮擋、平行，或者已經(jīng)進行特征提取的點，0表示還未進行特征提取處理int *cloudNeighborPicked;// 1表示角點，-1表示平面點int *cloudLabel;FeatureExtraction(){// 訂閱當前激光幀運動畸變校正后的點云信息subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());// 發(fā)布當前激光幀提取特征之后的點云信息pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);// 發(fā)布當前激光幀的角點點云pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);// 發(fā)布當前激光幀的面點點云pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);// 初始化initializationValue();}void initializationValue(){cloudSmoothness.resize(N_SCAN*Horizon_SCAN);downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);extractedCloud.reset(new pcl::PointCloud<PointType>());cornerCloud.reset(new pcl::PointCloud<PointType>());surfaceCloud.reset(new pcl::PointCloud<PointType>());cloudCurvature = new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];cloudLabel = new int[N_SCAN*Horizon_SCAN];}//接收imageProjection.cpp中發(fā)布的去畸變的點云，實時處理的回調(diào)函數(shù)void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){//msgIn即為回調(diào)函數(shù)獲取的去畸變點云信息cloudInfo = *msgIn; // new cloud infocloudHeader = msgIn->header; // new cloud headerpcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction// 計算當前激光幀點云中每個點的曲率calculateSmoothness();// 標記屬于遮擋、平行兩種情況的點，不做特征提取markOccludedPoints();// 點云角點、平面點特征提取// 1、遍歷掃描線，每根掃描線掃描一周的點云劃分為6段，針對每段提取20個角點、不限數(shù)量的平面點，加入角點集合、平面點集合// 2、認為非角點的點都是平面點，加入平面點云集合，最后降采樣extractFeatures();// 發(fā)布角點、面點點云，發(fā)布帶特征點云數(shù)據(jù)的當前激光幀點云信息publishFeatureCloud();}void calculateSmoothness(){// 遍歷當前激光幀運動畸變校正后的有效點云int cloudSize = extractedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++){// 用當前激光點前后5個點計算當前點的曲率//注意，這里把前后五個點共10個點加起來，還減去了10倍的當前點float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]+ cloudInfo.pointRange[i+5]; // 距離差值平方作為曲率cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;//0表示還未進行特征提取處理,1表示遮擋、平行，或者已經(jīng)進行特征提取的點cloudNeighborPicked[i] = 0;//1表示角點，-1表示平面點cloudLabel[i] = 0;// 存儲該點曲率值、激光點一維索引//之所以可以這樣操作，是因為在initializationValue部分，對cloudSmoothness進行過初始化，//否則直接對cloudSmoothness[i]賦值，一定會報段錯誤cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}void markOccludedPoints(){int cloudSize = extractedCloud->points.size();// mark occluded points and parallel beam pointsfor (int i = 5; i < cloudSize - 6; ++i){// 當前點和下一個點的range值float depth1 = cloudInfo.pointRange[i];float depth2 = cloudInfo.pointRange[i+1];// 兩個激光點之間的一維索引差值，如果在一條掃描線上，那么值為1；//如果兩個點之間有一些無效點被剔除了，可能會比1大，但不會特別大// 如果恰好前一個點在掃描一周的結(jié)束時刻，下一個點是另一條掃描線的起始時刻，那么值會很大int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));// 兩個點在同一掃描線上，且距離相差大于0.3，認為存在遮擋關(guān)系//（也就是這兩個點不在同一平面上，如果在同一平面上，距離相差不會太大）// 遠處的點會被遮擋，標記一下該點以及相鄰的5個點，后面不再進行特征提取if (columnDiff < 10){// 10 pixel diff in range imageif (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}// parallel beam// 用前后相鄰點判斷當前點所在平面是否與激光束方向平行//diff1和diff2是當前點距離前后兩個點的距離float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));//如果當前點距離左右鄰點都過遠，則視其為瑕點，因為入射角可能太小導(dǎo)致誤差較大// 選擇距離變化較大的點，并將他們標記為1if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])cloudNeighborPicked[i] = 1;}}void extractFeatures(){cornerCloud->clear();surfaceCloud->clear();pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());for (int i = 0; i < N_SCAN; i++){surfaceCloudScan->clear();// 將一條掃描線掃描一周的點云數(shù)據(jù)，劃分為6段，每段分開提取有限數(shù)量的特征，保證特征均勻分布for (int j = 0; j < 6; j++){// 每段點云的起始、結(jié)束索引；startRingIndex為掃描線起始第5個激光點在一維數(shù)組中的索引//注意：所有的點云在這里都是以"一維數(shù)組"的形式保存//startRingIndex和 endRingIndex 在imageProjection.cpp中的 cloudExtraction函數(shù)里被填入//假設(shè) 當前ring在一維數(shù)組中起始點是m，結(jié)尾點為n（不包括n），那么6段的起始點分別為：// m + [(n-m)/6]*j j從0～5// 化簡為 [（6-j)*m + nj ]/6// 6段的終止點分別為：// m + (n-m)/6 + [(n-m)/6]*j -1 j從0～5,-1是因為最后一個,減去1// 化簡為 [（5-j)*m + (j+1)*n ]/6 -1//這塊不必細究邊緣值到底是不是劃分的準（例如考慮前五個點是不是都不要，還是說只不要前四個點），//只是盡可能的分開成六段，首位相接的地方不要。因為龐大的點云中，一兩個點其實無關(guān)緊要。int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;if (sp >= ep)continue;// 按照曲率從小到大排序點云//可以看出之前的byvalue在這里被當成了判斷函數(shù)來用std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());int largestPickedNum = 0;// 按照曲率從大到小遍歷for (int k = ep; k >= sp; k--){// 激光點的索引int ind = cloudSmoothness[k].ind;// 當前激光點還未被處理，且曲率大于閾值，則認為是角點if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold){// 每段只取20個角點，如果單條掃描線掃描一周是1800個點，則劃分6段，每段300個點，從中提取20個角點largestPickedNum++;if (largestPickedNum <= 20){// 標記為角點,加入角點點云cloudLabel[ind] = 1;cornerCloud->push_back(extractedCloud->points[ind]);} else {break;}// 標記已被處理cloudNeighborPicked[ind] = 1;// 同一條掃描線上后5個點標記一下，不再處理，避免特征聚集for (int l = 1; l <= 5; l++){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));//大于10，說明距離遠，則不作標記if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}// 同一條掃描線上前5個點標記一下，不再處理，避免特征聚集for (int l = -1; l >= -5; l--){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}// 按照曲率從小到大遍歷for (int k = sp; k <= ep; k++){// 激光點的索引int ind = cloudSmoothness[k].ind;// 當前激光點還未被處理，且曲率小于閾值，則認為是平面點if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold){// 標記為平面點cloudLabel[ind] = -1;// 標記已被處理cloudNeighborPicked[ind] = 1;// 同一條掃描線上后5個點標記一下，不再處理，避免特征聚集for (int l = 1; l <= 5; l++) {int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}// 同一條掃描線上前5個點標記一下，不再處理，避免特征聚集for (int l = -1; l >= -5; l--) {int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}// 平面點和未被處理的點(<=0)，都認為是平面點，加入平面點云集合for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0){surfaceCloudScan->push_back(extractedCloud->points[k]);}}}// 平面點云降采樣surfaceCloudScanDS->clear();downSizeFilter.setInputCloud(surfaceCloudScan);downSizeFilter.filter(*surfaceCloudScanDS);// 加入平面點云集合*surfaceCloud += *surfaceCloudScanDS;//用surfaceCloudScan來裝數(shù)據(jù)，然后放到downSizeFilter里，//再用downSizeFilter進行.filter()操作，把結(jié)果輸出到*surfaceCloudScanDS里。//最后把DS裝到surfaceCloud中。DS指的是DownSample。//同樣角點（邊緣點）則沒有這樣的操作，直接就用cornerCloud來裝點云。}}/*** 清理*/void freeCloudInfoMemory(){cloudInfo.startRingIndex.clear();cloudInfo.endRingIndex.clear();cloudInfo.pointColInd.clear();cloudInfo.pointRange.clear();}/*** 發(fā)布角點、面點點云，發(fā)布帶特征點云數(shù)據(jù)的當前激光幀點云信息*/void publishFeatureCloud(){// free cloud info memoryfreeCloudInfoMemory();// save newly extracted features// 發(fā)布角點、面點點云，用于rviz展示cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);// publish to mapOptimization// 發(fā)布當前激光幀點云信息，加入了角點、面點點云數(shù)據(jù)，發(fā)布給mapOptimization// 和imageProjection.cpp發(fā)布的不是同一個話題，// image發(fā)布的是"lio_sam/deskew/cloud_info"，// 這里發(fā)布的是"lio_sam/feature/cloud_info"，// 因此不用擔(dān)心地圖優(yōu)化部分的沖突pubLaserCloudInfo.publish(cloudInfo);} };int main(int argc, char** argv) {ros::init(argc, argv, "lio_sam");FeatureExtraction FE;ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");ros::spin();return 0; }

總結(jié)

這個cpp原理比較簡單，就是訂閱剛剛在imageProjection.cpp總結(jié)中的3.5部分中發(fā)布的lio_sam/deskew/cloud_info消息，然后提取邊緣點（角點），平面點，然后填充cloud_info的cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式發(fā)布出去。

1.calculateSmoothness，計算當前激光幀點云中每個點的曲率，比較簡單不詳細說了。

2.markOccludedPoints，標記被遮擋的點。

這點還是可以說一下，第一個，在cloudinfo中，pointCollnd記錄的是當前點在原始掃瞄線圖像中的列值，列值在10以內(nèi)，即2度范圍內(nèi)（10*360/1800），兩點之間深度差0.3m，就不考慮這種點；第二，當前點左右的有效點的距離和它比都太大了，那么不要這個點。

?3.extractFeatures：

對每條線上大曲率進行排序，每條線分為6段，每段最多提取20個。大于閾值就是角點（默認0.1），并且標記該點周圍其他點已經(jīng)被提取了。小于閾值同理，當成平面點。不過平面點要經(jīng)過一個降采樣過程（角點不用，可能是平面點比較多）。角點以"lio_sam/feature/cloud_corner"發(fā)布，平面點以"lio_sam/feature/cloud_surface"發(fā)布，然后把二者合在一起，在lio_sam/deskew/cloud_info的基礎(chǔ)上，填充cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式發(fā)布出去。（注意名字不一樣了，同樣是cloud_info，只不過deskew/cloud_info是imageProjection.cpp發(fā)布的，feature/cloud_info是featureExtraction.cpp發(fā)布的，后者比前者多了角點和平面點字段。deskew/cloud_info在此之后就算壽終正寢了，沒有其他的進程需要監(jiān)聽它了。）

imuPreintergration.cpp

注釋

#include "utility.h"#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Pose3.h> #include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/BetweenFactor.h> #include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Values.h> #include <gtsam/inference/Symbol.h>#include <gtsam/nonlinear/ISAM2.h> #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)// 訂閱激光里程計（來自MapOptimization）和IMU里程計， //根據(jù)前一時刻激光里程計，和該時刻到當前時刻的IMU里程計變換增量， //計算當前時刻IMU里程計； //rviz展示IMU里程計軌跡（局部）。 class TransformFusion : public ParamServer { public:std::mutex mtx;ros::Subscriber subImuOdometry;ros::Subscriber subLaserOdometry;ros::Publisher pubImuOdometry;ros::Publisher pubImuPath;Eigen::Affine3f lidarOdomAffine;Eigen::Affine3f imuOdomAffineFront;Eigen::Affine3f imuOdomAffineBack;tf::TransformListener tfListener;tf::StampedTransform lidar2Baselink;double lidarOdomTime = -1;deque<nav_msgs::Odometry> imuOdomQueue;TransformFusion(){// 如果lidar系與baselink系不同（激光系和載體系），需要外部提供二者之間的變換關(guān)系if(lidarFrame != baselinkFrame){try{// 等待3stfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));// lidar系到baselink系的變換tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());}}// 訂閱激光里程計，來自mapOptimizationsubLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());// 訂閱imu里程計，來自IMUPreintegration(IMUPreintegration.cpp中的類IMUPreintegration)//topic name: odometry/imu_incremental//注意區(qū)分lio_sam/mapping/odometry_incremental//目前可以明確的一點，odometry/imu_incremental是增量內(nèi)容，即兩幀激光里程計之間的預(yù)積分內(nèi)容,（加上開始的激光里程計本身有的位姿）//imuIntegratorImu_本身是個積分器，只有兩幀之間的預(yù)積分，但是發(fā)布的時候發(fā)布的實際是結(jié)合了前述里程計本身有的位姿//如這個predict里的prevStateOdom://currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());// 發(fā)布imu里程計，用于rviz展示pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);// 發(fā)布imu里程計軌跡pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);}/*** 里程計對應(yīng)變換矩陣*/Eigen::Affine3f odom2affine(nav_msgs::Odometry odom){double x, y, z, roll, pitch, yaw;x = odom.pose.pose.position.x;y = odom.pose.pose.position.y;z = odom.pose.pose.position.z;tf::Quaternion orientation;tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);return pcl::getTransformation(x, y, z, roll, pitch, yaw);}/*** 訂閱激光里程計的回調(diào)函數(shù)，來自mapOptimization*/void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){std::lock_guard<std::mutex> lock(mtx);// 激光里程計對應(yīng)變換矩陣lidarOdomAffine = odom2affine(*odomMsg);// 激光里程計時間戳lidarOdomTime = odomMsg->header.stamp.toSec();//這二者里面保存的都是最近的一個雷達激光里程計的變換和時間戳（不再是用一個vector之類的東西保存起來）}/*** 訂閱imu里程計，來自IMUPreintegration* 1、以最近一幀激光里程計位姿為基礎(chǔ)，計算該時刻與當前時刻間imu里程計增量位姿變換，相乘得到當前時刻imu里程計位姿* 2、發(fā)布當前時刻里程計位姿，用于rviz展示；發(fā)布imu里程計路徑，注：只是最近一幀激光里程計時刻與當前時刻之間的一段*/void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){// 發(fā)布tf，map與odom系設(shè)為同一個系static tf::TransformBroadcaster tfMap2Odom;static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));std::lock_guard<std::mutex> lock(mtx);// 添加imu里程計到隊列，注：imu里程計由本cpp中的另一個類imuPreintegration來發(fā)布imuOdomQueue.push_back(*odomMsg);// get latest odometry (at current IMU stamp)// 從imu里程計隊列中刪除當前（最近的一幀）激光里程計時刻之前的數(shù)據(jù)// lidarOdomTime初始化為-1，在收到lidar里程計數(shù)據(jù)后，在回調(diào)函數(shù)lidarOdometryHandler中被賦值時間戳if (lidarOdomTime == -1)return;while (!imuOdomQueue.empty()){if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)imuOdomQueue.pop_front();elsebreak;}// 最近的一幀激光里程計時刻對應(yīng)imu里程計位姿Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());// 當前時刻imu里程計位姿Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// imu里程計增量位姿變換Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;// 當前時刻imu里程計位姿=最近的一幀激光里程計位姿 * imu里程計增量位姿變換 //lidarOdomAffine在本類的lidarOdometryHandler回調(diào)函數(shù)中被賦值，消息來源于mapOptimization.cpp發(fā)布,是激光里程計Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);// publish latest odometry//發(fā)布名為"odometry/imu"的話題//也就是說，這個函數(shù)先監(jiān)聽了類IMUPreintegration中發(fā)布的odometry/imu_incremental，//然后計算imu二者之間的增量，然后在激光的基礎(chǔ)上加上增量，重新發(fā)布//可以看出發(fā)布的并非odometry/imu_incremental的一部分子數(shù)據(jù)，而是把x，y，z，roll，pitch，yaw都經(jīng)過了激光里程計的修正，才發(fā)布//可以看出這次發(fā)布的內(nèi)容，是當前時刻里程計位姿.發(fā)布名稱為"odometry/imu"nav_msgs::Odometry laserOdometry = imuOdomQueue.back(); laserOdometry.pose.pose.position.x = x; //賦上新的值laserOdometry.pose.pose.position.y = y;laserOdometry.pose.pose.position.z = z;laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);pubImuOdometry.publish(laserOdometry);// 發(fā)布tf，當前時刻odom與baselink系變換關(guān)系//由于之前把map和odom坐標系固定了，因此這里我認為發(fā)布的就是真正的最終位姿關(guān)系//map優(yōu)化提供激光，預(yù)積分提供imu，imu之間變換再乘以激光里程計得到各個時刻精確位姿static tf::TransformBroadcaster tfOdom2BaseLink;tf::Transform tCur;tf::poseMsgToTF(laserOdometry.pose.pose, tCur);if(lidarFrame != baselinkFrame)tCur = tCur * lidar2Baselink;tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);tfOdom2BaseLink.sendTransform(odom_2_baselink);// publish IMU path// 發(fā)布imu里程計路徑，注：只是最近一幀激光里程計時刻與當前時刻之間的一段static nav_msgs::Path imuPath;static double last_path_time = -1;double imuTime = imuOdomQueue.back().header.stamp.toSec();// 每隔0.1s添加一個if (imuTime - last_path_time > 0.1){last_path_time = imuTime;geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;pose_stamped.header.frame_id = odometryFrame; //就叫"odom"pose_stamped.pose = laserOdometry.pose.pose;imuPath.poses.push_back(pose_stamped);// 刪除最近一幀激光里程計時刻之前的imu里程計while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)imuPath.poses.erase(imuPath.poses.begin());if (pubImuPath.getNumSubscribers() != 0){imuPath.header.stamp = imuOdomQueue.back().header.stamp;imuPath.header.frame_id = odometryFrame;pubImuPath.publish(imuPath);}}} };class IMUPreintegration : public ParamServer { public:std::mutex mtx;ros::Subscriber subImu;ros::Subscriber subOdometry;ros::Publisher pubImuOdometry;bool systemInitialized = false;// 噪聲協(xié)方差gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;gtsam::Vector noiseModelBetweenBias;// imu預(yù)積分器//imuIntegratorOpt_負責(zé)預(yù)積分兩個激光里程計之間的imu數(shù)據(jù)，作為約束加入因子圖，并且優(yōu)化出biasgtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;//imuIntegratorImu_用來根據(jù)新的激光里程計到達后已經(jīng)優(yōu)化好的bias，預(yù)測從當前幀開始，下一幀激光里程計到達之前的imu里程計增量gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;// imu數(shù)據(jù)隊列//imuQueOpt用來給imuIntegratorOpt_提供數(shù)據(jù)來源，不要的就彈出(從隊頭開始出發(fā)，比當前激光里程計數(shù)據(jù)早的imu通通積分，用一個扔一個)；std::deque<sensor_msgs::Imu> imuQueOpt;//imuQueImu用來給imuIntegratorImu_提供數(shù)據(jù)來源，不要的就彈出(彈出當前激光里程計之前的imu數(shù)據(jù),預(yù)積分用完一個彈一個)；std::deque<sensor_msgs::Imu> imuQueImu;// imu因子圖優(yōu)化過程中的狀態(tài)變量gtsam::Pose3 prevPose_;gtsam::Vector3 prevVel_;gtsam::NavState prevState_;gtsam::imuBias::ConstantBias prevBias_;// imu狀態(tài)gtsam::NavState prevStateOdom;gtsam::imuBias::ConstantBias prevBiasOdom;bool doneFirstOpt = false;double lastImuT_imu = -1;double lastImuT_opt = -1;// ISAM2優(yōu)化器gtsam::ISAM2 optimizer;gtsam::NonlinearFactorGraph graphFactors; //總的因子圖模型gtsam::Values graphValues; //因子圖模型中的值const double delta_t = 0;int key = 1;// imu-lidar位姿變換//這點要注意，tixiaoshan這里命名的很垃圾，這只是一個平移變換，//同樣頭文件的imuConverter中，也只有一個旋轉(zhuǎn)變換。這里絕對不可以理解為把imu數(shù)據(jù)轉(zhuǎn)到lidar下的變換矩陣。//事實上，作者后續(xù)是把imu數(shù)據(jù)先用imuConverter旋轉(zhuǎn)到雷達系下（但其實還差了個平移）。//作者真正是把雷達數(shù)據(jù)又根據(jù)lidar2Imu反向平移了一下，和轉(zhuǎn)換以后差了個平移的imu數(shù)據(jù)在“中間系”對齊，//之后算完又從中間系通過imu2Lidar挪回了雷達系進行publish。gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));IMUPreintegration(){//訂閱imu原始數(shù)據(jù)，用下面因子圖優(yōu)化的結(jié)果，施加兩幀之間的imu預(yù)計分量，預(yù)測每一時刻（imu頻率）的imu里程計//imuTopic name: "imu_correct"subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());// 訂閱激光里程計，來自mapOptimization，用兩幀之間的imu預(yù)計分量構(gòu)建因子圖，// 優(yōu)化當前幀位姿（這個位姿僅用于更新每時刻的imu里程計，以及下一次因子圖優(yōu)化）subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());//發(fā)布imu里程計: odometry/imu_incrementalpubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);// imu預(yù)積分的噪聲協(xié)方差boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);//imuAccNoise和imuGyrNoise都是定義在頭文件中的高斯白噪聲，由配置文件中寫入p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuousp->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous//對于速度的積分誤差？這塊暫時不太理解p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities//假設(shè)沒有初始的biasgtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias// 噪聲先驗//Diagonal對角線矩陣//發(fā)現(xiàn)diagonal型一般調(diào)用.finished(),注釋中說finished()意為設(shè)置完所有系數(shù)后返回構(gòu)建的矩陣priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, mpriorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/spriorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good// 激光里程計scan-to-map優(yōu)化過程中發(fā)生退化，則選擇一個較大的協(xié)方差correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, mcorrectionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, mnoiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();//imu預(yù)積分器，用于預(yù)測每一時刻（imu頻率）的imu里程計（轉(zhuǎn)到lidar系了，與激光里程計同一個系）imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread//imu預(yù)積分器，用于因子圖優(yōu)化imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization }void resetOptimization(){gtsam::ISAM2Params optParameters;optParameters.relinearizeThreshold = 0.1;optParameters.relinearizeSkip = 1;optimizer = gtsam::ISAM2(optParameters);gtsam::NonlinearFactorGraph newGraphFactors;graphFactors = newGraphFactors;gtsam::Values NewGraphValues;graphValues = NewGraphValues;}void resetParams(){lastImuT_imu = -1;doneFirstOpt = false;systemInitialized = false;}// 訂閱的是激光里程計,"lio_sam/mapping/odometry_incremental"void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){std::lock_guard<std::mutex> lock(mtx);// 當前幀激光里程計時間戳double currentCorrectionTime = ROS_TIME(odomMsg);// 確保imu優(yōu)化隊列中有imu數(shù)據(jù)進行預(yù)積分if (imuQueOpt.empty())return;// 當前幀激光位姿，來自scan-to-map匹配、因子圖優(yōu)化后的位姿float p_x = odomMsg->pose.pose.position.x;float p_y = odomMsg->pose.pose.position.y;float p_z = odomMsg->pose.pose.position.z;float r_x = odomMsg->pose.pose.orientation.x;float r_y = odomMsg->pose.pose.orientation.y;float r_z = odomMsg->pose.pose.orientation.z;float r_w = odomMsg->pose.pose.orientation.w;bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));// 0. initialize system// 0. 系統(tǒng)初始化，第一幀if (systemInitialized == false){// 重置ISAM2優(yōu)化器resetOptimization();// pop old IMU message// 從imu優(yōu)化隊列中刪除當前幀激光里程計時刻之前的imu數(shù)據(jù),delta_t=0while (!imuQueOpt.empty()){if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t){lastImuT_opt = ROS_TIME(&imuQueOpt.front());imuQueOpt.pop_front();}elsebreak;}// initial pose// 添加里程計位姿先驗因子//lidarPose 為本回調(diào)函數(shù)收到的激光里程計數(shù)據(jù)，重組成gtsam的pose格式//并轉(zhuǎn)到imu坐標系下,我猜測compose可能類似于左乘之類的含義吧prevPose_ = lidarPose.compose(lidar2Imu);//X可能是固定搭配（當使用Pose時），如果是速度則是V，bias則是Bgtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);//通過調(diào)用總的因子圖模型的add方式，添加第一個因子//PriorFactor 概念可看gtsam 包括了位姿速度 bias //加入PriorFactor在圖優(yōu)化中基本都是必需的前提//各種noise都定義在構(gòu)造函數(shù)當中g(shù)raphFactors.add(priorPose);// initial velocityprevVel_ = gtsam::Vector3(0, 0, 0);gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);graphFactors.add(priorVel);// initial biasprevBias_ = gtsam::imuBias::ConstantBias();gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);graphFactors.add(priorBias);// add values// 變量節(jié)點賦初值graphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// optimize once// 優(yōu)化一次optimizer.update(graphFactors, graphValues);//圖和節(jié)點均清零為什么要清零不能繼續(xù)用嗎?//是因為節(jié)點信息保存在gtsam::ISAM2 optimizer，所以要清理后才能繼續(xù)使用graphFactors.resize(0);graphValues.clear();//積分器重置,重置優(yōu)化之后的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);key = 1;systemInitialized = true;return;}// reset graph for speed// 每隔100幀激光里程計，重置ISAM2優(yōu)化器，保證優(yōu)化效率if (key == 100){// get updated noise before reset// 前一幀的位姿、速度、偏置噪聲模型//保存最后的噪聲值gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));// reset graph// 重置ISAM2優(yōu)化器resetOptimization();// add pose// 添加位姿先驗因子，用前一幀的值初始化//重置之后還有類似與初始化的過程區(qū)別在于噪聲值不同//prevPose_等三項，也是上一時刻得到的，//（初始時刻是lidar里程計的pose直接用lidar2IMU變量轉(zhuǎn)到imu坐標系下，而此處則是通過上一時刻，即接下來的后續(xù)優(yōu)化中得到）gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);graphFactors.add(priorPose);// add velocitygtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);graphFactors.add(priorVel);// add biasgtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);graphFactors.add(priorBias);// add valuesgraphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// optimize onceoptimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();key = 1;}// 1. integrate imu data and optimize// 1. 計算前一幀與當前幀之間的imu預(yù)積分量，用前一幀狀態(tài)施加預(yù)積分量得到當前幀初始狀態(tài)估計，// 添加來自mapOptimization的當前幀位姿，進行因子圖優(yōu)化，更新當前幀狀態(tài)while (!imuQueOpt.empty()){// pop and integrate imu data that is between two optimizations// 提取前一幀與當前幀之間的imu數(shù)據(jù)，計算預(yù)積分sensor_msgs::Imu *thisImu = &imuQueOpt.front();double imuTime = ROS_TIME(thisImu);//currentCorrectionTime是當前回調(diào)函數(shù)收到的激光里程計數(shù)據(jù)的時間if (imuTime < currentCorrectionTime - delta_t){double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);// imu預(yù)積分數(shù)據(jù)輸入：加速度、角速度、dt// 加入的是這個用來因子圖優(yōu)化的預(yù)積分器imuIntegratorOpt_,注意加入了上一步算出的dt//作者要求的9軸imu數(shù)據(jù)中歐拉角在本程序文件中沒有任何用到,全在地圖優(yōu)化里用到的imuIntegratorOpt_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);//在推出一次數(shù)據(jù)前保存上一個數(shù)據(jù)的時間戳lastImuT_opt = imuTime;// 從隊列中刪除已經(jīng)處理的imu數(shù)據(jù)imuQueOpt.pop_front();}elsebreak;}// add imu factor to graph//利用兩幀之間的IMU數(shù)據(jù)完成了預(yù)積分后增加imu因子到因子圖中,//注意后面容易被遮擋，imuIntegratorOpt_的值經(jīng)過格式轉(zhuǎn)換被傳入preint_imu，//因此可以推測imuIntegratorOpt_中的integrateMeasurement函數(shù)應(yīng)該就是一個簡單的積分輪子，//傳入數(shù)據(jù)和dt，得到一個積分量,數(shù)據(jù)會被存放在imuIntegratorOpt_中const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);// 參數(shù)：前一幀位姿，前一幀速度，當前幀位姿，當前幀速度，前一幀偏置，預(yù)計分量gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);graphFactors.add(imu_factor);// add imu bias between factor// 添加imu偏置因子，前一幀偏置B(key - 1)，當前幀偏置B(key)，觀測值，噪聲協(xié)方差；deltaTij()是積分段的時間graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));// add pose factor// 添加位姿因子gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);graphFactors.add(pose_factor);// insert predicted values// 用前一幀的狀態(tài)、偏置，施加imu預(yù)計分量，得到當前幀的狀態(tài)gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);// 變量節(jié)點賦初值graphValues.insert(X(key), propState_.pose());graphValues.insert(V(key), propState_.v());graphValues.insert(B(key), prevBias_);// optimizeoptimizer.update(graphFactors, graphValues);optimizer.update();graphFactors.resize(0);graphValues.clear();// Overwrite the beginning of the preintegration for the next step.// 優(yōu)化結(jié)果gtsam::Values result = optimizer.calculateEstimate();// 更新當前幀位姿、速度prevPose_ = result.at<gtsam::Pose3>(X(key));prevVel_ = result.at<gtsam::Vector3>(V(key));// 更新當前幀狀態(tài)prevState_ = gtsam::NavState(prevPose_, prevVel_);// 更新當前幀imu偏置prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));// Reset the optimization preintegration object.//重置預(yù)積分器，設(shè)置新的偏置，這樣下一幀激光里程計進來的時候，預(yù)積分量就是兩幀之間的增量imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);// check optimization// imu因子圖優(yōu)化結(jié)果，速度或者偏置過大，認為失敗if (failureDetection(prevVel_, prevBias_)){resetParams();return;}// 2. after optiization, re-propagate imu odometry preintegration// 2. 優(yōu)化之后，執(zhí)行重傳播；優(yōu)化更新了imu的偏置，//用最新的偏置重新計算當前激光里程計時刻之后的imu預(yù)積分，這個預(yù)積分用于計算每時刻位姿prevStateOdom = prevState_;prevBiasOdom = prevBias_;// first pop imu message older than current correction data// 從imu隊列中刪除當前激光里程計時刻之前的imu數(shù)據(jù)double lastImuQT = -1;//注意，這里是要“刪除”當前幀“之前”的imu數(shù)據(jù)，是想根據(jù)當前幀“之后”的累積遞推。//而前面imuIntegratorOpt_做的事情是，“提取”當前幀“之前”的imu數(shù)據(jù)，用兩幀之間的imu數(shù)據(jù)進行積分。處理過的就彈出來。//因此，新到一幀激光幀里程計數(shù)據(jù)時，imuQueOpt隊列變化如下：//當前幀之前的數(shù)據(jù)被提出來做積分，用一個刪一個（這樣下一幀到達后，隊列中就不會有現(xiàn)在這幀之前的數(shù)據(jù)了）//那么在更新完以后，imuQueOpt隊列不再變化，剩下的原始imu數(shù)據(jù)用作下一次優(yōu)化時的數(shù)據(jù)。//而imuQueImu隊列則是把當前幀之前的imu數(shù)據(jù)都給直接剔除掉，僅保留當前幀之后的imu數(shù)據(jù)，//用作兩幀lidar里程計到達時刻之間發(fā)布的imu增量式里程計的預(yù)測。//imuQueImu和imuQueOpt的區(qū)別要明確,imuIntegratorImu_和imuIntegratorOpt_的區(qū)別也要明確,見imuhandler中的注釋while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t){lastImuQT = ROS_TIME(&imuQueImu.front());imuQueImu.pop_front();}// repropogate// 對剩余的imu數(shù)據(jù)計算預(yù)積分if (!imuQueImu.empty()){// reset bias use the newly optimized bias// 傳入狀態(tài),重置預(yù)積分器和最新的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);// integrate imu message from the beginning of this optimization// 計算預(yù)積分//利用imuQueImu中的數(shù)據(jù)進行預(yù)積分主要區(qū)別舊在于上一行的更新了biasfor (int i = 0; i < (int)imuQueImu.size(); ++i){sensor_msgs::Imu *thisImu = &imuQueImu[i];double imuTime = ROS_TIME(thisImu);double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);// 注意:加入的是這個用于傳播的的預(yù)積分器imuIntegratorImu_,(之前用來計算的是imuIntegratorOpt_,）//注意加入了上一步算出的dt//結(jié)果被存放在imuIntegratorImu_中imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);lastImuQT = imuTime;}}++key;//設(shè)置成True，用來通知另一個負責(zé)發(fā)布imu里程計的回調(diào)函數(shù)imuHandler“可以發(fā)布了”doneFirstOpt = true;}/*** imu因子圖優(yōu)化結(jié)果，速度或者偏置過大，認為失敗*/bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur){Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());if (vel.norm() > 30){ROS_WARN("Large velocity, reset IMU-preintegration!");return true;}Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());if (ba.norm() > 1.0 || bg.norm() > 1.0){ROS_WARN("Large bias, reset IMU-preintegration!");return true;}return false;}/*** 訂閱imu原始數(shù)據(jù)* 1、用上一幀激光里程計時刻對應(yīng)的狀態(tài)、偏置，* 施加從該時刻開始到當前時刻的imu預(yù)計分量，得到當前時刻的狀態(tài)，也就是imu里程計* 2、imu里程計位姿轉(zhuǎn)到lidar系，發(fā)布里程計*/void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw){std::lock_guard<std::mutex> lock(mtx);// imu原始測量數(shù)據(jù)轉(zhuǎn)換到lidar系，加速度、角速度、RPYsensor_msgs::Imu thisImu = imuConverter(*imu_raw);// 添加當前幀imu數(shù)據(jù)到隊列// 兩個雙端隊列分別裝著優(yōu)化前后的imu數(shù)據(jù)imuQueOpt.push_back(thisImu);imuQueImu.push_back(thisImu);// 要求上一次imu因子圖優(yōu)化執(zhí)行成功，確保更新了上一幀（激光里程計幀）的狀態(tài)、偏置，預(yù)積分已經(jīng)被重新計算// 這里需要先在odomhandler中優(yōu)化一次后再進行該函數(shù)后續(xù)的工作if (doneFirstOpt == false)return;double imuTime = ROS_TIME(&thisImu);//lastImuT_imu變量初始被賦值為-1// 獲得時間間隔, 第一次為1/500,之后是兩次imuTime間的差double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);lastImuT_imu = imuTime;// integrate this single imu message// imu預(yù)積分器添加一幀imu數(shù)據(jù)，注：這個預(yù)積分器的起始時刻是上一幀激光里程計時刻imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);// predict odometry// 用上一幀激光里程計時刻對應(yīng)的狀態(tài)、偏置，施加從該時刻開始到當前時刻的imu預(yù)計分量，得到當前時刻的狀態(tài)gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);// publish odometry// 發(fā)布imu里程計（轉(zhuǎn)到lidar系，與激光里程計同一個系）nav_msgs::Odometry odometry;odometry.header.stamp = thisImu.header.stamp;odometry.header.frame_id = odometryFrame; //"odom"odometry.child_frame_id = "odom_imu";// transform imu pose to ldiar//預(yù)測值currentState獲得imu位姿, 再由imu到雷達變換, 獲得雷達位姿gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);// 這里留疑問，本cpp讀完后補充：// 為什么currentState獲得的是imu的位姿？原始imu數(shù)據(jù)難道不是先轉(zhuǎn)換成雷達坐標系下的數(shù)據(jù)（this imu）才再送到imu預(yù)積分器中嗎？//答：在之前的優(yōu)化函數(shù)odometryHandler中，thisIMU是直接從imuQueOpt中取值.//而imuQueOpt中的內(nèi)容，是已經(jīng)從imu原始測量數(shù)據(jù)轉(zhuǎn)換到了lidar"中間系"系下（在本函數(shù)第二行）。離真正的雷達系還差了一個平移//odometryHandler函數(shù)中根據(jù)prevPose_ = lidarPose.compose(lidar2Imu)得到激光幀先驗位姿(lidarPose)轉(zhuǎn)換到imu系下,（相當于從真正的雷達系扭到上面的中間系中）//作為初值構(gòu)建了因子進行優(yōu)化；//在其imuIntegratorOpt_->integrateMeasurement中得到的應(yīng)該是dt之間的預(yù)積分量，//由于其處在循環(huán)中，因此是在遞推累積計算兩幀之間的預(yù)積分量。//（相當于是每到一幀，就把二者之間的預(yù)積分量計算一遍，并且優(yōu)化一遍，存儲進imuIntegratorOpt_）中。//因為本函數(shù)為imu回調(diào)函數(shù)，每收到一個imu數(shù)據(jù)，當之前因子圖算完的情況下，//在imuIntegratorImu_的基礎(chǔ)上繼續(xù)遞推新收到的imu數(shù)據(jù)，并且進行預(yù)測。//最后把imu再轉(zhuǎn)回lidar系下進行發(fā)布。//注意：這里發(fā)布的是兩幀之間的“增量”imu里程計信息，//imuIntegratorImu_本身是個積分器，只有兩幀之間的預(yù)積分，但是發(fā)布的時候發(fā)布的實際是結(jié)合了前述里程計本身有的位姿//如這個predict里的prevStateOdom://currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);//關(guān)于imuIntegratorImu_在兩個回調(diào)函數(shù)中都進行integrateMeasurement操作，之間是否會有沖突呢？//我覺得關(guān)鍵在于odometryHandler中有一句：imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom)，//在imuIntegratorOpt_優(yōu)化完兩幀imu數(shù)據(jù)之后，imuIntegratorImu_直接把積分和bias給reset掉，//然后開始根據(jù)imuIntegratorOpt_優(yōu)化出的bias來更新imuIntegratorImu_。//imuIntegratorImu_和imuIntegratorOpt_的區(qū)別在于，opt存儲的是新到一幀，和上一幀之間的預(yù)積分量，作為約束，執(zhí)行優(yōu)化。//優(yōu)化后，imuIntegratorImu_利用新的bias，在新到的這一幀的基礎(chǔ)上，遞推“之后”的預(yù)積分量。//（絕對不能把imuIntegratorOpt_和imuIntegratorImu_理解為同一批imu數(shù)據(jù)在優(yōu)化前后的不同值）//在更新的過程中不用擔(dān)心會被imuHandler中的imuIntegratorImu_->integrateMeasurement給頂?shù)?#xff0c;//這是因為imuHandler要根據(jù)doneFirstOpt來檢查odometryHandler是不是已經(jīng)更新完bias了。//因為更新并不是實時的，而是一幀激光數(shù)據(jù)到了才更新。//可是下一幀激光并沒有到，但是在此期間imu增量式里程計還是得照常發(fā)布，//如果當前幀已經(jīng)更新完bias了，然后就可以直接利用這個bias計算后面新到的ImuIntegratorImu_，odometry.pose.pose.position.x = lidarPose.translation().x();odometry.pose.pose.position.y = lidarPose.translation().y();odometry.pose.pose.position.z = lidarPose.translation().z();odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();odometry.twist.twist.linear.x = currentState.velocity().x();odometry.twist.twist.linear.y = currentState.velocity().y();odometry.twist.twist.linear.z = currentState.velocity().z();odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();pubImuOdometry.publish(odometry);} }; //還有兩個問題： //1.第一個是，為什么imu原始數(shù)據(jù)先要根據(jù)imuConverter變到lidar系， //那么之后imuintegrator->integrateMeasurement算到的預(yù)積分數(shù)據(jù)不就是lidar系下的嗎？ //在處理的時候又是把lidar里程計的坐標系根據(jù)compose函數(shù)變到imu系？這難道不是不對應(yīng)了嗎://2.變量gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), //gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); //這里為什么不用配置文件中的extRot，extQRPY之類的內(nèi)容呢，而只是用了extTrans數(shù)據(jù)？//關(guān)于這點，我在github中找到了解釋:https://github.com/TixiaoShan/LIO-SAM/issues/30, //imuConverter() to align the axis of the two coordinates,并沒有涉及到平移, //lidar2Imu or imu2Lidar 卻只有平移的內(nèi)容 //因此收到imu后，先用imuConverter()轉(zhuǎn)換到雷達系下，（但其實和雷達之間仍然差了一個平移）， //因此又把雷達的內(nèi)容用只含有平移的lidar2Imu 和原本差了一個平移的imu數(shù)據(jù)真正對齊 //（相當于是imu旋轉(zhuǎn)到雷達系下以后不平移，然后把雷達倒著平移過來,在一個“中間系”對齊）。 //在算完以后，等發(fā)布的時候，又用imu2Lidar又倒回到了正兒八經(jīng)的雷達系。//那么tixiaoshan為什么在默認里把平移參數(shù)設(shè)置為0，0，0？ //他在github中的解釋為: 我在不同的數(shù)據(jù)集中改變了幾次IMU的安裝位置。但是位置總是靠近激光雷達。 //所以每次測試不同的數(shù)據(jù)集時，我都不必費心去修改這個參數(shù)。嚴格地說，我的方法并不理想。需要提供此參數(shù)以獲得更好的性能。 int main(int argc, char** argv) {ros::init(argc, argv, "roboat_loam");IMUPreintegration ImuP;TransformFusion TF;ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");ros::MultiThreadedSpinner spinner(4);spinner.spin();return 0; }

總結(jié)

這個cpp文件主要有兩個類，一個叫IMUPreintegration類，一個叫TransformFusion類。

現(xiàn)在我們分開講，先說IMUPreintegration類。

關(guān)于IMU原始數(shù)據(jù)，送入imuhandle中：

1.1 imuhandle：

1.1.1 imu原始數(shù)據(jù)，會先被坐標系轉(zhuǎn)換，通過調(diào)用頭文件里的imuConverter函數(shù)，轉(zhuǎn)換到一個“雷達中間系”中，其實和真正的雷達系差了一個平移。

1.1.2 轉(zhuǎn)換后，會存入兩個隊列，一個imuQueOpt隊列，一個imuQueImu隊列。這兩隊列有什么區(qū)別和聯(lián)系呢？這個主要在另一個回調(diào)函數(shù)odometryHandler會被處理，在那個地方我會講。這里我們可以先理解為，imuQueImu是真正我們要用的數(shù)據(jù)，imuQueOpt是一個中間緩存的數(shù)據(jù)結(jié)構(gòu)，用來優(yōu)化imu的bias之類的東西。

1.1.3 在標志位doneFirstOpt為True的時候（注意這個標志位，這是一個很重要的變量，之后會再提到），每到一個imu數(shù)據(jù)，就用imuIntegratorImu_這個預(yù)積分器，把這一幀imu數(shù)據(jù)累積進去，然后預(yù)測當前時刻的狀態(tài)：currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系統(tǒng)所保持的狀態(tài)。

1.1.4 把currentState，通過imu2Lidar，從“中間系”給平移到真正的雷達系，然后發(fā)布出去。發(fā)布的話題就叫odometry/imu_incremental，這也就是imageProjection.cpp的總結(jié)部分的第2點部分提到的“imu”里程計。

1.2. odomHandle：這部分訂閱的是/mapping/odometry_incremental，這個話題是由mapOptmization.cpp發(fā)布的，可以把它理解為激光里程計。同理，也不要被incremental誤導(dǎo)，覺得好像是兩幀激光之間的變換，可不是這樣的啊。它和imu里程計性質(zhì)類似，就是相對世界坐標系的位姿。

1.2.1 初始化系統(tǒng)：

從imuQueOpt隊列里，刪掉當前這幀激光里程計時間上之前的數(shù)據(jù)，然后把雷達的pose變換到“中間系”，保存為prevPose。圖優(yōu)化部分，加入亂七八糟各種東西，priorVel，priorBias，把兩個預(yù)積分器imuIntegratorImu_，imuIntegratorOpt_給reset一下。（之后簡稱imu積分器和opt積分器）

這兩個預(yù)積分器opt積分器和imu積分器有什么區(qū)別呢？馬上就要講，在1.2.3部分。

1.2.2 清理緩存：

100幀激光里程計數(shù)據(jù)了，就把優(yōu)化器給reset一下（用前面的先驗協(xié)方差給reset一下）。注意，1.2.1和1.2.2的主要區(qū)別在于，1.2.1中的亂七八糟協(xié)方差數(shù)據(jù)，是用構(gòu)造函數(shù)中寫入的一大堆（我認為是經(jīng)驗值），而1.2.2這里的協(xié)方差，用的是optimizer.marginalCovariance這個輪子算出來的先驗協(xié)方差。

1.2.3 正式處理：

假設(shè)數(shù)據(jù)如下分布：

之前imu數(shù)據(jù) ——————第一幀開始——————第二幀開始————之后imu數(shù)據(jù)

1.2.3.1 把“第一幀開始”——“第二幀開始”這個之間的imu數(shù)據(jù)拿出來，送入opt積分器。這樣得到二者之間的預(yù)積分，構(gòu)建imu因子。

1.2.3.2 然后把Xkey-1 到Xkey之間，加入這個imu因子以及激光里程計提供的pose因子，整體做一個優(yōu)化。優(yōu)化的結(jié)果就是bias，以及“第二幀開始”這個時刻的系統(tǒng)位姿。

1.2.3.3 把優(yōu)化的結(jié)果（主要是bias），重置opt積分器和imu積分器。然后把當前幀（上圖的“第二幀開始”）之前的數(shù)據(jù)給刪掉，用imu積分器，從“第二幀開始”這里開始往后積分。（我們需要明確一點，在這個處理過程中，imu隊列也在持續(xù)的進數(shù)據(jù)，（即1.1的imuhandle中）），這里處理完，那么就置donefirst=True，這樣1.1.3部分，就可以無縫銜接接著在這里的基礎(chǔ)上對imu積分器繼續(xù)積分。（可以看出，這點處理，Tixiaoshan還是做的比較牛的）

回顧：在1.1.3部分，發(fā)布imu里程計，在這里我們可以的出結(jié)果，它并非是純粹的imu里程計，因為時不時是要加入激光里程計的信息做因子來優(yōu)化得到imu的bias等信息的。

接下來我們開始講TransformFusion類。

2.1 lidarOdometryHandler：

這部分監(jiān)聽的是/mapping/odometry，（也就是激光雷達里程計）這個回調(diào)函數(shù)比較特殊，它并沒有把雷達里程計的東西再像別的回調(diào)函數(shù)一樣，時刻存到什么隊列里去。而是就保存當前的雷達里程計的數(shù)據(jù)到lidarOdomAffine里面，把時間戳存到lidarOdomTime里面去。

注意，這里/mapping/odometry和/mapping/odometry_incremental有什么區(qū)別呢？我認為本質(zhì)上區(qū)別不大，前者是激光里程計部分直接優(yōu)化得到的激光位姿，后者相當于激光的位姿經(jīng)過旋轉(zhuǎn)約束和z軸約束的限制以后，又和原始imu信息里面的角度做了一個加權(quán)以后的位姿。

2.2 imuOdometryHandler

這部分監(jiān)聽的是/imu/odometry_incremental（也就是上面我一直在說的imu里程計），把imu里程計放到imuodomQueue里面保存，然后把lidarOdomTime之前的數(shù)據(jù)扔掉，用imu里程計的兩時刻差異，再加上lidarOdomAffine的基礎(chǔ)進行發(fā)布。

實際上，/imu/odometry_incremental本身就是雷達里程計基礎(chǔ)上imu數(shù)據(jù)上的發(fā)布，而在現(xiàn)在這里，也是雷達里程計在“imu里程計”上的一個“再次精修”。最后會發(fā)布兩個內(nèi)容，一個是odometry/imu，但是這個實際上是無人監(jiān)聽的，我覺得作者主要是發(fā)布tf變換，順手給它發(fā)布了。當然我覺得用戶可以監(jiān)聽這個數(shù)據(jù)，我覺得這個應(yīng)該是頻率上最高的一個里程計數(shù)據(jù)了。

另外還會發(fā)布一個path，用來rviz顯示，名字叫l(wèi)io-sam/imu/path。

mapOptimization.cpp

這個cpp的內(nèi)容和前三個cpp的內(nèi)容加起來行數(shù)差不多，比較復(fù)雜。也是最容易混亂的一個cpp文件，主要是里面有太多的變量，和相近的命名。我用A3紙記錄流程，足足寫了4頁，說實話寫到這里的時候我已經(jīng)不想往上打了，想干脆把A3紙掃描一下傳上來。但是我的字跡寫的實在是太潦草了，所以這里還是硬著頭皮敲吧。因為不方便畫箭頭，所以還是得自己去編號。

老規(guī)矩，先放代碼注釋：

注釋

#include "utility.h" #include "lio_sam/cloud_info.h" #include "lio_sam/save_map.h"#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Pose3.h> #include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/BetweenFactor.h> #include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Values.h> #include <gtsam/inference/Symbol.h>#include <gtsam/nonlinear/ISAM2.h>using namespace gtsam;using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::G; // GPS pose/** A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)*//*** 6D位姿點云結(jié)構(gòu)定義 */ struct PointXYZIRPYT {PCL_ADD_POINT4DPCL_ADD_INTENSITY; // preferred way of adding a XYZ+paddingfloat roll;float pitch;float yaw;double time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignmentPOINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,(float, x, x) (float, y, y)(float, z, z) (float, intensity, intensity)(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)(double, time, time))typedef PointXYZIRPYT PointTypePose;class mapOptimization : public ParamServer {public:// gtsamNonlinearFactorGraph gtSAMgraph;Values initialEstimate;Values optimizedEstimate;ISAM2 *isam;Values isamCurrentEstimate;Eigen::MatrixXd poseCovariance;ros::Publisher pubLaserCloudSurround;ros::Publisher pubLaserOdometryGlobal;ros::Publisher pubLaserOdometryIncremental;ros::Publisher pubKeyPoses;ros::Publisher pubPath;ros::Publisher pubHistoryKeyFrames;ros::Publisher pubIcpKeyFrames;ros::Publisher pubRecentKeyFrames;ros::Publisher pubRecentKeyFrame;ros::Publisher pubCloudRegisteredRaw;ros::Publisher pubLoopConstraintEdge;ros::Subscriber subCloud;ros::Subscriber subGPS;ros::Subscriber subLoop;ros::ServiceServer srvSaveMap;std::deque<nav_msgs::Odometry> gpsQueue;lio_sam::cloud_info cloudInfo;// 歷史所有關(guān)鍵幀的角點集合（降采樣）vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;// 歷史所有關(guān)鍵幀的平面點集合（降采樣）vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;// 歷史關(guān)鍵幀位姿（位置）pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;// 歷史關(guān)鍵幀位姿pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;// 當前激光幀角點集合pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization// 當前激光幀平面點集合pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization// 當前激光幀角點集合，降采樣，DS: DownSizepcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization// 當前激光幀平面點集合，降采樣pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization// 當前幀與局部map匹配上了的角點、平面點，加入同一集合；后面是對應(yīng)點的參數(shù)pcl::PointCloud<PointType>::Ptr laserCloudOri;pcl::PointCloud<PointType>::Ptr coeffSel;// 當前幀與局部map匹配上了的角點、參數(shù)、標記std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computationstd::vector<PointType> coeffSelCornerVec;std::vector<bool> laserCloudOriCornerFlag;// 當前幀與局部map匹配上了的平面點、參數(shù)、標記std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computationstd::vector<PointType> coeffSelSurfVec;std::vector<bool> laserCloudOriSurfFlag;map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;// 局部map的角點集合pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;// 局部map的平面點集合pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;// 局部map的角點集合，降采樣pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;// 局部map的平面點集合，降采樣pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;// 局部關(guān)鍵幀構(gòu)建的map點云，對應(yīng)kdtree，用于scan-to-map找相鄰點pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;// 降采樣pcl::VoxelGrid<PointType> downSizeFilterCorner;pcl::VoxelGrid<PointType> downSizeFilterSurf;pcl::VoxelGrid<PointType> downSizeFilterICP;pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimizationros::Time timeLaserInfoStamp;double timeLaserInfoCur;float transformTobeMapped[6];std::mutex mtx;std::mutex mtxLoopInfo;bool isDegenerate = false;cv::Mat matP;// 局部map角點數(shù)量int laserCloudCornerFromMapDSNum = 0;// 局部map平面點數(shù)量int laserCloudSurfFromMapDSNum = 0;// 當前激光幀角點數(shù)量int laserCloudCornerLastDSNum = 0;// 當前激光幀面點數(shù)量int laserCloudSurfLastDSNum = 0;bool aLoopIsClosed = false;map<int, int> loopIndexContainer; // from new to oldvector<pair<int, int>> loopIndexQueue;vector<gtsam::Pose3> loopPoseQueue;vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;deque<std_msgs::Float64MultiArray> loopInfoVec;nav_msgs::Path globalPath;// 當前幀位姿Eigen::Affine3f transPointAssociateToMap;// 前一幀位姿Eigen::Affine3f incrementalOdometryAffineFront;// 當前幀位姿Eigen::Affine3f incrementalOdometryAffineBack;mapOptimization(){ISAM2Params parameters;parameters.relinearizeThreshold = 0.1;parameters.relinearizeSkip = 1;isam = new ISAM2(parameters);// 發(fā)布歷史關(guān)鍵幀里程計pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);// 發(fā)布局部關(guān)鍵幀map的特征點云pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);// 發(fā)布激光里程計，rviz中表現(xiàn)為坐標軸pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);// 發(fā)布激光里程計，它與上面的激光里程計基本一樣，只是roll、pitch用imu數(shù)據(jù)加權(quán)平均了一下，z做了限制pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);// 發(fā)布激光里程計路徑，rviz中表現(xiàn)為載體的運行軌跡pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);// 訂閱當前激光幀點云信息，來自featureExtractionsubCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());// 訂閱GPS里程計subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());// 訂閱來自外部閉環(huán)檢測程序提供的閉環(huán)數(shù)據(jù)，本程序沒有提供，這里實際沒用上subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());// 發(fā)布地圖保存服務(wù)srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);// 發(fā)布閉環(huán)匹配關(guān)鍵幀局部mappubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);// 發(fā)布當前關(guān)鍵幀經(jīng)過閉環(huán)優(yōu)化后的位姿變換之后的特征點云pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);// 發(fā)布閉環(huán)邊，rviz中表現(xiàn)為閉環(huán)幀之間的連線pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);// 發(fā)布局部map的降采樣平面點集合pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);// 發(fā)布歷史幀（累加的）的角點、平面點降采樣集合pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);// 發(fā)布當前幀原始點云配準之后的點云pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimizationallocateMemory();}void allocateMemory(){cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimizationlaserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimizationlaserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimizationlaserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimizationlaserCloudOri.reset(new pcl::PointCloud<PointType>());coeffSel.reset(new pcl::PointCloud<PointType>());laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());for (int i = 0; i < 6; ++i){transformTobeMapped[i] = 0;}matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));}/*** 訂閱當前激光幀點云信息，來自featureExtraction* 1、當前幀位姿初始化* 1) 如果是第一幀，用原始imu數(shù)據(jù)的RPY初始化當前幀位姿（旋轉(zhuǎn)部分）* 2) 后續(xù)幀，用imu里程計計算兩幀之間的增量位姿變換，作用于前一幀的激光位姿，得到當前幀激光位姿* 2、提取局部角點、平面點云集合，加入局部map* 1) 對最近的一幀關(guān)鍵幀，搜索時空維度上相鄰的關(guān)鍵幀集合，降采樣一下* 2) 對關(guān)鍵幀集合中的每一幀，提取對應(yīng)的角點、平面點，加入局部map中* 3、當前激光幀角點、平面點集合降采樣* 4、scan-to-map優(yōu)化當前幀位姿* (1) 要求當前幀特征點數(shù)量足夠多，且匹配的點數(shù)夠多，才執(zhí)行優(yōu)化* (2) 迭代30次（上限）優(yōu)化* 1) 當前激光幀角點尋找局部map匹配點* a.更新當前幀位姿，將當前幀角點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成直線（用距離中心點的協(xié)方差矩陣，特征值進行判斷），則認為匹配上了* b.計算當前幀角點到直線的距離、垂線的單位向量，存儲為角點參數(shù)* 2) 當前激光幀平面點尋找局部map匹配點* a.更新當前幀位姿，將當前幀平面點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成平面（最小二乘擬合平面），則認為匹配上了* b.計算當前幀平面點到平面的距離、垂線的單位向量，存儲為平面點參數(shù)* 3) 提取當前幀中與局部map匹配上了的角點、平面點，加入同一集合* 4) 對匹配特征點計算Jacobian矩陣，觀測值為特征點到直線、平面的距離，構(gòu)建高斯牛頓方程，迭代優(yōu)化當前位姿，存transformTobeMapped* (3)用imu原始RPY數(shù)據(jù)與scan-to-map優(yōu)化后的位姿進行加權(quán)融合，更新當前幀位姿的roll、pitch，約束z坐標* 5、設(shè)置當前幀為關(guān)鍵幀并執(zhí)行因子圖優(yōu)化* 1) 計算當前幀與前一幀位姿變換，如果變化太小，不設(shè)為關(guān)鍵幀，反之設(shè)為關(guān)鍵幀* 2) 添加激光里程計因子、GPS因子、閉環(huán)因子* 3) 執(zhí)行因子圖優(yōu)化* 4) 得到當前幀優(yōu)化后位姿，位姿協(xié)方差* 5) 添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加當前關(guān)鍵幀的角點、平面點集合* 6、更新因子圖中所有變量節(jié)點的位姿，也就是所有歷史關(guān)鍵幀的位姿，更新里程計軌跡* 7、發(fā)布激光里程計* 8、發(fā)布里程計、點云、軌跡*/void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){// extract time stamp// 當前激光幀時間戳timeLaserInfoStamp = msgIn->header.stamp;timeLaserInfoCur = msgIn->header.stamp.toSec();// extract info and feature cloud// 提取當前激光幀角點、平面點集合cloudInfo = *msgIn;pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);std::lock_guard<std::mutex> lock(mtx);// mapping執(zhí)行頻率控制static double timeLastProcessing = -1;if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval){timeLastProcessing = timeLaserInfoCur;// 當前幀位姿初始化// 1、如果是第一幀，用原始imu數(shù)據(jù)的RPY初始化當前幀位姿（旋轉(zhuǎn)部分）// 2、后續(xù)幀，用imu里程計計算兩幀之間的增量位姿變換，作用于前一幀的激光位姿，得到當前幀激光初始位姿updateInitialGuess();// 提取局部角點、平面點云集合，加入局部map// 1、對最近的一幀關(guān)鍵幀，搜索時空維度上相鄰的關(guān)鍵幀集合，降采樣一下// 2、對關(guān)鍵幀集合中的每一幀，提取對應(yīng)的角點、平面點，加入局部map中extractSurroundingKeyFrames();// 當前激光幀角點、平面點集合降采樣downsampleCurrentScan();// scan-to-map優(yōu)化當前幀位姿// 1、要求當前幀特征點數(shù)量足夠多，且匹配的點數(shù)夠多，才執(zhí)行優(yōu)化// 2、迭代30次（上限）優(yōu)化// 1) 當前激光幀角點尋找局部map匹配點// a.更新當前幀位姿，將當前幀角點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成直線（用距離中心點的協(xié)方差矩陣，特征值進行判斷），則認為匹配上了// b.計算當前幀角點到直線的距離、垂線的單位向量，存儲為角點參數(shù)// 2) 當前激光幀平面點尋找局部map匹配點// a.更新當前幀位姿，將當前幀平面點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成平面（最小二乘擬合平面），則認為匹配上了// b.計算當前幀平面點到平面的距離、垂線的單位向量，存儲為平面點參數(shù)// 3) 提取當前幀中與局部map匹配上了的角點、平面點，加入同一集合// 4) 對匹配特征點計算Jacobian矩陣，觀測值為特征點到直線、平面的距離，構(gòu)建高斯牛頓方程，迭代優(yōu)化當前位姿，存transformTobeMapped// 3、用imu原始RPY數(shù)據(jù)與scan-to-map優(yōu)化后的位姿進行加權(quán)融合，更新當前幀位姿的roll、pitch，約束z坐標scan2MapOptimization();// 設(shè)置當前幀為關(guān)鍵幀并執(zhí)行因子圖優(yōu)化// 1、計算當前幀與前一幀位姿變換，如果變化太小，不設(shè)為關(guān)鍵幀，反之設(shè)為關(guān)鍵幀// 2、添加激光里程計因子、GPS因子、閉環(huán)因子// 3、執(zhí)行因子圖優(yōu)化// 4、得到當前幀優(yōu)化后位姿，位姿協(xié)方差// 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加當前關(guān)鍵幀的角點、平面點集合saveKeyFramesAndFactor();// 更新因子圖中所有變量節(jié)點的位姿，也就是所有歷史關(guān)鍵幀的位姿，更新里程計軌跡correctPoses();// 發(fā)布激光里程計publishOdometry();// 發(fā)布里程計、點云、軌跡// 1、發(fā)布歷史關(guān)鍵幀位姿集合// 2、發(fā)布局部map的降采樣平面點集合// 3、發(fā)布歷史幀（累加的）的角點、平面點降采樣集合// 4、發(fā)布里程計軌跡publishFrames();}}/*** 當前幀位姿初始化* 1、如果是第一幀，用原始imu數(shù)據(jù)的RPY初始化當前幀位姿（旋轉(zhuǎn)部分）* 2、后續(xù)幀，用imu里程計計算兩幀之間的增量位姿變換，作用于前一幀的激光位姿，得到當前幀激光位姿*/void updateInitialGuess(){// save current transformation before any processing// 前一幀的位姿，注：這里指lidar的位姿，后面都簡寫成位姿incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);// 前一幀的初始化姿態(tài)角（來自原始imu數(shù)據(jù)），用于估計第一幀的位姿（旋轉(zhuǎn)部分）static Eigen::Affine3f lastImuTransformation;// initialization// 如果關(guān)鍵幀集合為空，繼續(xù)進行初始化if (cloudKeyPoses3D->points.empty()){// 當前幀位姿的旋轉(zhuǎn)部分，用激光幀信息中的RPY（來自imu原始數(shù)據(jù)）初始化transformTobeMapped[0] = cloudInfo.imuRollInit;transformTobeMapped[1] = cloudInfo.imuPitchInit;transformTobeMapped[2] = cloudInfo.imuYawInit;if (!useImuHeadingInitialization)transformTobeMapped[2] = 0;lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}// use imu pre-integration estimation for pose guess// 用當前幀和前一幀對應(yīng)的imu里程計計算相對位姿變換，再用前一幀的位姿與相對變換，計算當前幀的位姿，存transformTobeMappedstatic bool lastImuPreTransAvailable = false;static Eigen::Affine3f lastImuPreTransformation;//odomAvailable和imuAvailable均來源于imageProjection.cpp中賦值，//imuAvailable是遍歷激光幀前后起止時刻0.01s之內(nèi)的imu數(shù)據(jù)，//如果都沒有那就是false，因為imu頻率一般比激光幀快，因此這里應(yīng)該是都有的。//odomAvailable同理，是監(jiān)聽imu里程計的位姿，如果沒有緊挨著激光幀的imu里程計數(shù)據(jù)，那么就是false；//這倆應(yīng)該一般都有if (cloudInfo.odomAvailable == true){// cloudInfo來自featureExtraction.cpp發(fā)布的lio_sam/feature/cloud_info,//而其中的initialGuessX等信息本質(zhì)上來源于ImageProjection.cpp發(fā)布的deskew/cloud_info信息，//而deskew/cloud_info中的initialGuessX則來源于ImageProjection.cpp中的回調(diào)函數(shù)odometryHandler，//odometryHandler訂閱的是imuPreintegration.cpp發(fā)布的odometry/imu_incremental話題，//該話題發(fā)布的xyz是imu在前一幀雷達基礎(chǔ)上的增量位姿//糾正一個觀點：增量位姿，指的絕不是預(yù)積分位姿！！是在前一幀雷達的基礎(chǔ)上(包括該基礎(chǔ)!!)的（基礎(chǔ)不是0）的位姿//當前幀的初始估計位姿（來自imu里程計），后面用來計算增量位姿變換Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ, cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);if (lastImuPreTransAvailable == false){// 賦值給前一幀//lastImuPreTransAvailable是一個靜態(tài)變量，初始被設(shè)置為false,之后就變成了true//也就是說這段只調(diào)用一次，就是初始時，把imu位姿賦值給lastImuPreTransformationlastImuPreTransformation = transBack;lastImuPreTransAvailable = true;} else {// 當前幀相對于前一幀的位姿變換，imu里程計計算得到//lastImuPreTransformation就是上一幀激光時刻的imu位姿,transBack是這一幀時刻的imu位姿//求完逆相乘以后才是增量，絕不可把imu_incremental發(fā)布的當成是兩激光間的增量Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;// 前一幀的位姿Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);// 當前幀的位姿Eigen::Affine3f transFinal = transTobe * transIncre;//將transFinal傳入，結(jié)果輸出至transformTobeMapped中pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);// 當前幀初始位姿賦值作為前一幀lastImuPreTransformation = transBack;lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}}// use imu incremental estimation for pose guess (only rotation)// 只在第一幀調(diào)用（注意上面的return），用imu數(shù)據(jù)初始化當前幀位姿，僅初始化旋轉(zhuǎn)部分if (cloudInfo.imuAvailable == true){//注：這一時刻的transBack和之前if (cloudInfo.odomAvailable == true)內(nèi)部的transBack不同，//之前獲得的是initialGuessRoll等，但是在這里是imuRollInit，它來源于imageProjection中的imuQueue，直接存儲原始imu數(shù)據(jù)的。//那么對于第一幀數(shù)據(jù)，目前的lastImuTransformation是initialGuessX等，即imu里程計的數(shù)據(jù)；//而transBack是imuRollInit是imu的瞬時原始數(shù)據(jù)roll、pitch和yaw三個角。//那么imuRollInit和initialGuessRoll這兩者有啥區(qū)別呢？//imuRollInit是imu姿態(tài)角，在imageProjection中一收到，就馬上賦值給它要發(fā)布的cloud_info，//而initialGuessRoll是imu里程計發(fā)布的姿態(tài)角。//直觀上來說，imu原始數(shù)據(jù)收到速度是應(yīng)該快于imu里程計的數(shù)據(jù)的，因此感覺二者之間應(yīng)該有一個增量，//那么lastImuTransformation.inverse() * transBack算出增量，增量再和原先的transformTobeMapped計算,//結(jié)果依舊以transformTobeMapped來保存//感覺這里寫的非常奇怪Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);Eigen::Affine3f transFinal = transTobe * transIncre;pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}}/*** 提取局部角點、平面點云集合，加入局部map* 1、對最近的一幀關(guān)鍵幀，搜索時空維度上相鄰的關(guān)鍵幀集合，降采樣一下* 2、對關(guān)鍵幀集合中的每一幀，提取對應(yīng)的角點、平面點，加入局部map中*/void extractSurroundingKeyFrames(){if (cloudKeyPoses3D->points.empty() == true)return; // if (loopClosureEnableFlag == true)// {// extractForLoopClosure(); // } else {// extractNearby();// }extractNearby();}void extractNearby(){pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;// extract all the nearby key poses and downsample them// kdtree的輸入，全局關(guān)鍵幀位姿集合（歷史所有關(guān)鍵幀集合）kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree//創(chuàng)建Kd樹然后搜索半徑在配置文件中//指定半徑范圍查找近鄰//球狀固定距離半徑近鄰搜索//surroundingKeyframeSearchRadius是搜索半徑，pointSearchInd應(yīng)該是返回的index，pointSearchSqDis應(yīng)該是依次距離中心點的距離kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);for (int i = 0; i < (int)pointSearchInd.size(); ++i){int id = pointSearchInd[i];//保存附近關(guān)鍵幀,加入相鄰關(guān)鍵幀位姿集合中surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);}//降采樣//把相鄰關(guān)鍵幀位姿集合，進行下采樣，濾波后存入surroundingKeyPosesDSdownSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);for(auto& pt : surroundingKeyPosesDS->points){//k近鄰搜索,找出最近的k個節(jié)點（這里是1）kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);//把強度替換掉，注意是從原始關(guān)鍵幀數(shù)據(jù)中替換的,相當于是把下采樣以后的點的強度，換成是原始點強度//注意，這里的intensity應(yīng)該不是強度，因為在函數(shù)saveKeyFramesAndFactor中://thisPose3D.intensity = cloudKeyPoses3D->size();//就是索引，只不過這里借用intensity結(jié)構(gòu)來存放pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;}// also extract some latest key frames in case the robot rotates in one position//提取了一些最新的關(guān)鍵幀，以防機器人在一個位置原地旋轉(zhuǎn) int numPoses = cloudKeyPoses3D->size();// 把10s內(nèi)的關(guān)鍵幀也加到surroundingKeyPosesDS中,注意是“也”，原先已經(jīng)裝了下采樣的位姿(位置)for (int i = numPoses-1; i >= 0; --i){if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);elsebreak;}//對降采樣后的點云進行提取出邊緣點和平面點對應(yīng)的localmap extractCloud(surroundingKeyPosesDS);}/*** 將相鄰關(guān)鍵幀集合對應(yīng)的角點、平面點，加入到局部map中，作為scan-to-map匹配的局部點云地圖*/void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract){// fuse the maplaserCloudCornerFromMap->clear();laserCloudSurfFromMap->clear(); // 遍歷當前幀（實際是取最近的一個關(guān)鍵幀來找它相鄰的關(guān)鍵幀集合）時空維度上相鄰的關(guān)鍵幀集合for (int i = 0; i < (int)cloudToExtract->size(); ++i){// 距離超過閾值，丟棄if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)continue;// 相鄰關(guān)鍵幀索引int thisKeyInd = (int)cloudToExtract->points[i].intensity;if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) {// transformed cloud available*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;} else {// transformed cloud not available// 相鄰關(guān)鍵幀對應(yīng)的角點、平面點云，通過6D位姿變換到世界坐標系下//transformPointCloud輸入的兩個形參，分別為點云和變換，返回變換位姿后的點pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);// 加入局部map*laserCloudCornerFromMap += laserCloudCornerTemp;*laserCloudSurfFromMap += laserCloudSurfTemp;laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);}}// Downsample the surrounding corner key frames (or map)// 降采樣局部角點mapdownSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();// Downsample the surrounding surf key frames (or map)// 降采樣局部平面點mapdownSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();// clear map cache if too large// 太大了，清空一下內(nèi)存if (laserCloudMapContainer.size() > 1000)laserCloudMapContainer.clear();}void downsampleCurrentScan(){// Downsample cloud from current scan//對當前幀點云降采樣剛剛完成了周圍關(guān)鍵幀的降采樣 //大量的降采樣工作無非是為了使點云稀疏化加快匹配以及實時性要求laserCloudCornerLastDS->clear();downSizeFilterCorner.setInputCloud(laserCloudCornerLast);downSizeFilterCorner.filter(*laserCloudCornerLastDS);laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();laserCloudSurfLastDS->clear();downSizeFilterSurf.setInputCloud(laserCloudSurfLast);downSizeFilterSurf.filter(*laserCloudSurfLastDS);laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();}void scan2MapOptimization(){//根據(jù)現(xiàn)有地圖與最新點云數(shù)據(jù)進行配準從而更新機器人精確位姿與融合建圖，//它分為角點優(yōu)化、平面點優(yōu)化、配準與更新等部分。// 優(yōu)化的過程與里程計的計算類似，是通過計算點到直線或平面的距離，構(gòu)建優(yōu)化公式再用LM法求解。if (cloudKeyPoses3D->points.empty())return;if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum){//構(gòu)建kdtreekdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);//迭代30次for (int iterCount = 0; iterCount < 30; iterCount++){laserCloudOri->clear();coeffSel->clear();//邊緣點匹配優(yōu)化cornerOptimization();//平面點匹配優(yōu)化surfOptimization();//組合優(yōu)化多項式系數(shù)combineOptimizationCoeffs();if (LMOptimization(iterCount) == true)break; }//使用了9軸imu的orientation與做transformTobeMapped插值，并且roll和pitch收到常量閾值約束（權(quán)重）transformUpdate();} else {ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);}}/*** 當前激光幀角點尋找局部map匹配點* 1、更新當前幀位姿，將當前幀角點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成直線（用距離中心點的協(xié)方差矩陣，特征值進行判斷），則認為匹配上了* 2、計算當前幀角點到直線的距離、垂線的單位向量，存儲為角點參數(shù)*/void cornerOptimization(){//實現(xiàn)transformTobeMapped的矩陣形式轉(zhuǎn)換下面調(diào)用的函數(shù)就一行就不展開了工具類函數(shù)// 把結(jié)果存入transPointAssociateToMap中updatePointAssociateToMap();#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < laserCloudCornerLastDSNum; i++){PointType pointOri, pointSel, coeff;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;pointOri = laserCloudCornerLastDS->points[i];//第i幀的點轉(zhuǎn)換到第一幀坐標系下//這里就調(diào)用了第一步中updatePointAssociateToMap中實現(xiàn)的transPointAssociateToMap，//然后利用這個變量，把pointOri的點轉(zhuǎn)換到pointSel下，pointSel作為輸出pointAssociateToMap(&pointOri, &pointSel);//kd樹的最近搜索kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));if (pointSearchSqDis[4] < 1.0) {float cx = 0, cy = 0, cz = 0;// 先求5個樣本的平均值for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;}cx /= 5; cy /= 5; cz /= 5;// 下面求矩陣matA1=[ax,ay,az]^t*[ax,ay,az]// 更準確地說應(yīng)該是在求協(xié)方差matA1float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;for (int j = 0; j < 5; j++) {float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;a11 += ax * ax; a12 += ax * ay; a13 += ax * az;a22 += ay * ay; a23 += ay * az;a33 += az * az;}a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;// 求正交陣的特征值和特征向量// 特征值：matD1，特征向量：matV1中對應(yīng)于LOAM論文里雷達建圖特征值與特征向量那塊cv::eigen(matA1, matD1, matV1);// 邊緣：與較大特征值相對應(yīng)的特征向量代表邊緣線的方向（一大兩小，大方向）// 以下這一大塊是在計算點到邊緣的距離，最后通過系數(shù)s來判斷是否距離很近// 如果距離很近就認為這個點在邊緣上，需要放到laserCloudOri中// 如果最大的特征值相比次大特征值，大很多，認為構(gòu)成了線，角點是合格的if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {// 當前幀角點坐標（map系下）float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;// 局部map對應(yīng)中心角點，沿著特征向量（直線方向）方向，前后各取一個點float x1 = cx + 0.1 * matV1.at<float>(0, 0);float y1 = cy + 0.1 * matV1.at<float>(0, 1);float z1 = cz + 0.1 * matV1.at<float>(0, 2);float x2 = cx - 0.1 * matV1.at<float>(0, 0);float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(0, 2);// 這邊是在求[(x0-x1),(y0-y1),(z0-z1)]與[(x0-x2),(y0-y2),(z0-z2)]叉乘得到的向量的模長// 這個模長是由0.2*V1[0]和點[x0,y0,z0]構(gòu)成的平行四邊形的面積// 因為[(x0-x1),(y0-y1),(z0-z1)]x[(x0-x2),(y0-y2),(z0-z2)]=[XXX,YYY,ZZZ],// [XXX,YYY,ZZZ]=[(y0-y1)(z0-z2)-(y0-y2)(z0-z1),-(x0-x1)(z0-z2)+(x0-x2)(z0-z1),(x0-x1)(y0-y2)-(x0-x2)(y0-y1)]// area_012，也就是三個點組成的三角形面積*2，叉積的模|axb|=a*b*sin(theta)float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));// line_12，底邊邊長float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));// 兩次叉積，得到點到直線的垂線段單位向量，x分量，下面同理// 求叉乘結(jié)果[la',lb',lc']=[(x1-x2),(y1-y2),(z1-z2)]x[XXX,YYY,ZZZ]// [la,lb,lc]=[la',lb',lc']/a012/l12// 得到底邊上的高的方向向量[la,lb,lc]// LLL=[la,lb,lc]是V1[0]這條高上的單位法向量。||LLL||=1；//如不理解則看圖：// A// B C// 這里ABxAC，代表垂直于ABC面的法向量，其模長為平行四邊形面積//因此BCx(ABxAC),代表了BC和（ABC平面的法向量）的叉乘，那么其實這個向量就是A到BC的垂線的方向向量//那么(ABxAC)/|ABxAC|,代表著ABC平面的單位法向量//BCxABC平面單位法向量，即為一個長度為|BC|的（A到BC垂線的方向向量），因此再除以|BC|，得到A到BC垂線的單位方向向量float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;// 三角形的高，也就是點到直線距離// 計算點pointSel到直線的距離// 這里需要特別說明的是ld2代表的是點pointSel到過點[cx,cy,cz]的方向向量直線的距離float ld2 = a012 / l12;// 下面涉及到一個魯棒核函數(shù)，作者簡單地設(shè)計了這個核函數(shù)。// 距離越大，s越小，是個距離懲罰因子（權(quán)重）float s = 1 - 0.9 * fabs(ld2);// coeff代表系數(shù)的意思// coff用于保存距離的方向向量coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;// intensity本質(zhì)上構(gòu)成了一個核函數(shù)，ld2越接近于1，增長越慢// intensity=(1-0.9*ld2)*ld2=ld2-0.9*ld2*ld2coeff.intensity = s * ld2;// 程序末尾根據(jù)s的值來判斷是否將點云點放入點云集合laserCloudOri以及coeffSel中。// 所以就應(yīng)該認為這個點是邊緣點// s>0.1 也就是要求點到直線的距離ld2要小于1m// s越大說明ld2越小(離邊緣線越近)，這樣就說明點pointOri在直線上if (s > 0.1) {laserCloudOriCornerVec[i] = pointOri;coeffSelCornerVec[i] = coeff;laserCloudOriCornerFlag[i] = true;}}}}}/*** 當前激光幀平面點尋找局部map匹配點* 1、更新當前幀位姿，將當前幀平面點坐標變換到map系下，在局部map中查找5個最近點，距離小于1m，且5個點構(gòu)成平面（最小二乘擬合平面），則認為匹配上了* 2、計算當前幀平面點到平面的距離、垂線的單位向量，存儲為平面點參數(shù)*/void surfOptimization(){updatePointAssociateToMap();// 遍歷當前幀平面點集合#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < laserCloudSurfLastDSNum; i++){PointType pointOri, pointSel, coeff;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;// 尋找5個緊鄰點, 計算其特征值和特征向量// 平面點（坐標還是lidar系）pointOri = laserCloudSurfLastDS->points[i];// 根據(jù)當前幀位姿，變換到世界坐標系（map系）下pointAssociateToMap(&pointOri, &pointSel); // 在局部平面點map中查找當前平面點相鄰的5個平面點kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);Eigen::Matrix<float, 5, 3> matA0;Eigen::Matrix<float, 5, 1> matB0;Eigen::Vector3f matX0;matA0.setZero(); // 5*3 存儲5個緊鄰點matB0.fill(-1);matX0.setZero();// 只考慮附近1.0m內(nèi)if (pointSearchSqDis[4] < 1.0) {for (int j = 0; j < 5; j++) {matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;}// 求maxA0中點構(gòu)成的平面法向量//matB0是-1，這個函數(shù)用來求解AX=B的X，//也就是AX+BY+CZ+1=0matX0 = matA0.colPivHouseholderQr().solve(matB0);// 假設(shè)平面方程為ax+by+cz+1=0，這里就是求方程的系數(shù)abc，d=1float pa = matX0(0, 0);float pb = matX0(1, 0);float pc = matX0(2, 0);float pd = 1;// 單位法向量// 對[pa,pb,pc,pd]進行單位化float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;// 檢查平面是否合格，如果5個點中有點到平面的距離超過0.2m，那么認為這些點太分散了，不構(gòu)成平面bool planeValid = true;for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}if (planeValid) {// 當前激光幀點到平面距離//點(x0,y0,z0)到了平面Ax+By+Cz+D=0的距離為：d=|Ax0+By0+Cz0+D|/√(A^2+B^2+C^2)//但是會發(fā)現(xiàn)下面的分母開了兩次方，不知道為什么，分母多開一次方會更小，這因此求出的距離會更大float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 距離越大，s越小，是個距離懲罰因子（權(quán)重）// 后面部分相除求的是[pa,pb,pc,pd]與pointSel的夾角余弦值(兩個sqrt，其實并不是余弦值)// 這個夾角余弦值越小越好，越小證明所求的[pa,pb,pc,pd]與平面越垂直float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));// 點到平面垂線單位法向量（其實等價于平面法向量）coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1) {// 當前激光幀平面點，加入匹配集合中.//如果s>0.1,代表fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z))這一項<1,即"偽距離"<1laserCloudOriSurfVec[i] = pointOri;coeffSelSurfVec[i] = coeff;laserCloudOriSurfFlag[i] = true;}}}}}/*** 提取當前幀中與局部map匹配上了的角點、平面點，加入同一集合*/void combineOptimizationCoeffs(){// combine corner coeffs// 遍歷當前幀角點集合，提取出與局部map匹配上了的角點for (int i = 0; i < laserCloudCornerLastDSNum; ++i){if (laserCloudOriCornerFlag[i] == true){laserCloudOri->push_back(laserCloudOriCornerVec[i]);coeffSel->push_back(coeffSelCornerVec[i]);}}// combine surf coeffs// 遍歷當前幀平面點集合，提取出與局部map匹配上了的平面點for (int i = 0; i < laserCloudSurfLastDSNum; ++i){if (laserCloudOriSurfFlag[i] == true){laserCloudOri->push_back(laserCloudOriSurfVec[i]);coeffSel->push_back(coeffSelSurfVec[i]);}}// reset flag for next iteration// 清空標記std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);}/*** scan-to-map優(yōu)化* 對匹配特征點計算Jacobian矩陣，觀測值為特征點到直線、平面的距離，構(gòu)建高斯牛頓方程，迭代優(yōu)化當前位姿，存transformTobeMapped* 公式推導(dǎo)：todo*/bool LMOptimization(int iterCount){//由于LOAM里雷達的特殊坐標系所以這里也轉(zhuǎn)了一次 // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation// lidar <- camera --- camera <- lidar// x = z --- x = y// y = x --- y = z// z = y --- z = x// roll = yaw --- roll = pitch// pitch = roll --- pitch = yaw// yaw = pitch --- yaw = roll// lidar -> camerafloat srx = sin(transformTobeMapped[1]);float crx = cos(transformTobeMapped[1]);float sry = sin(transformTobeMapped[2]);float cry = cos(transformTobeMapped[2]);float srz = sin(transformTobeMapped[0]);float crz = cos(transformTobeMapped[0]);// 當前幀匹配特征點數(shù)太少int laserCloudSelNum = laserCloudOri->size();if (laserCloudSelNum < 50) {return false;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));PointType pointOri, coeff;// 遍歷匹配特征點，構(gòu)建Jacobian矩陣for (int i = 0; i < laserCloudSelNum; i++) {// lidar -> camerapointOri.x = laserCloudOri->points[i].y;pointOri.y = laserCloudOri->points[i].z;pointOri.z = laserCloudOri->points[i].x;// lidar -> cameracoeff.x = coeffSel->points[i].y;coeff.y = coeffSel->points[i].z;coeff.z = coeffSel->points[i].x;coeff.intensity = coeffSel->points[i].intensity;// in camera//https://wykxwyc.github.io/2019/08/01/The-Math-Formula-in-LeGO-LOAM/// 求雅克比矩陣中的元素，距離d對roll角度的偏導(dǎo)量即d(d)/d(roll)//各種cos sin的是旋轉(zhuǎn)矩陣對roll求導(dǎo)，pointOri.x是點的坐標，coeff.x等是距離到局部點的偏導(dǎo)，也就是法向量（建議看鏈接）//注意：鏈接當中的R0-5公式中，ex和ey是反的//另一個鏈接https://blog.csdn.net/weixin_37835423/article/details/111587379#commentBox當中寫的更好float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;// 同上，求解的是對pitch的偏導(dǎo)量float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;// lidar -> camera//matA就是誤差對旋轉(zhuǎn)和平移變量的雅克比矩陣matA.at<float>(i, 0) = arz;matA.at<float>(i, 1) = arx;matA.at<float>(i, 2) = ary;//對平移求誤差就是法向量，見鏈接matA.at<float>(i, 3) = coeff.z;matA.at<float>(i, 4) = coeff.x;matA.at<float>(i, 5) = coeff.y;// 殘差項matB.at<float>(i, 0) = -coeff.intensity;}// 將矩陣由matA轉(zhuǎn)置生成matAt// 先進行計算，以便于后邊調(diào)用 cv::solve求解cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// 利用高斯牛頓法進行求解，// 高斯牛頓法的原型是J^(T)*J * delta(x) = -J*f(x)// J是雅克比矩陣，這里是A，f(x)是優(yōu)化目標，這里是-B(符號在給B賦值時候就放進去了)// 通過QR分解的方式，求解matAtA*matX=matAtB，得到解matXcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// iterCount==0 說明是第一次迭代，需要初始化if (iterCount == 0) {// 對近似的Hessian矩陣求特征值和特征向量，cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));// 對近似的Hessian矩陣求特征值和特征向量，//matE特征值,matV是特征向量//退化方向只與原始的約束方向 A有關(guān)，與原始約束的位置 b 無關(guān)//算這個的目的是要判斷退化，即約束中較小的偏移會導(dǎo)致解所在的局部區(qū)域發(fā)生較大的變化cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;//初次優(yōu)化時，特征值門限設(shè)置為100，小于這個值認為是退化了//系統(tǒng)退化與否和系統(tǒng)是否存在解沒有必然聯(lián)系，即使系統(tǒng)出現(xiàn)退化，系統(tǒng)也是可能存在解的，//因此需要將系統(tǒng)的解進行調(diào)整，一個策略就是將解進行投影，//對于退化方向，使用優(yōu)化的狀態(tài)估計值，對于非退化方向，依然使用方程的解。//另一個策略就是直接拋棄解在退化方向的分量。//對于退化方向，我們不考慮，直接丟棄，只考慮非退化方向解的增量。float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}//以下這步可以參考鏈接：// https://blog.csdn.net/i_robots/article/details/108724606// 以及https://zhuanlan.zhihu.com/p/258159552matP = matV.inv() * matV2;}if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}// 更新當前位姿 x = x + delta_xtransformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));// 旋轉(zhuǎn)或者平移量足夠小就停止這次迭代過程if (deltaR < 0.05 && deltaT < 0.05) {return true; // converged}return false; // keep optimizing}/*** 用imu原始RPY數(shù)據(jù)與scan-to-map優(yōu)化后的位姿進行加權(quán)融合，更新當前幀位姿的roll、pitch，約束z坐標*/void transformUpdate(){if (cloudInfo.imuAvailable == true){// 俯仰角小于1.4if (std::abs(cloudInfo.imuPitchInit) < 1.4){double imuWeight = imuRPYWeight;tf::Quaternion imuQuaternion;tf::Quaternion transformQuaternion;double rollMid, pitchMid, yawMid;// slerp roll// roll角求加權(quán)均值，用scan-to-map優(yōu)化得到的位姿與imu原始RPY數(shù)據(jù)，進行加權(quán)平均transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);transformTobeMapped[0] = rollMid;// slerp pitch// pitch角求加權(quán)均值，用scan-to-map優(yōu)化得到的位姿與imu原始RPY數(shù)據(jù)，進行加權(quán)平均transformQuaternion.setRPY(0, transformTobeMapped[1], 0);imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);transformTobeMapped[1] = pitchMid;}}// 更新當前幀位姿的roll, pitch, z坐標；因為是小車，roll、pitch是相對穩(wěn)定的，// 不會有很大變動，一定程度上可以信賴imu的數(shù)據(jù)，z是進行高度約束transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);// 當前幀位姿incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);}/*** 設(shè)置當前幀為關(guān)鍵幀并執(zhí)行因子圖優(yōu)化* 1、計算當前幀與前一幀位姿變換，如果變化太小，不設(shè)為關(guān)鍵幀，反之設(shè)為關(guān)鍵幀* 2、添加激光里程計因子、GPS因子、閉環(huán)因子* 3、執(zhí)行因子圖優(yōu)化* 4、得到當前幀優(yōu)化后位姿，位姿協(xié)方差* 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加當前關(guān)鍵幀的角點、平面點集合*/ void saveKeyFramesAndFactor(){// 計算當前幀與前一幀位姿變換，如果變化太小，不設(shè)為關(guān)鍵幀，反之設(shè)為關(guān)鍵幀if (saveFrame() == false)return;// odom factoraddOdomFactor();// gps factoraddGPSFactor();// loop factoraddLoopFactor();// cout << "****************************************************" << endl;// gtSAMgraph.print("GTSAM Graph:\n");// update iSAM// 執(zhí)行優(yōu)化isam->update(gtSAMgraph, initialEstimate);isam->update();if (aLoopIsClosed == true){isam->update();isam->update();isam->update();isam->update();isam->update();}// update之后要清空一下保存的因子圖，注：歷史數(shù)據(jù)不會清掉，ISAM保存起來了gtSAMgraph.resize(0);initialEstimate.clear();//save key posesPointType thisPose3D;PointTypePose thisPose6D;Pose3 latestEstimate;// 優(yōu)化結(jié)果isamCurrentEstimate = isam->calculateEstimate();// 當前幀位姿結(jié)果latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);// cout << "****************************************************" << endl;// isamCurrentEstimate.print("Current estimate: ");// cloudKeyPoses3D加入當前幀位姿thisPose3D.x = latestEstimate.translation().x();thisPose3D.y = latestEstimate.translation().y();thisPose3D.z = latestEstimate.translation().z();// 索引thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as indexcloudKeyPoses3D->push_back(thisPose3D);// cloudKeyPoses6D加入當前幀位姿thisPose6D.x = thisPose3D.x;thisPose6D.y = thisPose3D.y;thisPose6D.z = thisPose3D.z;thisPose6D.intensity = thisPose3D.intensity ; // this can be used as indexthisPose6D.roll = latestEstimate.rotation().roll();thisPose6D.pitch = latestEstimate.rotation().pitch();thisPose6D.yaw = latestEstimate.rotation().yaw();thisPose6D.time = timeLaserInfoCur;cloudKeyPoses6D->push_back(thisPose6D);// cout << "****************************************************" << endl;// cout << "Pose covariance:" << endl;// cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;// 位姿協(xié)方差poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);// save updated transform// transformTobeMapped更新當前幀位姿transformTobeMapped[0] = latestEstimate.rotation().roll();transformTobeMapped[1] = latestEstimate.rotation().pitch();transformTobeMapped[2] = latestEstimate.rotation().yaw();transformTobeMapped[3] = latestEstimate.translation().x();transformTobeMapped[4] = latestEstimate.translation().y();transformTobeMapped[5] = latestEstimate.translation().z();// save all the received edge and surf points// 當前幀激光角點、平面點，降采樣集合pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);// save key frame cloud// 保存特征點降采樣集合cornerCloudKeyFrames.push_back(thisCornerKeyFrame);surfCloudKeyFrames.push_back(thisSurfKeyFrame);// save path for visualization// 更新里程計軌跡updatePath(thisPose6D);}/*** 計算當前幀與前一幀位姿變換，如果變化太小，不設(shè)為關(guān)鍵幀，反之設(shè)為關(guān)鍵幀*/bool saveFrame(){if (cloudKeyPoses3D->points.empty())return true;// 前一幀位姿//注：最開始沒有的時候，在函數(shù)extractCloud里面有Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());// 當前幀位姿Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);// 位姿變換增量Eigen::Affine3f transBetween = transStart.inverse() * transFinal;float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);// 旋轉(zhuǎn)和平移量都較小，當前幀不設(shè)為關(guān)鍵幀if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&abs(pitch) < surroundingkeyframeAddingAngleThreshold && abs(yaw) < surroundingkeyframeAddingAngleThreshold &&sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)return false;return true;}/*** 添加激光里程計因子*/void addOdomFactor(){if (cloudKeyPoses3D->points.empty()){// 第一幀初始化先驗因子noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*metergtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));// 變量節(jié)點設(shè)置初始值initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));}else{// 添加激光里程計因子noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);// 參數(shù)：前一幀id，當前幀id，前一幀與當前幀的位姿變換（作為觀測值），噪聲協(xié)方差gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));// 變量節(jié)點設(shè)置初始值initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);}}/*** 添加GPS因子*/void addGPSFactor(){if (gpsQueue.empty())return;// wait for system initialized and settles down// 如果沒有關(guān)鍵幀，或者首尾關(guān)鍵幀距離小于5m，不添加gps因子if (cloudKeyPoses3D->points.empty())return;else{if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)return;}// pose covariance small, no need to correct// 位姿協(xié)方差很小，沒必要加入GPS數(shù)據(jù)進行校正//3和4我猜可能是x和y？（6維，roll，pitch，yaw，x，y，z）if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)return;// last gps positionstatic PointType lastGPSPoint;while (!gpsQueue.empty()){// 刪除當前幀0.2s之前的里程計if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2){// message too oldgpsQueue.pop_front();}// 超過當前幀0.2s之后，退出else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2){// message too newbreak;}else{nav_msgs::Odometry thisGPS = gpsQueue.front();gpsQueue.pop_front();// GPS噪聲協(xié)方差太大，不能用// GPS too noisy, skipfloat noise_x = thisGPS.pose.covariance[0];float noise_y = thisGPS.pose.covariance[7];float noise_z = thisGPS.pose.covariance[14];if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)continue;// GPS里程計位置float gps_x = thisGPS.pose.pose.position.x;float gps_y = thisGPS.pose.pose.position.y;float gps_z = thisGPS.pose.pose.position.z;if (!useGpsElevation){gps_z = transformTobeMapped[5];noise_z = 0.01;}// GPS not properly initialized (0,0,0)// (0,0,0)無效數(shù)據(jù)if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)continue;// Add GPS every a few meters// 每隔5m添加一個GPS里程計PointType curGPSPoint;curGPSPoint.x = gps_x;curGPSPoint.y = gps_y;curGPSPoint.z = gps_z;if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)continue;elselastGPSPoint = curGPSPoint;// 添加GPS因子gtsam::Vector Vector3(3);Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);gtSAMgraph.add(gps_factor);aLoopIsClosed = true;break;}}}/*** 添加閉環(huán)因子*/ void addLoopFactor(){if (loopIndexQueue.empty())return;// 閉環(huán)隊列for (int i = 0; i < (int)loopIndexQueue.size(); ++i){// 閉環(huán)邊對應(yīng)兩幀的索引int indexFrom = loopIndexQueue[i].first;int indexTo = loopIndexQueue[i].second;// 閉環(huán)邊的位姿變換gtsam::Pose3 poseBetween = loopPoseQueue[i];gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));}loopIndexQueue.clear();loopPoseQueue.clear();loopNoiseQueue.clear();aLoopIsClosed = true;}/*** 更新里程計軌跡*/void updatePath(const PointTypePose& pose_in){geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);pose_stamped.header.frame_id = odometryFrame;pose_stamped.pose.position.x = pose_in.x;pose_stamped.pose.position.y = pose_in.y;pose_stamped.pose.position.z = pose_in.z;tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);pose_stamped.pose.orientation.x = q.x();pose_stamped.pose.orientation.y = q.y();pose_stamped.pose.orientation.z = q.z();pose_stamped.pose.orientation.w = q.w();globalPath.poses.push_back(pose_stamped);}/*** 更新因子圖中所有變量節(jié)點的位姿，也就是所有歷史關(guān)鍵幀的位姿，更新里程計軌跡*/void correctPoses(){if (cloudKeyPoses3D->points.empty())return;if (aLoopIsClosed == true){// clear map cache// 清空局部maplaserCloudMapContainer.clear();// clear path// 清空里程計軌跡globalPath.poses.clear();// update key poses// 更新因子圖中所有變量節(jié)點的位姿，也就是所有歷史關(guān)鍵幀的位姿int numPoses = isamCurrentEstimate.size();for (int i = 0; i < numPoses; ++i){cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();updatePath(cloudKeyPoses6D->points[i]);}aLoopIsClosed = false;}}/*** 發(fā)布激光里程計*/void publishOdometry(){// Publish odometry for ROS (global)// 發(fā)布激光里程計，odom等價mapnav_msgs::Odometry laserOdometryROS;laserOdometryROS.header.stamp = timeLaserInfoStamp;laserOdometryROS.header.frame_id = odometryFrame;laserOdometryROS.child_frame_id = "odom_mapping";laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);pubLaserOdometryGlobal.publish(laserOdometryROS);// Publish TF// 發(fā)布TF，odom->lidarstatic tf::TransformBroadcaster br;tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");br.sendTransform(trans_odom_to_lidar);// Publish odometry for ROS (incremental)static bool lastIncreOdomPubFlag = false;static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msgstatic Eigen::Affine3f increOdomAffine; // incremental odometry in affine//第一次數(shù)據(jù)直接用全局里程計初始化if (lastIncreOdomPubFlag == false){lastIncreOdomPubFlag = true;laserOdomIncremental = laserOdometryROS;increOdomAffine = trans2Affine3f(transformTobeMapped);} else {// 當前幀與前一幀之間的位姿變換Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;increOdomAffine = increOdomAffine * affineIncre;float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);if (cloudInfo.imuAvailable == true){if (std::abs(cloudInfo.imuPitchInit) < 1.4){double imuWeight = 0.1;tf::Quaternion imuQuaternion;tf::Quaternion transformQuaternion;double rollMid, pitchMid, yawMid;// slerp roll// roll姿態(tài)角加權(quán)平均transformQuaternion.setRPY(roll, 0, 0);imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);roll = rollMid;// slerp pitch// pitch姿態(tài)角加權(quán)平均transformQuaternion.setRPY(0, pitch, 0);imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);pitch = pitchMid;}}laserOdomIncremental.header.stamp = timeLaserInfoStamp;laserOdomIncremental.header.frame_id = odometryFrame;laserOdomIncremental.child_frame_id = "odom_mapping";laserOdomIncremental.pose.pose.position.x = x;laserOdomIncremental.pose.pose.position.y = y;laserOdomIncremental.pose.pose.position.z = z;laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);if (isDegenerate)laserOdomIncremental.pose.covariance[0] = 1;elselaserOdomIncremental.pose.covariance[0] = 0;}pubLaserOdometryIncremental.publish(laserOdomIncremental);}/*** 發(fā)布里程計、點云、軌跡* 1、發(fā)布歷史關(guān)鍵幀位姿集合* 2、發(fā)布局部map的降采樣平面點集合* 3、發(fā)布歷史幀（累加的）的角點、平面點降采樣集合* 4、發(fā)布里程計軌跡*/void publishFrames(){if (cloudKeyPoses3D->points.empty())return;// publish key poses// 發(fā)布歷史關(guān)鍵幀位姿集合publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);// Publish surrounding key frames// 發(fā)布局部map的降采樣平面點集合publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);// publish registered key frame// 發(fā)布當前幀的角點、平面點降采樣集合if (pubRecentKeyFrame.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);}// publish registered high-res raw cloud// 發(fā)布當前幀原始點云配準之后的點云if (pubCloudRegisteredRaw.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);}// publish path// 發(fā)布里程計軌跡if (pubPath.getNumSubscribers() != 0){globalPath.header.stamp = timeLaserInfoStamp;globalPath.header.frame_id = odometryFrame;pubPath.publish(globalPath);}}void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg){gpsQueue.push_back(*gpsMsg);}// 根據(jù)當前幀位姿，變換到世界坐標系（map系）下void pointAssociateToMap(PointType const * const pi, PointType * const po){po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);po->intensity = pi->intensity;}pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn){pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());int cloudSize = cloudIn->size();cloudOut->resize(cloudSize);Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < cloudSize; ++i){const auto &pointFrom = cloudIn->points[i];cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);cloudOut->points[i].intensity = pointFrom.intensity;}return cloudOut;}gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint){return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));}gtsam::Pose3 trans2gtsamPose(float transformIn[]){return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]), gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));}Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint){ return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);}/*** Eigen格式的位姿變換*/Eigen::Affine3f trans2Affine3f(float transformIn[]){return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);}PointTypePose trans2PointTypePose(float transformIn[]){PointTypePose thisPose6D;thisPose6D.x = transformIn[3];thisPose6D.y = transformIn[4];thisPose6D.z = transformIn[5];thisPose6D.roll = transformIn[0];thisPose6D.pitch = transformIn[1];thisPose6D.yaw = transformIn[2];return thisPose6D;}/*** 保存全局關(guān)鍵幀特征點集合*/bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res){string saveMapDirectory;cout << "****************************************************" << endl;cout << "Saving map to pcd files ..." << endl;if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;else saveMapDirectory = std::getenv("HOME") + req.destination;cout << "Save destination: " << saveMapDirectory << endl;// create directory and remove old files;int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());// save key frame transformationspcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);// extract global point cloud mappcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";}if(req.resolution != 0){cout << "\n\nSave resolution: " << req.resolution << endl;// down-sample and save corner clouddownSizeFilterCorner.setInputCloud(globalCornerCloud);downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);downSizeFilterCorner.filter(*globalCornerCloudDS);pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);// down-sample and save surf clouddownSizeFilterSurf.setInputCloud(globalSurfCloud);downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);downSizeFilterSurf.filter(*globalSurfCloudDS);pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);}else{// save corner cloudpcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);// save surf cloudpcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);}// save global point cloud map// 保存到一起，全局關(guān)鍵幀特征點集合*globalMapCloud += *globalCornerCloud;*globalMapCloud += *globalSurfCloud;int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);res.success = ret == 0;downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);cout << "****************************************************" << endl;cout << "Saving map to pcd files completed\n" << endl;return true;}/*** 展示線程* 1、發(fā)布局部關(guān)鍵幀map的特征點云* 2、保存全局關(guān)鍵幀特征點集合*/ void visualizeGlobalMapThread(){ros::Rate rate(0.2);while (ros::ok()){rate.sleep();// 發(fā)布局部關(guān)鍵幀map的特征點云publishGlobalMap();}if (savePCD == false)return;lio_sam::save_mapRequest req;lio_sam::save_mapResponse res;if(!saveMapService(req, res)){cout << "Fail to save map" << endl;}}/*** 發(fā)布局部關(guān)鍵幀map的特征點云*/void publishGlobalMap(){if (pubLaserCloudSurround.getNumSubscribers() == 0)return;if (cloudKeyPoses3D->points.empty() == true)return;pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());// kd-tree to find near key frames to visualize// kdtree查找最近一幀關(guān)鍵幀相鄰的關(guān)鍵幀集合std::vector<int> pointSearchIndGlobalMap;std::vector<float> pointSearchSqDisGlobalMap;// search near key frames to visualizemtx.lock();kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);mtx.unlock();for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);// downsample near selected key frames// 降采樣pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualizationdownSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualizationdownSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);for(auto& pt : globalMapKeyPosesDS->points){kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;}// extract visualized and downsampled key frames// 提取局部相鄰關(guān)鍵幀對應(yīng)的特征點云for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){// 距離過大if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)continue;int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);}// downsample visualized points// 降采樣，發(fā)布pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualizationdownSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualizationdownSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);}/*** 訂閱來自外部閉環(huán)檢測程序提供的閉環(huán)數(shù)據(jù)，本程序沒有提供，這里實際沒用上*/void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg){std::lock_guard<std::mutex> lock(mtxLoopInfo);if (loopMsg->data.size() != 2)return;loopInfoVec.push_back(*loopMsg);while (loopInfoVec.size() > 5)loopInfoVec.pop_front();}/*** 閉環(huán)線程* 1、閉環(huán)scan-to-map，icp優(yōu)化位姿* 1) 在歷史關(guān)鍵幀中查找與當前關(guān)鍵幀距離最近的關(guān)鍵幀集合，選擇時間相隔較遠的一幀作為候選閉環(huán)幀* 2) 提取當前關(guān)鍵幀特征點集合，降采樣；提取閉環(huán)匹配關(guān)鍵幀前后相鄰若干幀的關(guān)鍵幀特征點集合，降采樣* 3) 執(zhí)行scan-to-map優(yōu)化，調(diào)用icp方法，得到優(yōu)化后位姿，構(gòu)造閉環(huán)因子需要的數(shù)據(jù)，在因子圖優(yōu)化中一并加入更新位姿* 2、rviz展示閉環(huán)邊*/void loopClosureThread(){if (loopClosureEnableFlag == false)return;ros::Rate rate(loopClosureFrequency);while (ros::ok()){rate.sleep();performLoopClosure();visualizeLoopClosure();}}/*** 閉環(huán)scan-to-map，icp優(yōu)化位姿* 1、在歷史關(guān)鍵幀中查找與當前關(guān)鍵幀距離最近的關(guān)鍵幀集合，選擇時間相隔較遠的一幀作為候選閉環(huán)幀* 2、提取當前關(guān)鍵幀特征點集合，降采樣；提取閉環(huán)匹配關(guān)鍵幀前后相鄰若干幀的關(guān)鍵幀特征點集合，降采樣* 3、執(zhí)行scan-to-map優(yōu)化，調(diào)用icp方法，得到優(yōu)化后位姿，構(gòu)造閉環(huán)因子需要的數(shù)據(jù)，在因子圖優(yōu)化中一并加入更新位姿* 注：閉環(huán)的時候沒有立即更新當前幀的位姿，而是添加閉環(huán)因子，讓圖優(yōu)化去更新位姿*/void performLoopClosure(){if (cloudKeyPoses3D->points.empty() == true)return;mtx.lock();*copy_cloudKeyPoses3D = *cloudKeyPoses3D;*copy_cloudKeyPoses6D = *cloudKeyPoses6D;mtx.unlock();// find keys// 當前關(guān)鍵幀索引，候選閉環(huán)匹配幀索引int loopKeyCur;int loopKeyPre;// not-usedif (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)// 在歷史關(guān)鍵幀中查找與當前關(guān)鍵幀距離最近的關(guān)鍵幀集合，選擇時間相隔較遠的一幀作為候選閉環(huán)幀if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)return;// extract cloud// 提取pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());{// 提取當前關(guān)鍵幀特征點集合，降采樣;//loopFindNearKeyframes形參分別為點云集合，當前幀的索引，搜索半徑loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);// 提取閉環(huán)匹配關(guān)鍵幀前后相鄰若干幀的關(guān)鍵幀特征點集合，降采樣loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);// 如果特征點較少，返回if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)return;// 發(fā)布閉環(huán)匹配關(guān)鍵幀局部mapif (pubHistoryKeyFrames.getNumSubscribers() != 0)publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);}// ICP Settings// ICP參數(shù)設(shè)置static pcl::IterativeClosestPoint<PointType, PointType> icp;icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-6);icp.setEuclideanFitnessEpsilon(1e-6);icp.setRANSACIterations(0);// Align clouds// scan-to-map，調(diào)用icp匹配//icp.setInputSource,icp.setInputTargeticp.setInputSource(cureKeyframeCloud);icp.setInputTarget(prevKeyframeCloud);pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());icp.align(*unused_result);// 未收斂，或者匹配不夠好if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)return;// publish corrected cloud// 發(fā)布當前關(guān)鍵幀經(jīng)過閉環(huán)優(yōu)化后的位姿變換之后的特征點云if (pubIcpKeyFrames.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);}// Get pose transformation// 閉環(huán)優(yōu)化得到的當前關(guān)鍵幀與閉環(huán)關(guān)鍵幀之間的位姿變換float x, y, z, roll, pitch, yaw;Eigen::Affine3f correctionLidarFrame;correctionLidarFrame = icp.getFinalTransformation();// transform from world origin to wrong pose// 閉環(huán)優(yōu)化前當前幀位姿Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);// transform from world origin to corrected pose// 閉環(huán)優(yōu)化后當前幀位姿Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed framepcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));// 閉環(huán)匹配幀的位姿gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);gtsam::Vector Vector6(6);float noiseScore = icp.getFitnessScore();Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);// Add pose constraint// 添加閉環(huán)因子需要的數(shù)據(jù)//這些內(nèi)容會在函數(shù)addLoopFactor中用到mtx.lock();loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));loopPoseQueue.push_back(poseFrom.between(poseTo));loopNoiseQueue.push_back(constraintNoise);mtx.unlock();// add loop constriantloopIndexContainer[loopKeyCur] = loopKeyPre;}/*** not-used, 來自外部閉環(huán)檢測程序提供的閉環(huán)匹配索引對*/bool detectLoopClosureExternal(int *latestID, int *closestID){// this function is not used yet, please ignore itint loopKeyCur = -1;int loopKeyPre = -1;std::lock_guard<std::mutex> lock(mtxLoopInfo);if (loopInfoVec.empty())return false;double loopTimeCur = loopInfoVec.front().data[0];double loopTimePre = loopInfoVec.front().data[1];loopInfoVec.pop_front();if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)return false;int cloudSize = copy_cloudKeyPoses6D->size();if (cloudSize < 2)return false;// latest keyloopKeyCur = cloudSize - 1;for (int i = cloudSize - 1; i >= 0; --i){if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);elsebreak;}// previous keyloopKeyPre = 0;for (int i = 0; i < cloudSize; ++i){if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);elsebreak;}if (loopKeyCur == loopKeyPre)return false;auto it = loopIndexContainer.find(loopKeyCur);if (it != loopIndexContainer.end())return false;*latestID = loopKeyCur;*closestID = loopKeyPre;return true;}/*** 在歷史關(guān)鍵幀中查找與當前關(guān)鍵幀距離最近的關(guān)鍵幀集合，選擇時間相隔較遠的一幀作為候選閉環(huán)幀*/bool detectLoopClosureDistance(int *latestID, int *closestID){int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;int loopKeyPre = -1;// check loop constraint added before// 當前幀已經(jīng)添加過閉環(huán)對應(yīng)關(guān)系，不再繼續(xù)添加auto it = loopIndexContainer.find(loopKeyCur);if (it != loopIndexContainer.end())return false;// find the closest history key frame// 在歷史關(guān)鍵幀中查找與當前關(guān)鍵幀距離最近的關(guān)鍵幀集合//配置文件中默認historyKeyframeSearchRadius=15mstd::vector<int> pointSearchIndLoop;std::vector<float> pointSearchSqDisLoop;kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);// 在候選關(guān)鍵幀集合中，找到與當前幀時間相隔較遠的幀，設(shè)為候選匹配幀//配置文件中默認30sfor (int i = 0; i < (int)pointSearchIndLoop.size(); ++i){int id = pointSearchIndLoop[i];if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff){loopKeyPre = id;break;}}if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)return false;*latestID = loopKeyCur;*closestID = loopKeyPre;return true;}/*** 提取key索引的關(guān)鍵幀前后相鄰若干幀的關(guān)鍵幀特征點集合，降采樣*/void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum){// extract near keyframes// 提取key索引的關(guān)鍵幀前后相鄰若干幀的關(guān)鍵幀特征點集合nearKeyframes->clear();int cloudSize = copy_cloudKeyPoses6D->size();//通過-searchNum 到 +searchNum，搜索key兩側(cè)內(nèi)容for (int i = -searchNum; i <= searchNum; ++i){int keyNear = key + i;if (keyNear < 0 || keyNear >= cloudSize )continue;*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);}if (nearKeyframes->empty())return;// downsample near keyframes// 降采樣pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());downSizeFilterICP.setInputCloud(nearKeyframes);downSizeFilterICP.filter(*cloud_temp);*nearKeyframes = *cloud_temp;}/*** rviz展示閉環(huán)邊*/void visualizeLoopClosure(){if (loopIndexContainer.empty())return;visualization_msgs::MarkerArray markerArray;// loop nodesvisualization_msgs::Marker markerNode;markerNode.header.frame_id = odometryFrame;markerNode.header.stamp = timeLaserInfoStamp;markerNode.action = visualization_msgs::Marker::ADD;markerNode.type = visualization_msgs::Marker::SPHERE_LIST;markerNode.ns = "loop_nodes";markerNode.id = 0;markerNode.pose.orientation.w = 1;markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3; markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;markerNode.color.a = 1;// loop edgesvisualization_msgs::Marker markerEdge;markerEdge.header.frame_id = odometryFrame;markerEdge.header.stamp = timeLaserInfoStamp;markerEdge.action = visualization_msgs::Marker::ADD;markerEdge.type = visualization_msgs::Marker::LINE_LIST;markerEdge.ns = "loop_edges";markerEdge.id = 1;markerEdge.pose.orientation.w = 1;markerEdge.scale.x = 0.1;markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;markerEdge.color.a = 1;// 遍歷閉環(huán)for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it){int key_cur = it->first;int key_pre = it->second;geometry_msgs::Point p;p.x = copy_cloudKeyPoses6D->points[key_cur].x;p.y = copy_cloudKeyPoses6D->points[key_cur].y;p.z = copy_cloudKeyPoses6D->points[key_cur].z;markerNode.points.push_back(p);markerEdge.points.push_back(p);p.x = copy_cloudKeyPoses6D->points[key_pre].x;p.y = copy_cloudKeyPoses6D->points[key_pre].y;p.z = copy_cloudKeyPoses6D->points[key_pre].z;markerNode.points.push_back(p);markerEdge.points.push_back(p);}markerArray.markers.push_back(markerNode);markerArray.markers.push_back(markerEdge);pubLoopConstraintEdge.publish(markerArray);}/*** not-used*/void extractForLoopClosure(){pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());int numPoses = cloudKeyPoses3D->size();for (int i = numPoses-1; i >= 0; --i){if ((int)cloudToExtract->size() <= surroundingKeyframeSize)cloudToExtract->push_back(cloudKeyPoses3D->points[i]);elsebreak;}// 將相鄰關(guān)鍵幀集合對應(yīng)的角點、平面點，加入到局部map中，作為scan-to-map匹配的局部點云地圖extractCloud(cloudToExtract);}void updatePointAssociateToMap(){transPointAssociateToMap = trans2Affine3f(transformTobeMapped);}//相當于clip函數(shù)float constraintTransformation(float value, float limit){if (value < -limit)value = -limit;if (value > limit)value = limit;return value;}};int main(int argc, char** argv) {ros::init(argc, argv, "lio_sam");mapOptimization MO;ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");std::thread loopthread(&mapOptimization::loopClosureThread, &MO);std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);ros::spin();loopthread.join();visualizeMapThread.join();return 0; }

總結(jié)

1.第一個巨大的回調(diào)函數(shù)：lasercloudinfoHandle：

這個函數(shù)監(jiān)聽的是/feature/cloud_info，關(guān)于這個話題里面包含的內(nèi)容，我已經(jīng)在featureExtraction.cpp的總結(jié)部分說過了。這里回憶一下，cloud_info是作者自定義的一個特殊的msg類型，包含了當前激光幀的角點集合，平面點集合，一維數(shù)組，每根線開始的點和結(jié)束點在一維數(shù)組中的索引……

那么收到數(shù)據(jù)，先保存時間戳，然后提取已經(jīng)在featureExtraction.cpp中被提取的角點和平面點信息，保存到*laserCloudCornerLast和*laserCloudSurfLast中。請記住這兩個命名。

頻率控制：當前時刻-上一時刻>=0.15s時，才開始下列循環(huán)：

1.1? UpdateInitGuess：當前幀初始化。

1.1.1 當關(guān)鍵幀集合為空時，用激光幀的imu原始數(shù)據(jù)角度數(shù)據(jù)初始化，并且把數(shù)據(jù)用lastImuTransformation保存（記住這個命名），返回。

此時推薦回顧一下imageProjection.cpp部分的總結(jié)3.2

1.1.2 假如cloudInfo.odomAvailable=true時，那么就用一個transBack來記錄cloudInfo.initialGuessX等信息，（這個信息其實來自于imupreintegration.cpp中發(fā)布的imu里程計數(shù)據(jù)），然后記錄增量，在之前系統(tǒng)狀態(tài)transformTobeMapped的基礎(chǔ)上，加上這個增量，再次存入transformTobeMapped。 注意這個transformTobeMapped，這個數(shù)據(jù)結(jié)構(gòu)，在這個cpp里，我們可以理解為就是它在存儲著激光里程計部分的系統(tǒng)狀態(tài)，包括位置與姿態(tài)。

注意，這里有一個lastImuPreTransformation，這個用來保存上一時刻imu里程計的數(shù)據(jù)，根據(jù)它和當前的imu里程計來算增量。不要和lastImuTransformation變量混起來，雖然這倆變量名字長的很像。

然后覆蓋lastImuTransformation（在這個case里沒用到），返回；

1.1.3 假如cloudInfo.imuAvailable=true，那么進入這個case：

注意，lastImuTransformation在1.1.1和1.1.2中并未用到，只是不斷的在替換成最新數(shù)據(jù)。當cloudInfo.odomAvailable一直是true的時候，程序壓根也不會進入到這個case。

但是，凡事總有例外，萬一哪里沒有銜接好，imu里程計沒有及時算出來，那么就導(dǎo)致此時激光幀的位姿沒有一個初始化的數(shù)據(jù)（我們之后是要在初始化的基礎(chǔ)上進行優(yōu)化的），那么之后的優(yōu)化就無從進行。因此，就要用到這個case。

這里主要思路是用imuRollInit數(shù)據(jù)來初始化，如果你回顧過imageProjection.cpp部分的總結(jié)3.2 那么你應(yīng)該就會懂，這里的數(shù)據(jù)來源是原始imu數(shù)據(jù)的角度信息。那么如果這里有數(shù)據(jù)，就用lastImuTransformation當成最新的上一時刻數(shù)據(jù)，當前數(shù)據(jù)transBack和它算一個增量，然后累積到系統(tǒng)值transformTobeMapped上面去。最后更新覆蓋lastImuTransformation，返回。

?1.2 extractSurroundingKeyFrames：這個是比較復(fù)雜的一個函數(shù)，以下的內(nèi)容希望讀者可以心平氣和的，每個字都依次念一遍。

如果沒有關(guān)鍵幀，那就算了，返回；

如果有，就調(diào)用extractNearby函數(shù)。

關(guān)鍵幀是啥？cloudKeyPoses3D，我們要記住這個變量，雖然到現(xiàn)在為止，我們還不知道它是怎么來的，但是這個東西是怎么獲取的，我們在后續(xù)必須弄明白。

在這里我先劇透一下：它里面裝的是歷史的“關(guān)鍵幀”的位置，即x，y，z三個值。需要明確：這里裝的絕不是歷史的關(guān)鍵幀位置處的點云。而是歷史關(guān)鍵幀記錄時刻機器人所在的位置。

同理還有一個cloudKeyPoses6D，它比這個3D還多了三個角度信息。之所以要用一個6D一個3D分別來裝關(guān)鍵幀，我現(xiàn)在直接揭曉答案：用3D裝，是因為我們要根據(jù)這個來構(gòu)建KD樹，搜索歷史上最近的節(jié)點。“最近”指的是距離上最近，即xyz空間坐標最近，和角度無關(guān)。而cloudKeyPoses6D，是用來投影點云的，把當前幀點云投影到世界坐標系下，那么投影就必須要用角度信息了，所以作者分別用了一個3D和一個6D來裝數(shù)據(jù)。

Kd樹的原理我這里不寫，隨便放一個鏈接：機器學(xué)習(xí)——詳解KD-Tree原理 - 知乎，實際上代碼里也只是調(diào)庫，所以這里我不寫。

那么接下來，我介紹extractNearby函數(shù)：

1.2.1 使用kd樹，搜索當前最近的關(guān)鍵幀，在給定半徑內(nèi)（默認是50m）所有關(guān)鍵幀最近的位置，并把結(jié)果返回到pointSearchInd，把距離返回到pointSearchSqDis中。

1.2.2 根據(jù)索引pointSearchInd，把相鄰關(guān)鍵幀存入到surroundingKeyPoses中。

1.2.3 下采樣，裝進surroundingKeyPosesDS中，并在原始的surroundingKeyPoses其中找到最近的一個點，替換掉索引。（關(guān)于這個，我的理解是，下采樣后不太準確了，好幾個不同的關(guān)鍵幀可能因為下采樣的原因混成了一個，所以要用原始數(shù)據(jù)對索引進行一個修正，這樣以后才方便根據(jù)索引投影點云）

1.2.4 順手把10s內(nèi)的關(guān)鍵幀cloudKeyPoses3D中的位置也加入到surroundingKeyPosesDS中。

1.2.5 extractCloud：提取邊緣和平面點對應(yīng)的localmap，把surroundingKeyPosesDS傳入到函數(shù)中：

1.2.5.1 對輸入的surroundingKeyPosesDS進行遍歷，找到50m之內(nèi)的位置，然后用transformPointCloud把對應(yīng)位置的點云，進行變換到世界坐標系下。

如何變換呢？根據(jù)上面提到的cloudKeyPoses6D的位姿，然后把cornerCloudKeyFrame和surfCloudKeyFrame中根據(jù)索引找到點云，投影到世界坐標系下。

那么在這里，cornerCloudKeyFrame和surfCloudKeyFrame是什么？之前從來沒有出現(xiàn)過。我這里同樣進行劇透，它里面存放的是下采樣的點云。注意總結(jié)1中的*laserCloudCornerLast和*laserCloudSurfLast這兩個東西，這是瞬時點云，這個東西會在之后被下采樣，然后裝入cornerCloudKeyFrame中。

1.2.5.2在角點點云和平面點點云被投影到世界坐標系下后，會被加入到laserCloudCornerFromMap和laserCloudSurfFromMap等數(shù)據(jù)結(jié)構(gòu)中，然后再合出一個pair類型的Container<關(guān)鍵幀號，<角點集合，平面點集合>>。

1.3 downsampleCurrentScan：

這部分比較簡單，就是對最外層的回調(diào)函數(shù)中的laserCloudCornerLast之類的東西，進行一個下采樣，保存到laserCloudCornerLastDS這些以DS結(jié)尾的數(shù)據(jù)結(jié)構(gòu)中，并且把數(shù)目存到laserCloudCornerLastDSNum這種以DSNUM結(jié)尾的數(shù)據(jù)結(jié)構(gòu)中。其實就是代表了當前幀點云的角點/平面點的下采樣集合，和數(shù)目值。

1.4 Scan2MapOptimization：

這個函數(shù)是本cpp中第二復(fù)雜的函數(shù)。我們現(xiàn)在把它展開。

1.4.1 首先，沒有關(guān)鍵幀保存，那就返回，不處理；

1.4.2 如果DSNUM這種記錄角點和平面點的數(shù)據(jù)結(jié)構(gòu)中，發(fā)現(xiàn)數(shù)目不夠多，也不處理；只有在數(shù)目足夠多的時候才進行處理，默認最少要10個角點，100個平面點。

迭代30次：

?1.4.2.1 邊緣點匹配優(yōu)化：CornerOptimization

1.4.2.2 平面點匹配優(yōu)化：SurfOptimization

1.4.2.3 組合優(yōu)化多項式系數(shù)：combineOptimizationCoeffs

1.4.2.4 LMOptimization判斷迭代誤差是否足夠小，如果是true則認為迭代完成，返回；

1.4.3 transformUpdate：原始的imu的rpy，在這里和優(yōu)化后的激光里程計位姿進行一個加權(quán)融合。

?接下來，我們依次展開這些函數(shù)：

?1.4.2.1 邊緣點匹配優(yōu)化：CornerOptimization

把系統(tǒng)狀態(tài)transformTobeMapped做一個數(shù)據(jù)格式轉(zhuǎn)換，變成transPointAssociateToMap形式

從當前角點下采樣集合laserCloudCornerLastDS進行遍歷，找到世界坐標系下最近的5個點，要求小于1m。

求5個樣本的均值，協(xié)方差矩陣。對協(xié)方差矩陣進行特征值分解，如果最大特征值大于次大特征值的3倍，那么就認為構(gòu)成線。

一旦發(fā)現(xiàn)構(gòu)成線，那么就在均值沿著最大特征向量方向（把它看成線的方向）前后各取一個點（+-0.1 x 方向）。

X0為當前點，X1和X2為“X0附近的5個點一起算出的均值沿方向前后各取的一點”，叉乘計算三點面積a012，x1x2底邊長度l12。然后再做一次叉乘，得到X0距離x1，x2連線的垂線段的單位方向向量（la，lb，lc）。并計算點到直線的距離ld2=a012/l12。

用一個魯棒和函數(shù)，使得距離ld2越大，s越小。然后用coeff來保存“魯棒后”的距離，和“魯棒后”的點到線的垂線的方向向量。

如果點到直線的距離小于1m，那么存入數(shù)據(jù)結(jié)構(gòu)，laserCloudOriCornerVec為原始點云，coeffSelCornerVec為魯棒距離和魯棒向量，laserCloudOriCornerFlag代表當前點X0 已經(jīng)找到對應(yīng)的線。

思考：為什么要加入方向向量呢？是因為這個在優(yōu)化的偏導(dǎo)數(shù)中會被用到。

1.4.2.2 平面點匹配優(yōu)化：SurfOptimization

和上面的同理，對系統(tǒng)狀態(tài)量transformTobeMapped進行數(shù)據(jù)格式轉(zhuǎn)換；

從當前角點下采樣集合laserCloudSurfLastDS進行遍歷，找到世界坐標系下最近的5個點，要求小于1m。

直接用matA0存儲這個5個點，求解Ax+By+Cz+1=0的ABC系數(shù)（用QR分解）

然后對ABCD，代碼中為pa，pb，pc，pd=1進行單位化。

根據(jù)點x0到平面的距離d= (分母為1）判斷是否構(gòu)成平面。如果有一個大于0.2m則不構(gòu)成。

pd2為點到平面的距離，也用魯棒和函數(shù)處理，并且比上兩次開方（這點我不理解，我猜就是用來魯棒的，換成1次開方可能也差不多，意義或許不大），然后和角點部分類似，得到s，存入數(shù)據(jù)結(jié)構(gòu)。

?1.4.2.3 組合優(yōu)化多項式系數(shù)：combineOptimizationCoeffs

這個比較簡單，就是把CornerOptimization和SurfOptimization中已經(jīng)確定匹配關(guān)系的點提取出來，laserCloudOri統(tǒng)一把角點和平面點裝在一起，coeffSel統(tǒng)一裝之前計算得到的“魯棒優(yōu)化向量”（角點就是點到直線的“魯棒垂線”，平面點就是點到平面的“魯棒法線”）。

優(yōu)化向量會在LMOptimization中進行優(yōu)化。

?1.4.2.4 LMOptimization判斷迭代誤差是否足夠小，如果是true則認為迭代完成，返回。

這一部分大內(nèi)容，主要麻煩在原理上面。

這里推薦一個閱讀：LIO-SAM-scan2MapOptimization()優(yōu)化原理及代碼解析

這個文章中公式寫的非常好。我就不照搬了。

另外在推導(dǎo)部分，可以仔細研究一下這篇文章：

LeGO-LOAM中的數(shù)學(xué)公式推導(dǎo)

雖然是Lego-loam的推導(dǎo)，但是Lego-loam和lio-sam在這部分的原理是一樣的，因此可以通用?？赐赀@篇文章，就能理解1.4.2.3中我提到的“優(yōu)化向量”是干啥用的。

總之，照著原理，構(gòu)建JtJ*delt_x=-JTf，然后構(gòu)建MatAtA，matAtB，利用cv:solve提供的QR分解，得到matX，即delta_x。

當特征點缺乏時，狀態(tài)估計方法會發(fā)生退化。特征值小于閾值，則視為退化方向向量。這塊的理論，可以參考LOAM SLAM原理之防止非線性優(yōu)化解退化

更新位姿，判斷收斂與否。那么真正的雷達里程計系統(tǒng)狀態(tài)transformTobeMapped，就是在這里被更新。

1.4.3 transformUpdate：原始的imu的rpy，在這里和優(yōu)化后的激光里程計位姿進行一個加權(quán)融合。

當imuAvailable=True的時候，并且俯仰角<1.4，那么對位姿和原始imu的rpy做一個加權(quán)平均，（權(quán)重在配置文件中可以被設(shè)置為0.01）。主要是對roll，pitch僅加權(quán)平均，并且對z進行一個高度約束（也就是clip，不得超過配置文件中的z_tollerance，這個主要是一個小trick，應(yīng)對不能飛起來的無人小車用的），更新transformTobeMapped。

好了，那么現(xiàn)在回到回調(diào)函數(shù)的主流程：

1.5 saveKeyFramesAndFactor：之前函數(shù)二話不說就用了一些并沒有出現(xiàn)過的數(shù)據(jù)結(jié)構(gòu)，例如什么cloudKeyPoses3D，cornerCloudKeyFrame之類的東西，看完這個函數(shù)將明白這些變量是怎么來的。

1.5.1 saveFrame：計算當前幀和前一幀位姿變換，如果太小不設(shè)關(guān)鍵幀。默認是角度<0.2，距離<1m，兩者不滿足其一就不保存；

1.5.2 addOdomFactor：

這個是要加入激光里程計因子，給圖優(yōu)化的模型gtSAMgraph。在1.5之前別的函數(shù)里，如果沒有關(guān)鍵幀，直接就跳過了。但是這里不能跳過。

如果暫時還沒有關(guān)鍵幀，就把當前激光系統(tǒng)狀態(tài)transformTobeMapped，打包成一個PriorFactor加入到gtSAMgraph里。如果目前已經(jīng)有關(guān)鍵幀了，就把最后一個關(guān)鍵幀，和當前狀態(tài)transformTobeMapped計算一個增量，把這個增量打包成一個BetweenFactor，加入到gtSAMgraph里頭去。

initialEstimate代表變量初值，用transformTobeMapped賦值。

1.5.3 addGpsFactor：

GPS的篩選規(guī)則為：如果沒有GPS信息，沒有關(guān)鍵幀信息，首尾關(guān)鍵幀小于5m，或者位姿的協(xié)方差很小（x，y的協(xié)方差閾值小于25），就不添加GPS。

否則，遍歷GPS列表，當前幀0.2s以前的不要，或者GPS的協(xié)方差太大了也不要，無效數(shù)據(jù)也不要…… 找到正常數(shù)據(jù)，打包成一個gps_factor，加入gtSAMgraph里面。

1.5.4 addLoopFactor：

這個其實和當前的回調(diào)函數(shù)無關(guān)，因為當前回調(diào)函數(shù)監(jiān)聽的是/feature/cloud_info信息，回環(huán)是由其他線程監(jiān)控和檢測的。那么在這里，它查詢回環(huán)隊列，加入回環(huán)因子，就是一個順手的事情，反正現(xiàn)在要更新優(yōu)化，那么查一下，如果有候選的等在那里，就順手加入優(yōu)化。如果用做飯來比喻這件事，那么另外的回環(huán)檢測的線程就是相當于另一個人在備菜，這里addLoopFactor相當于是在炒菜，備好了就先炒，沒有備好就算了。

1.5.5 gtsam正常更新。如果有回環(huán)那就多更新幾次。

1.5.6 把cloudKeyPoses3D，cloudKeyPoses6D，分別裝上信息，cloudKeyPoses3D代表關(guān)鍵幀的位置，cloudKeyPoses6D代表關(guān)鍵幀的位置+姿態(tài)，為什么要有一個3D一個6D呢？6D里不已經(jīng)包含了3D信息嗎？這個問題我在1.2處已經(jīng)解釋過了。

1.5.7 用優(yōu)化結(jié)果更新transformTobeMapped。

1.5.8 cornerCloudKeyFrames，surfCloudKeyFrames裝入信息，回顧一下，回調(diào)函數(shù)開頭收到的點云數(shù)據(jù)為laserCloudCornerLast，laserCloudSurfLast，然后在downsampleCurrentScan處這倆信息被下采樣，加上了DS后綴。在這里把它裝到cornerCloudKeyFrames和surfCloudKeyFrames中。

（回顧：cornerCloudKeyFrames代表關(guān)鍵幀位置處的角點點云，surfCloudKeyFrames代表關(guān)鍵幀位置處的平面點點云。這倆東西就是上面1.2處extractSurroundingKeyFrames用到的內(nèi)容，cornerCloudKeyFrames通過cloudKeyPoses6D變換到世界系下，被存到laserCloudCornerFromMap里面，這個FromMap又在scan2MapOptimization函數(shù)中被設(shè)置到kdtreeCornerFromMap這個Kd樹里，在cornerOptimization函數(shù)里，就是把當前幀的激光點云依據(jù)1.1的初值transformTobeMapped，變換到世界坐標系下，再用kdtreeCornerFromMap進行kd搜索，建立匹配關(guān)系，優(yōu)化transformTobeMapped。）

1.5.9 updatePath，更新里程計軌跡。把cloudKeyPoses6D傳入，保存在globalPath中。不過暫時還沒有進行發(fā)布。

?1.6 correctPoses：

如果發(fā)現(xiàn)回環(huán)的話，就把歷史關(guān)鍵幀通通更新一遍。我們剛剛在1.5.5里面雖然更新過了，但是結(jié)果都是保存在gtsam里面的，cloudKeyPoses3D和cloudKeyPoses6D，這倆保存位置和位姿的變量仍然保留著更新前的關(guān)鍵幀位姿。所以就根據(jù)更新結(jié)果，把他倆更新一遍。

為什么不更新cornerCloudKeyFrames和surfCloudKeyFrames呢？因為沒有必要更新，這倆存的是機器人坐標系下的點云，和機器人在世界系下的位姿是無關(guān)的。

1.7 publishOdometry：

到此為止，激光里程計部分的transformTobeMapped就不再更新了。回顧一下transformTobeMapped經(jīng)歷了哪些變換：在1.1部分用imu角度初值或是imu里程計初值賦值，然后在scan2mapOptimization里面根據(jù)點到線、點到面的方程進行更新，再在transformUpdate里和原始imu的rpy信息進行一個很小的加權(quán)融合（不過這一步我覺得沒啥大用），最后在saveKeyFrameAndFactor里面再加入GPS因子和回環(huán)因子進行一輪優(yōu)化。

最后把transformTobeMapped發(fā)布出去，其他cpp文件里，接收的“激光里程計”就是這么個東西。也就是lio_sam/mapping/odometry_incremental.

1.8 publishFrames:

這個純粹就是把亂七八糟東西都發(fā)布出去，不管有沒有用。如果用戶需要就可以監(jiān)聽它。

1.8.1發(fā)布關(guān)鍵幀位姿集合，把cloudKeyPoses3D發(fā)布成lio_sam/mapping/trajectory

1.8.2發(fā)布局部降采樣平面點，把laserCloudSurfFromMapDS（歷史默認50m內(nèi)的點，在extractCloud中被設(shè)置），發(fā)布為lio_sam/mapping/map_local

1.8.3發(fā)布當前幀的下采樣角點和平面點，用優(yōu)化后的激光里程計位姿transformTobeMapped投影到世界系下發(fā)布，/lio_sam/mapping/cloud_registered

1.8.4發(fā)布原始點云經(jīng)過配準的點云：輸入的/feature/cloud_info的cloud_deskewed字段是由featureExtraction.cpp發(fā)布的，其cloud_deskewed是源于imageProjection.cpp發(fā)布的原始去畸變點云。把它發(fā)布到世界坐標系下，然后以/lio_sam/mapping/cloud_registered_raw的形式發(fā)布。

1.8.5發(fā)布軌跡，把1.5.9里裝好在globalPath里面但是還沒有發(fā)布的軌跡發(fā)布出去，名為/lio_sam/mapping/path。

那么到現(xiàn)在，基本上mapOptimization.cpp的內(nèi)容就結(jié)束了，但是還有一些尾巴：

2.gpshandle：監(jiān)聽GPS數(shù)據(jù)，保存到GPS隊列里。

3.loopinfohandle：監(jiān)聽"lio_loop/loop_closure_detection",訂閱來自外部閉環(huán)檢測程序提供的閉環(huán)數(shù)據(jù)，本程序沒有提供，這里實際沒用上。

4.loopClosureThread：這個線程在主函數(shù)里單獨開了一個線程，簡要說一下：

4.1 讀取配置文件中是否開啟回環(huán)檢測。

4.2開始無限循環(huán)：

4.2.1 performLoopClosure：

在歷史幀中搜索距離關(guān)鍵幀最近，時間間各較遠的關(guān)鍵幀（默認是30s以外，15m以內(nèi)）沒找到就返回，如果找到了，結(jié)果就放在loopKeyPre當中，loopKeyCur保存最近一個關(guān)鍵幀。

把最近一個關(guān)鍵幀的特征點提出來，放入cureKeyframeCloud里；回環(huán)候選幀前后各25幀也提取出來，放入prevKeyframeCloud里。

把prevKeyframeCloud發(fā)布出去，名為lio_sam/mapping/icp_loop_closure_history/cloud

調(diào)用pcl庫的icp輪子，設(shè)定閾值，參數(shù)，用setInputSource，setInputTarget傳入兩個點云，用align對齊。成功閾值設(shè)定為0.3，成功則存在icp.getFinalTransformation里面。把當前關(guān)鍵幀的點云，用這個結(jié)果icp.getFinalTransformation，轉(zhuǎn)換以后，以lio_sam/mapping/icp_loop_closure_corrected_cloud發(fā)布出去。

把當前幀的的位姿用icp.getFinalTransformation結(jié)果校正一下，把pair<當前，回環(huán)>，間隔位姿，噪聲用隊列存起來，等待addLoopFactor來調(diào)用，即上面的1.5.4部分。

4.2.2 visualizeLoopClosure:

這部分內(nèi)容沒啥好說的，就是用于rviz展示，把關(guān)鍵幀節(jié)點和二者的約束用點和線連起來，以lio_sam/mapping/loop_closure_constraints發(fā)布出去。

5. 最后一個線程，visualizeGlobalMapThread:

這個主要是兩塊內(nèi)容：

5.1 publishGlobalmap：把當前關(guān)鍵幀附近1000m（默認）的關(guān)鍵幀找出來（其實也就是全局的了），降采樣，變換到世界系下，然后發(fā)布為lio_sam/mapping/map_global.

5.2 saveMapService:這個用來保存pcd格式的點云地圖。在配置文件中可以設(shè)置開啟與否，和存儲位置。注意，當程序結(jié)束時，ctrl+c以后，才會啟動保存任務(wù)。這個部分的代碼，和發(fā)布globalmap部分的核心內(nèi)容基本一致，反正就是把cornerCloudKeyFrames，surfCloudKeyFrames用cloudKeyPoses6D變換到世界系下，分別保存角點pcd和平面點pcd，以及全局(合起來）的pcd文件。

我的專欄地址是?微鑒道長SLAM學(xué)習(xí)筆記（目錄），希望感興趣的朋友多多關(guān)注。我目前在杭州從事強化學(xué)習(xí)導(dǎo)航和slam建圖相關(guān)的導(dǎo)航工作，我的聯(lián)系郵箱是zkyy828@163.com，歡迎交流和合作。

總結(jié)

以上是生活随笔為你收集整理的SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇：站多久可以“抵消”久坐伤害？世卫组织推荐
下一篇： Qt （高仿Visio）流程图组件开发（