寫在前面

本系列文章將對(duì)LOAM源代碼進(jìn)行講解，在講解過程中，涉及到論文中提到的部分，會(huì)結(jié)合論文以及我自己的理解進(jìn)行解讀，尤其是對(duì)于其中坐標(biāo)變換的部分，將會(huì)進(jìn)行詳細(xì)的講解。

作者：K.Fire | 來源：3DCV

原文閱讀：手撕LOAM源代碼laserOdometry.cpp特征檢測(cè)與匹配

添加微信：dddvisiona，備注：3D點(diǎn)云，拉你入群。文末附行業(yè)細(xì)分群。

本來是懶得寫的，一個(gè)是怕自己以后忘了，另外是我在學(xué)習(xí)過程中，其實(shí)沒有感覺哪一個(gè)博主能講解的通篇都能讓我很明白，特別是坐標(biāo)變換部分的代碼，所以想著自己學(xué)完之后，按照自己的理解，也寫一個(gè)LOAM解讀，希望能對(duì)后續(xù)學(xué)習(xí)LOAM的同學(xué)們有所幫助。

整體框架

LOAM多牛逼就不用多說了，直接開始

先貼一下我詳細(xì)注釋的LOAM代碼，在這個(gè)版本的代碼上加入了我自己的理解。

我覺得最重要也是最惡心的一部分是其中的坐標(biāo)變換，在代碼里面真的看著頭大，所以先明確一下坐標(biāo)系(都是右手坐標(biāo)系)：

• IMU(IMU坐標(biāo)系imu)：x軸向前，y軸向左，z軸向上

• LIDAR(激光雷達(dá)坐標(biāo)系l)：x軸向前，y軸向左，z軸向上

• CAMERA(相機(jī)坐標(biāo)系，也可以理解為里程計(jì)坐標(biāo)系c)：z軸向前，x軸向左，y軸向上

• WORLD(世界坐標(biāo)系w，也叫全局坐標(biāo)系，與里程計(jì)第一幀init重合)：z軸向前，x軸向左，y軸向上

• MAP(地圖坐標(biāo)系map，一定程度上可以理解為里程計(jì)第一幀init)：z軸向前，x軸向左，y軸向上

坐標(biāo)變換約定： 為了清晰，變換矩陣的形式與《SLAM十四講中一樣》，即：表示B坐標(biāo)系相對(duì)于A坐標(biāo)系的變換，B中一個(gè)向量通過可以變換到A中的向量。 首先對(duì)照ros的節(jié)點(diǎn)圖和論文中提到的算法框架來看一下：

可以看到節(jié)點(diǎn)圖和論文中的框架是一一對(duì)應(yīng)的，這幾個(gè)模塊的功能如下：

• scanRegistration：對(duì)原始點(diǎn)云進(jìn)行預(yù)處理，計(jì)算曲率，提取特征點(diǎn)

• laserOdometry：對(duì)當(dāng)前sweep與上一次sweep進(jìn)行特征匹配，計(jì)算一個(gè)快速(10Hz)但粗略的位姿估計(jì)

• laserMapping：對(duì)當(dāng)前sweep與一個(gè)局部子圖進(jìn)行特征匹配，計(jì)算一個(gè)慢速(1Hz)比較精確的位姿估計(jì)

• transformMaintenance：對(duì)兩個(gè)模塊計(jì)算出的位姿進(jìn)行融合，得到最終的精確地位姿估計(jì)

本文介紹laserOdometry模塊，它相當(dāng)于SLAM的前端，它其實(shí)是一個(gè)scan-to-scan的過程，可以得到高頻率但精度略低的位姿變換，它的具體功能如下：

1. 接收特征點(diǎn)話題、全部點(diǎn)云話題、IMU話題，并保存到對(duì)應(yīng)的變量中

2. 將當(dāng)前sweep與上一次sweep進(jìn)行特征匹配，得到edge point匹配對(duì)應(yīng)的直線以及planar point匹配對(duì)應(yīng)的平面

3. 計(jì)算雅可比矩陣，使用高斯牛頓法(論文中說的是LM法)進(jìn)行優(yōu)化，得到估計(jì)出的相鄰兩幀的位姿變換

4. 累積位姿變換，并用IMU修正，得到當(dāng)前幀到初始幀的累積位姿變換

5. 發(fā)布話題并更新tf變換

一、main()函數(shù)以及回調(diào)函數(shù)

main()函數(shù)是很簡(jiǎn)單的，就是定義了一系列的發(fā)布者和訂閱者，訂閱了來自scanRegistration節(jié)點(diǎn)發(fā)布的話題；然后定義了一個(gè)tf發(fā)布器，發(fā)布當(dāng)前幀(/laser_odom)到初始幀(/camera_init)的坐標(biāo)變換；然后定義了一些列下面會(huì)用到的變量。

其中有6個(gè)訂閱者，分別看一下它們的回調(diào)函數(shù)。

int?main(int?argc,?char**?argv){??ros::init(argc,?argv,?"laserOdometry");??ros::NodeHandle?nh;??ros::Subscriber?subCornerPointsSharp?=?nh.subscribe<sensor_msgs::PointCloud2>?????????????????????????????????????????("/laser_cloud_sharp",?2,?laserCloudSharpHandler);??ros::Subscriber?subCornerPointsLessSharp?=?nh.subscribe<sensor_msgs::PointCloud2>?????????????????????????????????????????????("/laser_cloud_less_sharp",?2,?laserCloudLessSharpHandler);??ros::Subscriber?subSurfPointsFlat?=?nh.subscribe<sensor_msgs::PointCloud2>??????????????????????????????????????("/laser_cloud_flat",?2,?laserCloudFlatHandler);??ros::Subscriber?subSurfPointsLessFlat?=?nh.subscribe<sensor_msgs::PointCloud2>??????????????????????????????????????????("/laser_cloud_less_flat",?2,?laserCloudLessFlatHandler);??ros::Subscriber?subLaserCloudFullRes?=?nh.subscribe<sensor_msgs::PointCloud2>??????????????????????????????????????????("/velodyne_cloud_2",?2,?laserCloudFullResHandler);??ros::Subscriber?subImuTrans?=?nh.subscribe<sensor_msgs::PointCloud2>?????????????????????????????????("/imu_trans",?5,?imuTransHandler);??ros::Publisher?pubLaserCloudCornerLast?=?nh.advertise<sensor_msgs::PointCloud2>???????????????????????????????????????????("/laser_cloud_corner_last",?2);??ros::Publisher?pubLaserCloudSurfLast?=?nh.advertise<sensor_msgs::PointCloud2>?????????????????????????????????????????("/laser_cloud_surf_last",?2);??ros::Publisher?pubLaserCloudFullRes?=?nh.advertise<sensor_msgs::PointCloud2>?????????????????????????????????????????("/velodyne_cloud_3",?2);??ros::Publisher?pubLaserOdometry?=?nh.advertise<nav_msgs::Odometry>?("/laser_odom_to_init",?5);??nav_msgs::Odometry?laserOdometry;??laserOdometry.header.frame_id?=?"/camera_init";??laserOdometry.child_frame_id?=?"/laser_odom";??tf::TransformBroadcaster?tfBroadcaster;??tf::StampedTransform?laserOdometryTrans;??laserOdometryTrans.frame_id_?=?"/camera_init";??laserOdometryTrans.child_frame_id_?=?"/laser_odom";??std::vector<int>?pointSearchInd;//搜索到的點(diǎn)序??std::vector<float>?pointSearchSqDis;//搜索到的點(diǎn)平方距離??PointType?pointOri,?pointSel;?????????/*選中的特征點(diǎn)*/??PointType?tripod1,?tripod2,?tripod3;??/*特征點(diǎn)的對(duì)應(yīng)點(diǎn)*/??PointType?pointProj;??????????????????/*unused*/??PointType?coeff;??????????????????????/*直線或平面的系數(shù)*/??//退化標(biāo)志??bool?isDegenerate?=?false;??//P矩陣，預(yù)測(cè)矩陣，用來處理退化情況??cv::Mat?matP(6,?6,?CV_32F,?cv::Scalar::all(0));??int?frameCount?=?skipFrameNum;????//?skipFrameNum?=?1??ros::Rate?rate(100);??bool?status?=?ros::ok();

接收特征點(diǎn)的回調(diào)函數(shù)

下面這五個(gè)回調(diào)函數(shù)的作用和代碼結(jié)構(gòu)都類似，都是接收scanRegistration發(fā)布的話題，分別接收了edge point、less edge point、planar point、less planar point、full cloud point(按scanID排列的全部點(diǎn)云)。

對(duì)于接收到點(diǎn)云之后都是如下操作：

• 記錄時(shí)間戳

• 保存到相應(yīng)變量中

• 濾除無效點(diǎn)

• 將接收標(biāo)志設(shè)置為true

void?laserCloudSharpHandler(const?sensor_msgs::PointCloud2ConstPtr&?cornerPointsSharp2){??timeCornerPointsSharp?=?cornerPointsSharp2->header.stamp.toSec();??cornerPointsSharp->clear();??pcl::fromROSMsg(*cornerPointsSharp2,?*cornerPointsSharp);??std::vector<int>?indices;??pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp,?indices);??newCornerPointsSharp?=?true;}void?laserCloudLessSharpHandler(const?sensor_msgs::PointCloud2ConstPtr&?cornerPointsLessSharp2){??timeCornerPointsLessSharp?=?cornerPointsLessSharp2->header.stamp.toSec();??cornerPointsLessSharp->clear();??pcl::fromROSMsg(*cornerPointsLessSharp2,?*cornerPointsLessSharp);??std::vector<int>?indices;??pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp,?indices);??newCornerPointsLessSharp?=?true;}void?laserCloudFlatHandler(const?sensor_msgs::PointCloud2ConstPtr&?surfPointsFlat2){??timeSurfPointsFlat?=?surfPointsFlat2->header.stamp.toSec();??surfPointsFlat->clear();??pcl::fromROSMsg(*surfPointsFlat2,?*surfPointsFlat);??std::vector<int>?indices;??pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat,?indices);??newSurfPointsFlat?=?true;}void?laserCloudLessFlatHandler(const?sensor_msgs::PointCloud2ConstPtr&?surfPointsLessFlat2){??timeSurfPointsLessFlat?=?surfPointsLessFlat2->header.stamp.toSec();??surfPointsLessFlat->clear();??pcl::fromROSMsg(*surfPointsLessFlat2,?*surfPointsLessFlat);??std::vector<int>?indices;??pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat,?indices);??newSurfPointsLessFlat?=?true;}//接收全部點(diǎn)void?laserCloudFullResHandler(const?sensor_msgs::PointCloud2ConstPtr&?laserCloudFullRes2){??timeLaserCloudFullRes?=?laserCloudFullRes2->header.stamp.toSec();??laserCloudFullRes->clear();??pcl::fromROSMsg(*laserCloudFullRes2,?*laserCloudFullRes);??std::vector<int>?indices;??pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes,?indices);??newLaserCloudFullRes?=?true;}

接收/imu_trans消息這個(gè)回調(diào)函數(shù)主要是接受了scanRegistration中發(fā)布的自定義imu話題，包括當(dāng)前sweep點(diǎn)云數(shù)據(jù)的IMU起始角、終止角、由于加減速產(chǎn)生的位移和速度畸變，保存到相應(yīng)變量中。

//接收imu消息void?imuTransHandler(const?sensor_msgs::PointCloud2ConstPtr&?imuTrans2){??timeImuTrans?=?imuTrans2->header.stamp.toSec();??imuTrans->clear();??pcl::fromROSMsg(*imuTrans2,?*imuTrans);??//根據(jù)發(fā)來的消息提取imu信息??imuPitchStart?=?imuTrans->points[0].x;??imuYawStart?=?imuTrans->points[0].y;??imuRollStart?=?imuTrans->points[0].z;??imuPitchLast?=?imuTrans->points[1].x;??imuYawLast?=?imuTrans->points[1].y;??imuRollLast?=?imuTrans->points[1].z;??imuShiftFromStartX?=?imuTrans->points[2].x;??imuShiftFromStartY?=?imuTrans->points[2].y;??imuShiftFromStartZ?=?imuTrans->points[2].z;??imuVeloFromStartX?=?imuTrans->points[3].x;??imuVeloFromStartY?=?imuTrans->points[3].y;??imuVeloFromStartZ?=?imuTrans->points[3].z;??newImuTrans?=?true;}

二、特征匹配

接收到第一幀點(diǎn)云數(shù)據(jù)時(shí)，先進(jìn)行一次初始化，因?yàn)榈谝粠c(diǎn)云沒法匹配..

這里就是直接把這一幀點(diǎn)云發(fā)送給laserMapping節(jié)點(diǎn)。

??????//初始化：將第一個(gè)點(diǎn)云數(shù)據(jù)集發(fā)送給laserMapping,從下一個(gè)點(diǎn)云數(shù)據(jù)開始處理??????if?(!systemInited)?{????????//將cornerPointsLessSharp與laserCloudCornerLast交換,目的保存cornerPointsLessSharp的值到laserCloudCornerLast中下輪使用????????pcl::PointCloud<PointType>::Ptr?laserCloudTemp?=?cornerPointsLessSharp;????????cornerPointsLessSharp?=?laserCloudCornerLast;????????laserCloudCornerLast?=?laserCloudTemp;????????//將surfPointLessFlat與laserCloudSurfLast交換，目的保存surfPointsLessFlat的值到laserCloudSurfLast中下輪使用????????laserCloudTemp?=?surfPointsLessFlat;????????surfPointsLessFlat?=?laserCloudSurfLast;????????laserCloudSurfLast?=?laserCloudTemp;????????//使用上一幀的特征點(diǎn)構(gòu)建kd-tree????????kdtreeCornerLast->setInputCloud(laserCloudCornerLast);??//所有的邊沿點(diǎn)集合????????kdtreeSurfLast->setInputCloud(laserCloudSurfLast);??????//所有的平面點(diǎn)集合????????//將cornerPointsLessSharp和surfPointLessFlat點(diǎn)也即邊沿點(diǎn)和平面點(diǎn)分別發(fā)送給laserMapping????????sensor_msgs::PointCloud2?laserCloudCornerLast2;????????pcl::toROSMsg(*laserCloudCornerLast,?laserCloudCornerLast2);????????laserCloudCornerLast2.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);????????laserCloudCornerLast2.header.frame_id?=?"/camera";????????pubLaserCloudCornerLast.publish(laserCloudCornerLast2);????????sensor_msgs::PointCloud2?laserCloudSurfLast2;????????pcl::toROSMsg(*laserCloudSurfLast,?laserCloudSurfLast2);????????laserCloudSurfLast2.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);????????laserCloudSurfLast2.header.frame_id?=?"/camera";????????pubLaserCloudSurfLast.publish(laserCloudSurfLast2);????????//記住原點(diǎn)的翻滾角和俯仰角????????transformSum[0]?+=?imuPitchStart;???//?IMU的y方向????????transformSum[2]?+=?imuRollStart;????//?IMU的x方向????????systemInited?=?true;????????continue;??????}

在scanRegistration模塊中，有一個(gè)TransformToStartIMU()函數(shù)，上篇文章提到它的作用是：沒有對(duì)點(diǎn)云坐標(biāo)系進(jìn)行變換，第i個(gè)點(diǎn)云依然處在里程計(jì)坐標(biāo)系下的curr時(shí)刻坐標(biāo)系中，只是對(duì)點(diǎn)云的位置進(jìn)行調(diào)整。在這里推薦工坊新課：（第二期）徹底搞懂基于LOAM框架的3D激光SLAM：源碼剖析到算法優(yōu)化

而這個(gè)函數(shù)呢，就是要對(duì)點(diǎn)云的坐標(biāo)系進(jìn)行變換，變換到里程計(jì)坐標(biāo)系下的初始時(shí)刻start坐標(biāo)系中，用于與上一幀的點(diǎn)云數(shù)據(jù)進(jìn)行匹配。

在這個(gè)函數(shù)中，首先根據(jù)每個(gè)點(diǎn)強(qiáng)度值intensity，提取出scanRegistration中計(jì)算的插值系數(shù)，下面開始了！

首先要明確：transform中保存的變量是上一幀相對(duì)于當(dāng)前幀的變換，也就是

而這里也體現(xiàn)了勻速運(yùn)動(dòng)假設(shè)，因?yàn)槲覀冊(cè)谶@里使用transform變量時(shí)，還沒有對(duì)其進(jìn)行更新(迭代更新過程在特征匹配和非線性最小二乘優(yōu)化中才進(jìn)行)，所以這里使用的transform變量與上上幀相對(duì)于上一幀的變換相同，這也就是作者假設(shè)了每個(gè)掃描周期車輛都進(jìn)行了完全相同的運(yùn)動(dòng)，用數(shù)學(xué)公式表示如下：

那么問題如下：

現(xiàn)在我們已知的量是：當(dāng)前時(shí)刻坐標(biāo)系下保持imustart角的的點(diǎn)云，上一幀相對(duì)于當(dāng)前幀的變換，也就是，使用s進(jìn)行插值后有：。

需要求解的是當(dāng)前sweep初始時(shí)刻坐標(biāo)系的點(diǎn)云。

推導(dǎo)過程：

根據(jù)坐標(biāo)變換公式有：

而為YXZ變換順序(解釋：當(dāng)前幀相對(duì)于上一幀的變換順序?yàn)閆XY，呢么反過來，這里是上一幀相當(dāng)于當(dāng)前幀的變換，就變成了YXZ)，所以，代入得：

這就推出了和代碼一致的變換順序：先減去，然后繞z周旋轉(zhuǎn)-rz，繞x軸旋轉(zhuǎn)-rx，繞y軸旋轉(zhuǎn)-ry。

//將該次sweep中每個(gè)點(diǎn)通過插值，變換到初始時(shí)刻startvoid?TransformToStart(PointType?const?*?const?pi,?PointType?*?const?po){??//插值系數(shù)計(jì)算，云中每個(gè)點(diǎn)的相對(duì)時(shí)間/點(diǎn)云周期10??float?s?=?10?*?(pi->intensity?-?int(pi->intensity));??//線性插值：根據(jù)每個(gè)點(diǎn)在點(diǎn)云中的相對(duì)位置關(guān)系，乘以相應(yīng)的旋轉(zhuǎn)平移系數(shù)??//這里的transform數(shù)值上和上次一樣，這里體現(xiàn)了勻速運(yùn)動(dòng)模型，就是每次時(shí)間間隔下，相對(duì)于上一次sweep的變換都是一樣的??//而在使用該函數(shù)之前進(jìn)行了一次操作：transform[3]?-=?imuVeloFromStartX?*?scanPeriod;??//這個(gè)操作是在勻速模型的基礎(chǔ)上，去除了由于加減速造成的畸變????//?R_curr_start?=?R_YXZ??float?rx?=?s?*?transform[0];??float?ry?=?s?*?transform[1];??float?rz?=?s?*?transform[2];??float?tx?=?s?*?transform[3];??float?ty?=?s?*?transform[4];??float?tz?=?s?*?transform[5];??/*pi是在curr坐標(biāo)系下?p_curr，需要變換到當(dāng)前sweep的初始時(shí)刻start坐標(biāo)系下???*?現(xiàn)在有關(guān)系：p_curr?=?R_curr_start?*?p_start?+?t_curr_start???*?變換一下有：變換一下有：p_start?=?R_curr_start^{-1}?*?(p_curr?-?t_curr_start)?=?R_YXZ^{-1}?*?(p_curr?-?t_curr_start)???*?代入定義量：p_start?=?R_transform^{-1}?*?(p_curr?-?t_transform)???*?展開已知角：p_start?=?R_-ry?*?R_-rx?*?R_-rz?*?(p_curr?-?t_transform)???*/????//平移后繞z軸旋轉(zhuǎn)（-rz）??float?x1?=?cos(rz)?*?(pi->x?-?tx)?+?sin(rz)?*?(pi->y?-?ty);??float?y1?=?-sin(rz)?*?(pi->x?-?tx)?+?cos(rz)?*?(pi->y?-?ty);??float?z1?=?(pi->z?-?tz);??//繞x軸旋轉(zhuǎn)（-rx）??float?x2?=?x1;??float?y2?=?cos(rx)?*?y1?+?sin(rx)?*?z1;??float?z2?=?-sin(rx)?*?y1?+?cos(rx)?*?z1;??//繞y軸旋轉(zhuǎn)（-ry）??po->x?=?cos(ry)?*?x2?-?sin(ry)?*?z2;??po->y?=?y2;??po->z?=?sin(ry)?*?x2?+?cos(ry)?*?z2;??po->intensity?=?pi->intensity;}

尋找匹配直線：

將該次sweep中每個(gè)點(diǎn)通過插值，變換到初始時(shí)刻start后，迭代五次時(shí)，重新查找最近點(diǎn)，相當(dāng)于每個(gè)匹配迭代優(yōu)化4次，A-LOAM代碼中的Ceres代碼的最大迭代次數(shù)為4。

然后先試用PCL中的KD-tree查找當(dāng)前點(diǎn)在上一幀中的最近鄰點(diǎn)，對(duì)應(yīng)論文中提到的j點(diǎn)，最近鄰在上一幀中的索引保存在pointSearchInd中，距離保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd對(duì)應(yīng)論文中的j點(diǎn)，minPointInd2對(duì)應(yīng)論文中的l點(diǎn)

int closestPointInd = -1, minPointInd2 = -1;

如果查找到了最近鄰的j點(diǎn)，就按照論文中所說，需要查找上一幀中，與j線號(hào)scanID相鄰且j的最近鄰點(diǎn)l，j和l構(gòu)成與待匹配點(diǎn)i的匹配直線edge。

下面這兩個(gè)for循環(huán)做了這么一件事情：向索引增大的方向查找(scanID>=j點(diǎn)的scanID)，如果查找到了相鄰幀有距離更小的點(diǎn)，就更新最小距離和索引；然后向索引減小的方向查找(scanID<=j點(diǎn)的scanID)，如果查找到了相鄰幀有距離更小的點(diǎn)，就更新最小距離和索引。然后將每個(gè)待匹配點(diǎn)對(duì)應(yīng)的j和l點(diǎn)的索引保存在pointSearchCornerInd1[]和pointSearchCornerInd2[]中。

計(jì)算直線參數(shù)：

當(dāng)找到了j點(diǎn)和l點(diǎn)，就可以進(jìn)行特征匹配，計(jì)算匹配直線的系數(shù)，對(duì)應(yīng)論文中的公式為：

這個(gè)公式的分子是i和j構(gòu)成的向量與i和l構(gòu)成的向量的叉乘，得到了一個(gè)與ijl三點(diǎn)構(gòu)成平面垂直的向量，而叉乘取模其實(shí)就是一個(gè)平行四邊形面積；分母是j和l構(gòu)成的向量，取模后為平行四邊形面積的底，二者相除就得到了i到直線jl的距離，下面是圖示：

代碼中變量代表的含義：

• x0：i點(diǎn)

• x1：j點(diǎn)

• x2：l點(diǎn)

• a012：論文中殘差的分子(兩個(gè)向量的叉乘)

• la、lb、lc：i點(diǎn)到j(luò)l線垂線方向的向量(jl方向的單位向量與ijl平面的單位法向量的叉乘得到)在xyz三個(gè)軸上的分量

• ld2：點(diǎn)到直線的距離，即

下面這個(gè)s可以看做一個(gè)權(quán)重，距離越大權(quán)重越小，距離越小權(quán)重越大，得到的權(quán)重范圍<=1,其實(shí)就是在求解非線性最小二乘問題時(shí)的核函數(shù)，s的本質(zhì)定義如下：

最后將權(quán)重大(>0.1)的，即距離比較小，且距離不為零的點(diǎn)放入laserCloudOri，對(duì)應(yīng)的直線系數(shù)放入coeffSel。

????????//論文中提到的Levenberg-Marquardt算法(L-M?method)，其實(shí)是高斯牛頓算法????????for?(int?iterCount?=?0;?iterCount?<?25;?iterCount++)?{??????????laserCloudOri->clear();??????????coeffSel->clear();??????????//處理當(dāng)前點(diǎn)云中的曲率最大的特征點(diǎn),從上個(gè)點(diǎn)云中曲率比較大的特征點(diǎn)中找兩個(gè)最近距離點(diǎn)，一個(gè)點(diǎn)使用kd-tree查找，另一個(gè)根據(jù)找到的點(diǎn)在其相鄰線找另外一個(gè)最近距離的點(diǎn)??????????for?(int?i?=?0;?i?<?cornerPointsSharpNum;?i++)?{????????????//將該次sweep中每個(gè)點(diǎn)通過插值，變換到初始時(shí)刻start????????????TransformToStart(&cornerPointsSharp->points[i],?&pointSel);????????????//每迭代五次，重新查找最近點(diǎn)，相當(dāng)于每個(gè)匹配迭代優(yōu)化4次，ALOAM代碼中的Ceres代碼的最大迭代次數(shù)為4????????????if?(iterCount?%?5?==?0)?{??????????????std::vector<int>?indices;??????????????pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast,?indices);??????????????//kd-tree查找一個(gè)最近距離點(diǎn)，邊沿點(diǎn)未經(jīng)過體素柵格濾波，一般邊沿點(diǎn)本來就比較少，不做濾波??????????????kdtreeCornerLast->nearestKSearch(pointSel,?1,?pointSearchInd,?pointSearchSqDis);??????????????int?closestPointInd?=?-1,?minPointInd2?=?-1;??????????????//closestPointInd對(duì)應(yīng)論文中的j點(diǎn)，minPointInd2對(duì)應(yīng)論文中的l點(diǎn)????????????????????????????//循環(huán)是尋找相鄰線最近點(diǎn)l??????????????//再次提醒：velodyne是2度一線，scanID相鄰并不代表線號(hào)相鄰，相鄰線度數(shù)相差2度，也即線號(hào)scanID相差2??????????????if?(pointSearchSqDis[0]?<?25)?{//找到的最近點(diǎn)距離的確很近的話????????????????closestPointInd?=?pointSearchInd[0];????????????????//提取最近點(diǎn)線號(hào)????????????????int?closestPointScan?=?int(laserCloudCornerLast->points[closestPointInd].intensity);????????????????float?pointSqDis,?minPointSqDis2?=?25;//初始門檻值25米，可大致過濾掉scanID相鄰，但實(shí)際線不相鄰的值????????????????//尋找距離目標(biāo)點(diǎn)最近距離的平方和最小的點(diǎn)????????????????for?(int?j?=?closestPointInd?+?1;?j?<?cornerPointsSharpNum;?j++)?{//向scanID增大的方向查找??????????????????if?(int(laserCloudCornerLast->points[j].intensity)?>?closestPointScan?+?2.5)?{//非相鄰線????????????????????break;??????????????????}...

尋找匹配平面：

將該次sweep中每個(gè)點(diǎn)通過插值，變換到初始時(shí)刻start后，迭代五次時(shí)，重新查找最近點(diǎn)，相當(dāng)于每個(gè)匹配迭代優(yōu)化4次，A-LOAM代碼中的Ceres代碼的最大迭代次數(shù)為4。

然后先試用PCL中的KD-tree查找當(dāng)前點(diǎn)在上一幀中的最近鄰點(diǎn)，對(duì)應(yīng)論文中提到的j點(diǎn)，最近鄰在上一幀中的索引保存在pointSearchInd中，距離保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd對(duì)應(yīng)論文中的j點(diǎn)、minPointInd2對(duì)應(yīng)論文中的l點(diǎn)、minPointInd3對(duì)應(yīng)論文中的m點(diǎn)。

int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;

如果查找到了最近鄰的j點(diǎn)，就按照論文中所說，需要查找上一幀中，與j線號(hào)scanID相同且j的最近鄰點(diǎn)l，然后查找一個(gè)與j線號(hào)scanID相鄰且最近鄰的點(diǎn)m。

下面這兩個(gè)for循環(huán)做了這么一件事情：向索引增大的方向查找(scanID>=j點(diǎn)的scanID)，如果查找到了相鄰線號(hào)的點(diǎn)j和相同線號(hào)的點(diǎn)m，有距離更小的點(diǎn)，就更新最小距離和索引；然后向索引減小的方向查找(scanID<=j點(diǎn)的scanID)，如果查找到了相鄰線號(hào)的點(diǎn)j和相同線號(hào)的點(diǎn)m，就更新最小距離和索引。然后將每個(gè)待匹配點(diǎn)對(duì)應(yīng)的j、l點(diǎn)、m點(diǎn)的索引保存在pointSearchSurfInd1[]、pointSearchSurfInd2[]和pointSearchSurfInd3[]中。

計(jì)算平面參數(shù)：

當(dāng)找到了j點(diǎn)、l點(diǎn)和m點(diǎn)，就可以進(jìn)行特征匹配，計(jì)算匹配直線的系數(shù)，對(duì)應(yīng)論文中的公式為：

這個(gè)公式的分母是一個(gè)lj向量和mj向量叉乘的形式，表示的是jlm平面的法向量的模；而分子是ij向量與jlm平面的法向量的向量積，向量積有一個(gè)意義是一個(gè)向量在另一個(gè)向量方向上的投影，這里就表示ij向量在jlm平面的法向量方向上的投影，也就是i到平面的距離，下面是圖示：

代碼中變量代表的含義：

• tripod1：j點(diǎn)

• tripod2：l點(diǎn)

• tripod3：m點(diǎn)

• pa、pb、pc：jlm平面的法向量在xyz三個(gè)軸上的分量，也可以理解為平面方程Ax+By+Cz+D=0的ABC系數(shù)

• pd：為平面方程的D系數(shù)

• ps：法向量的模

• pd2：點(diǎn)到平面的距離(將平面方程的系數(shù)歸一化后，代入點(diǎn)的坐標(biāo)，其實(shí)就是點(diǎn)到平面距離公式，即可得到點(diǎn)到平面的距離)

下面這個(gè)s可以看做一個(gè)權(quán)重，對(duì)應(yīng)于論文中的這一部分

距離越大權(quán)重越小，距離越小權(quán)重越大，得到的權(quán)重范圍<=1,其實(shí)就是在求解非線性最小二乘問題時(shí)的核函數(shù)，s的本質(zhì)定義如下：

最后將權(quán)重大(>0.1)的，即距離比較小，且距離不為零的點(diǎn)放入laserCloudOri，對(duì)應(yīng)的平面系數(shù)放入coeffSel。

??????????//對(duì)本次接收到的曲率最小的點(diǎn),從上次接收到的點(diǎn)云曲率比較小的點(diǎn)中找三點(diǎn)組成平面，一個(gè)使用kd-tree查找，另外一個(gè)在同一線上查找滿足要求的，第三個(gè)在不同線上查找滿足要求的??????????for?(int?i?=?0;?i?<?surfPointsFlatNum;?i++)?{????????????//將該次sweep中每個(gè)點(diǎn)通過插值，變換到初始時(shí)刻start????????????TransformToStart(&surfPointsFlat->points[i],?&pointSel);????????????if?(iterCount?%?5?==?0)?{??????????????//kd-tree最近點(diǎn)查找，在經(jīng)過體素柵格濾波之后的平面點(diǎn)中查找，一般平面點(diǎn)太多，濾波后最近點(diǎn)查找數(shù)據(jù)量小??????????????kdtreeSurfLast->nearestKSearch(pointSel,?1,?pointSearchInd,?pointSearchSqDis);????????????????????????????//closestPointInd對(duì)應(yīng)論文中的j、minPointInd2對(duì)應(yīng)論文中的l、minPointInd3對(duì)應(yīng)論文中的m??????????????int?closestPointInd?=?-1,?minPointInd2?=?-1,?minPointInd3?=?-1;??????????????if?(pointSearchSqDis[0]?<?25)?{????????????????closestPointInd?=?pointSearchInd[0];????????????????int?closestPointScan?=?int(laserCloudSurfLast->points[closestPointInd].intensity);????????????????float?pointSqDis,?minPointSqDis2?=?25,?minPointSqDis3?=?25;????????????????for?(int?j?=?closestPointInd?+?1;?j?<?surfPointsFlatNum;?j++)?{?//向上查找??????????????????if?(int(laserCloudSurfLast->points[j].intensity)?>?closestPointScan?+?2.5)?{????????????????????break;??????????????????}...

三、高斯牛頓優(yōu)化

在代碼中，作者是純手推的高斯牛頓算法，這種方式相比于使用Ceres等工具，會(huì)提高運(yùn)算速度，但是計(jì)算雅克比矩陣比較麻煩，需要清晰的思路和扎實(shí)的數(shù)學(xué)功底，下面我們一起來推導(dǎo)一下。

以edge point匹配為例，planar point是一樣的。

設(shè)誤差函數(shù)(點(diǎn)到直線的距離)為：

其中，X為待優(yōu)化變量，也就是transform[6]中存儲(chǔ)的變量，表示3軸旋轉(zhuǎn)rx、ry、rz和3軸平移量tx、ty、tz；D()表示兩點(diǎn)之間的距離，其計(jì)算公式為：

表示start時(shí)刻坐標(biāo)系下待匹配點(diǎn)i；表示start時(shí)刻坐標(biāo)系下點(diǎn)i到直線jl的垂點(diǎn)；另外根據(jù)之前TransformToStart()函數(shù)推導(dǎo)過的坐標(biāo)變換有：

根據(jù)鏈?zhǔn)椒▌t，f(x)對(duì)X求導(dǎo)有：

對(duì)其中每一項(xiàng)進(jìn)行計(jì)算：

D對(duì)求導(dǎo)的結(jié)果其實(shí)就是 進(jìn)行歸一化后的點(diǎn)到直線向量，它在xyz三個(gè)軸的分量就是前面求解出來的la、lb、lc變量。

用上面的結(jié)果，分別對(duì)rx,ry,rz,tx,ty,tz求導(dǎo)，將得到的結(jié)果(3x6矩陣)再與D對(duì)求導(dǎo)的結(jié)果(1x3矩陣)相乘，就可以得到代碼中顯示的結(jié)果(1x6矩陣)，分別賦值到matA的6個(gè)位置，matB是殘差項(xiàng)。

最后使用opencv的QR分解求解增量X即可。

??????????//滿足要求的特征點(diǎn)至少10個(gè)，特征匹配數(shù)量太少棄用此幀數(shù)據(jù)，不再進(jìn)行優(yōu)化步驟??????????if?(pointSelNum?<?10)?{????????????continue;??????????}??????????//?Hx=g??????????cv::Mat?matA(pointSelNum,?6,?CV_32F,?cv::Scalar::all(0));?????//?J??????????cv::Mat?matAt(6,?pointSelNum,?CV_32F,?cv::Scalar::all(0));????//?J^T??????????cv::Mat?matAtA(6,?6,?CV_32F,?cv::Scalar::all(0));?????????????//?H?=?J^T?*?J??????????cv::Mat?matB(pointSelNum,?1,?CV_32F,?cv::Scalar::all(0));?????//?e??????????cv::Mat?matAtB(6,?1,?CV_32F,?cv::Scalar::all(0));?????????????//?g?=?-J?*?e??????????cv::Mat?matX(6,?1,?CV_32F,?cv::Scalar::all(0));???????????????//?x??????????//計(jì)算matA,matB矩陣??????????for?(int?i?=?0;?i?<?pointSelNum;?i++)?{????????????pointOri?=?laserCloudOri->points[i];????????????coeff?=?coeffSel->points[i];????????????float?s?=?1;????????????float?srx?=?sin(s?*?transform[0]);????????????float?crx?=?cos(s?*?transform[0]);????????????float?sry?=?sin(s?*?transform[1]);????????????float?cry?=?cos(s?*?transform[1]);????????????float?srz?=?sin(s?*?transform[2]);????????????float?crz?=?cos(s?*?transform[2]);????????????float?tx?=?s?*?transform[3];????????????float?ty?=?s?*?transform[4];????????????float?tz?=?s?*?transform[5];????????????float?arx?=?(-s*crx*sry*srz*pointOri.x?+?s*crx*crz*sry*pointOri.y?+?s*srx*sry*pointOri.z???????????????????????+?s*tx*crx*sry*srz?-?s*ty*crx*crz*sry?-?s*tz*srx*sry)?*?coeff.x??????????????????????+?(s*srx*srz*pointOri.x?-?s*crz*srx*pointOri.y?+?s*crx*pointOri.z??????????????????????+?s*ty*crz*srx?-?s*tz*crx?-?s*tx*srx*srz)?*?coeff.y??????????????????????+?(s*crx*cry*srz*pointOri.x?-?s*crx*cry*crz*pointOri.y?-?s*cry*srx*pointOri.z??????????????????????+?s*tz*cry*srx?+?s*ty*crx*cry*crz?-?s*tx*crx*cry*srz)?*?coeff.z;????????????float?ary?=?((-s*crz*sry?-?s*cry*srx*srz)*pointOri.x???????????????????????+?(s*cry*crz*srx?-?s*sry*srz)*pointOri.y?-?s*crx*cry*pointOri.z???????????????????????+?tx*(s*crz*sry?+?s*cry*srx*srz)?+?ty*(s*sry*srz?-?s*cry*crz*srx)???????????????????????+?s*tz*crx*cry)?*?coeff.x??????????????????????+?((s*cry*crz?-?s*srx*sry*srz)*pointOri.x???????????????????????+?(s*cry*srz?+?s*crz*srx*sry)*pointOri.y?-?s*crx*sry*pointOri.z??????????????????????+?s*tz*crx*sry?-?ty*(s*cry*srz?+?s*crz*srx*sry)???????????????????????-?tx*(s*cry*crz?-?s*srx*sry*srz))?*?coeff.z;????????????float?arz?=?((-s*cry*srz?-?s*crz*srx*sry)*pointOri.x?+?(s*cry*crz?-?s*srx*sry*srz)*pointOri.y??????????????????????+?tx*(s*cry*srz?+?s*crz*srx*sry)?-?ty*(s*cry*crz?-?s*srx*sry*srz))?*?coeff.x??????????????????????+?(-s*crx*crz*pointOri.x?-?s*crx*srz*pointOri.y??????????????????????+?s*ty*crx*srz?+?s*tx*crx*crz)?*?coeff.y??????????????????????+?((s*cry*crz*srx?-?s*sry*srz)*pointOri.x?+?(s*crz*sry?+?s*cry*srx*srz)*pointOri.y??????????????????????+?tx*(s*sry*srz?-?s*cry*crz*srx)?-?ty*(s*crz*sry?+?s*cry*srx*srz))?*?coeff.z;????????????float?atx?=?-s*(cry*crz?-?srx*sry*srz)?*?coeff.x?+?s*crx*srz?*?coeff.y???????????????????????-?s*(crz*sry?+?cry*srx*srz)?*?coeff.z;??????????????float?aty?=?-s*(cry*srz?+?crz*srx*sry)?*?coeff.x?-?s*crx*crz?*?coeff.y???????????????????????-?s*(sry*srz?-?cry*crz*srx)?*?coeff.z;??????????????float?atz?=?s*crx*sry?*?coeff.x?-?s*srx?*?coeff.y?-?s*crx*cry?*?coeff.z;????????????float?d2?=?coeff.intensity;????????????matA.at<float>(i,?0)?=?arx;????????????matA.at<float>(i,?1)?=?ary;????????????matA.at<float>(i,?2)?=?arz;????????????matA.at<float>(i,?3)?=?atx;????????????matA.at<float>(i,?4)?=?aty;????????????matA.at<float>(i,?5)?=?atz;????????????matB.at<float>(i,?0)?=?-0.05?*?d2;??????????}??????????cv::transpose(matA,?matAt);??????????matAtA?=?matAt?*?matA;??????????matAtB?=?matAt?*?matB;??????????//求解matAtA?*?matX?=?matAtB??????????cv::solve(matAtA,?matAtB,?matX,?cv::DECOMP_QR);

代碼中使用的退化處理在Ji Zhang的這篇論文(《On Degeneracy of Optimization-based State Estimation Problems》)中有詳細(xì)論述。

簡(jiǎn)單來說，作者認(rèn)為退化只可能發(fā)生在第一次迭代時(shí)，先對(duì)矩陣求特征值，然后將得到的特征值與閾值(代碼中為10)進(jìn)行比較，如果小于閾值則認(rèn)為該特征值對(duì)應(yīng)的特征向量方向發(fā)生了退化，將對(duì)應(yīng)的特征向量置為0，然后按照論文中所述，計(jì)算一個(gè)P矩陣：

是原來的特征向量矩陣，是將退化方向置為0后的特征向量矩陣，然后用P矩陣與原來得到的解相乘，得到最終解：

??????????if?(iterCount?==?0)?{????????????//特征值1*6矩陣????????????cv::Mat?matE(1,?6,?CV_32F,?cv::Scalar::all(0));????????????//特征向量6*6矩陣????????????cv::Mat?matV(6,?6,?CV_32F,?cv::Scalar::all(0));????????????cv::Mat?matV2(6,?6,?CV_32F,?cv::Scalar::all(0));????????????//求解特征值/特征向量????????????cv::eigen(matAtA,?matE,?matV);????????????matV.copyTo(matV2);????????????isDegenerate?=?false;????????????//特征值取值門檻????????????float?eignThre[6]?=?{10,?10,?10,?10,?10,?10};????????????for?(int?i?=?5;?i?>=?0;?i--)?{//從小到大查找??????????????if?(matE.at<float>(0,?i)?<?eignThre[i])?{//特征值太小，則認(rèn)為處在退化環(huán)境中，發(fā)生了退化????????????????for?(int?j?=?0;?j?<?6;?j++)?{//對(duì)應(yīng)的特征向量置為0??????????????????matV2.at<float>(i,?j)?=?0;????????????????}????????????????isDegenerate?=?true;??????????????}?else?{????????????????break;??????????????}????????????}????????????//計(jì)算P矩陣????????????matP?=?matV.inv()?*?matV2;??//?論文中對(duì)應(yīng)的Vf^-1?*?V_u`??????????}??????????if?(isDegenerate)?{//如果發(fā)生退化，只使用預(yù)測(cè)矩陣P計(jì)算????????????cv::Mat?matX2(6,?1,?CV_32F,?cv::Scalar::all(0));????????????matX.copyTo(matX2);????????????matX?=?matP?*?matX2;??????????}

最后進(jìn)行迭代更新，如果更新量很小則終止迭代。

??????????//累加每次迭代的旋轉(zhuǎn)平移量??????????transform[0]?+=?matX.at<float>(0,?0);??????????transform[1]?+=?matX.at<float>(1,?0);??????????transform[2]?+=?matX.at<float>(2,?0);??????????transform[3]?+=?matX.at<float>(3,?0);??????????transform[4]?+=?matX.at<float>(4,?0);??????????transform[5]?+=?matX.at<float>(5,?0);??????????for(int?i=0;?i<6;?i++){????????????if(isnan(transform[i]))//判斷是否非數(shù)字??????????????transform[i]=0;??????????}??????????//計(jì)算旋轉(zhuǎn)平移量，如果很小就停止迭代??????????float?deltaR?=?sqrt(??????????????????????????????pow(rad2deg(matX.at<float>(0,?0)),?2)?+??????????????????????????????pow(rad2deg(matX.at<float>(1,?0)),?2)?+??????????????????????????????pow(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));??????????if?(deltaR?<?0.1?&&?deltaT?<?0.1)?{//迭代終止條件????????????break;??????????}

再次總結(jié)一下整個(gè)流程：

1. 一共迭代25次，每次分為edge point和planar point兩個(gè)處理過程

2. 每迭代5次時(shí)，重新尋找最近點(diǎn)

3. 其他情況時(shí)，根據(jù)找到的最近點(diǎn)和待匹配點(diǎn)，計(jì)算匹配得到的直線和平面方程

4. 根據(jù)計(jì)算匹配得到的直線和平面方程，計(jì)算雅可比矩陣，并求解迭代增量

5. 如果是第一次迭代，判斷是否發(fā)生退化

6. 更新迭代增量，并判斷終止條件

四、發(fā)布話題和坐標(biāo)變換

這一部分首先獎(jiǎng)transformSum更新為修正后的值，然后將歐拉角轉(zhuǎn)換成四元數(shù)，發(fā)布里程計(jì)話題、廣播tf坐標(biāo)變換，然后將點(diǎn)云的曲率比較大和比較小的點(diǎn)投影到掃描結(jié)束位置；如果當(dāng)前幀特征點(diǎn)數(shù)量足夠多，就將其插入到KD-tree中用于下一次特征匹配；然后發(fā)布話題，發(fā)布的話題有：

• /laser_cloud_corner_last：曲率較大的點(diǎn)--less edge point

• /laser_cloud_surf_last：曲率較小的點(diǎn)--less planar point

• /velodyne_cloud_3：全部點(diǎn)云--full cloud point

• /laser_odom_to_init：里程計(jì)坐標(biāo)系下，當(dāng)前時(shí)刻end相對(duì)于初始時(shí)刻的坐標(biāo)變換

//得到世界坐標(biāo)系下的轉(zhuǎn)移矩陣??????transformSum[0]?=?rx;??????transformSum[1]?=?ry;??????transformSum[2]?=?rz;??????transformSum[3]?=?tx;??????transformSum[4]?=?ty;??????transformSum[5]?=?tz;??????//歐拉角轉(zhuǎn)換成四元數(shù)??????geometry_msgs::Quaternion?geoQuat?=?tf::createQuaternionMsgFromRollPitchYaw(rz,?-rx,?-ry);??????//publish四元數(shù)和平移量??????laserOdometry.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);??????laserOdometry.pose.pose.orientation.x?=?-geoQuat.y;??????laserOdometry.pose.pose.orientation.y?=?-geoQuat.z;??????laserOdometry.pose.pose.orientation.z?=?geoQuat.x;??????laserOdometry.pose.pose.orientation.w?=?geoQuat.w;??????laserOdometry.pose.pose.position.x?=?tx;??????laserOdometry.pose.pose.position.y?=?ty;??????laserOdometry.pose.pose.position.z?=?tz;??????pubLaserOdometry.publish(laserOdometry);??????//廣播新的平移旋轉(zhuǎn)之后的坐標(biāo)系(rviz)??????laserOdometryTrans.stamp_?=?ros::Time().fromSec(timeSurfPointsLessFlat);??????laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y,?-geoQuat.z,?geoQuat.x,?geoQuat.w));??????laserOdometryTrans.setOrigin(tf::Vector3(tx,?ty,?tz));??????tfBroadcaster.sendTransform(laserOdometryTrans);??????//對(duì)點(diǎn)云的曲率比較大和比較小的點(diǎn)投影到掃描結(jié)束位置??????int?cornerPointsLessSharpNum?=?cornerPointsLessSharp->points.size();??????for?(int?i?=?0;?i?<?cornerPointsLessSharpNum;?i++)?{????????TransformToEnd(&cornerPointsLessSharp->points[i],?&cornerPointsLessSharp->points[i]);??????}??????int?surfPointsLessFlatNum?=?surfPointsLessFlat->points.size();??????for?(int?i?=?0;?i?<?surfPointsLessFlatNum;?i++)?{????????TransformToEnd(&surfPointsLessFlat->points[i],?&surfPointsLessFlat->points[i]);??????}??????frameCount++;??????//點(diǎn)云全部點(diǎn)，每間隔一個(gè)點(diǎn)云數(shù)據(jù)相對(duì)點(diǎn)云最后一個(gè)點(diǎn)進(jìn)行畸變校正??????if?(frameCount?>=?skipFrameNum?+?1)?{?????//?skipFrameNum?=?1????????int?laserCloudFullResNum?=?laserCloudFullRes->points.size();????????for?(int?i?=?0;?i?<?laserCloudFullResNum;?i++)?{??????????TransformToEnd(&laserCloudFullRes->points[i],?&laserCloudFullRes->points[i]);????????}??????}??????//畸變校正之后的點(diǎn)作為last點(diǎn)保存等下個(gè)點(diǎn)云進(jìn)來進(jìn)行匹配??????pcl::PointCloud<PointType>::Ptr?laserCloudTemp?=?cornerPointsLessSharp;??????cornerPointsLessSharp?=?laserCloudCornerLast;??????laserCloudCornerLast?=?laserCloudTemp;??????laserCloudTemp?=?surfPointsLessFlat;??????surfPointsLessFlat?=?laserCloudSurfLast;??????laserCloudSurfLast?=?laserCloudTemp;??????laserCloudCornerLastNum?=?laserCloudCornerLast->points.size();??????laserCloudSurfLastNum?=?laserCloudSurfLast->points.size();??????//點(diǎn)足夠多就構(gòu)建kd-tree，否則棄用此幀，沿用上一幀數(shù)據(jù)的kd-tree??????if?(laserCloudCornerLastNum?>?10?&&?laserCloudSurfLastNum?>?100)?{????????kdtreeCornerLast->setInputCloud(laserCloudCornerLast);????????kdtreeSurfLast->setInputCloud(laserCloudSurfLast);??????}??????//按照跳幀數(shù)publich邊沿點(diǎn)，平面點(diǎn)以及全部點(diǎn)給laserMapping(每隔一幀發(fā)一次)??????if?(frameCount?>=?skipFrameNum?+?1)?{????????frameCount?=?0;????????sensor_msgs::PointCloud2?laserCloudCornerLast2;????????pcl::toROSMsg(*laserCloudCornerLast,?laserCloudCornerLast2);????????laserCloudCornerLast2.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);????????laserCloudCornerLast2.header.frame_id?=?"/camera";????????pubLaserCloudCornerLast.publish(laserCloudCornerLast2);????????sensor_msgs::PointCloud2?laserCloudSurfLast2;????????pcl::toROSMsg(*laserCloudSurfLast,?laserCloudSurfLast2);????????laserCloudSurfLast2.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);????????laserCloudSurfLast2.header.frame_id?=?"/camera";????????pubLaserCloudSurfLast.publish(laserCloudSurfLast2);????????sensor_msgs::PointCloud2?laserCloudFullRes3;????????pcl::toROSMsg(*laserCloudFullRes,?laserCloudFullRes3);????????laserCloudFullRes3.header.stamp?=?ros::Time().fromSec(timeSurfPointsLessFlat);????????laserCloudFullRes3.header.frame_id?=?"/camera";????????pubLaserCloudFullRes.publish(laserCloudFullRes3);??????}????}????status?=?ros::ok();????rate.sleep();??}

這里對(duì)應(yīng)于論文中體到的，變換到下圖所示的將變換到。

在勻速運(yùn)動(dòng)假設(shè)的前提下，從Start時(shí)刻到End時(shí)刻，點(diǎn)云都將保持imuRPYStart的位置姿態(tài)，然后對(duì)其中里程計(jì)坐標(biāo)系下curr時(shí)刻的點(diǎn)進(jìn)行以下操作：

1.首先進(jìn)行了一個(gè)TransformToStart()函數(shù)的過程，將當(dāng)前點(diǎn)變換到里程計(jì)坐標(biāo)系下start時(shí)刻坐標(biāo)系下，得到x3、y3、z3：

2.然后變換到里程計(jì)坐標(biāo)系下end時(shí)刻坐標(biāo)系下，再次提醒，得到x6、y6、z6：

但是事實(shí)上，由于加減速情況的存在，會(huì)產(chǎn)生運(yùn)動(dòng)畸變，這就導(dǎo)致勻速運(yùn)動(dòng)假設(shè)不再成立，也就是End時(shí)刻的imuRPY角與Start時(shí)刻的imuRPY角不相等，需要使用IMU去除畸變。

3.上面的過程，總體上看的結(jié)果就是將所有點(diǎn)保持imuRPYStrat的姿態(tài)，搬運(yùn)到了雷達(dá)坐標(biāo)系下的end時(shí)刻，由于運(yùn)動(dòng)畸變的存在，下面要使用IMU進(jìn)行去畸變，首先將所有點(diǎn)轉(zhuǎn)換到世界坐標(biāo)系下，但仍然是里程計(jì)坐標(biāo)系的end時(shí)刻，只是使用IMU進(jìn)行了修正：

4.然后將所有點(diǎn)通過測(cè)量得到的imuRPYLast變換到全局(w)坐標(biāo)系下的當(dāng)前幀終止時(shí)刻，并且對(duì)應(yīng)了imuRPYLast姿態(tài)角：

我理解的這個(gè)過程如下：

//將上一幀點(diǎn)云中的點(diǎn)相對(duì)結(jié)束位置去除因勻速運(yùn)動(dòng)產(chǎn)生的畸變，效果相當(dāng)于得到在點(diǎn)云掃描結(jié)束位置靜止掃描得到的點(diǎn)云void?TransformToEnd(PointType?const?*?const?pi,?PointType?*?const?po){??//插值系數(shù)計(jì)算??float?s?=?10?*?(pi->intensity?-?int(pi->intensity));??//R_curr_start??float?rx?=?s?*?transform[0];??float?ry?=?s?*?transform[1];??float?rz?=?s?*?transform[2];??float?tx?=?s?*?transform[3];??float?ty?=?s?*?transform[4];??float?tz?=?s?*?transform[5];??//將點(diǎn)curr系下的點(diǎn)，變換到初始時(shí)刻start??//平移后繞z軸旋轉(zhuǎn)（-rz）??float?x1?=?cos(rz)?*?(pi->x?-?tx)?+?sin(rz)?*?(pi->y?-?ty);??float?y1?=?-sin(rz)?*?(pi->x?-?tx)?+?cos(rz)?*?(pi->y?-?ty);??float?z1?=?(pi->z?-?tz);??//繞x軸旋轉(zhuǎn)（-rx）??float?x2?=?x1;??float?y2?=?cos(rx)?*?y1?+?sin(rx)?*?z1;??float?z2?=?-sin(rx)?*?y1?+?cos(rx)?*?z1;??//繞y軸旋轉(zhuǎn)（-ry）??float?x3?=?cos(ry)?*?x2?-?sin(ry)?*?z2;??float?y3?=?y2;??float?z3?=?sin(ry)?*?x2?+?cos(ry)?*?z2;//求出了相對(duì)于起始點(diǎn)校正的坐標(biāo)??//R_end_start?=?R_YXZ??rx?=?transform[0];??ry?=?transform[1];??rz?=?transform[2];??tx?=?transform[3];??ty?=?transform[4];??tz?=?transform[5];??//變換到end坐標(biāo)系??//繞y軸旋轉(zhuǎn)（ry）??float?x4?=?cos(ry)?*?x3?+?sin(ry)?*?z3;??float?y4?=?y3;??float?z4?=?-sin(ry)?*?x3?+?cos(ry)?*?z3;??//繞x軸旋轉(zhuǎn)（rx）??float?x5?=?x4;??float?y5?=?cos(rx)?*?y4?-?sin(rx)?*?z4;??float?z5?=?sin(rx)?*?y4?+?cos(rx)?*?z4;??//繞z軸旋轉(zhuǎn)（rz），再平移??float?x6?=?cos(rz)?*?x5?-?sin(rz)?*?y5?+?tx;??float?y6?=?sin(rz)?*?x5?+?cos(rz)?*?y5?+?ty;??float?z6?=?z5?+?tz;??//使用IMU去除由于加減速產(chǎn)生的運(yùn)動(dòng)畸變，然后變換到全局(w)坐標(biāo)系下??//平移后繞z軸旋轉(zhuǎn)（imuRollStart）??float?x7?=?cos(imuRollStart)?*?(x6?-?imuShiftFromStartX)????????????-?sin(imuRollStart)?*?(y6?-?imuShiftFromStartY);??float?y7?=?sin(imuRollStart)?*?(x6?-?imuShiftFromStartX)????????????+?cos(imuRollStart)?*?(y6?-?imuShiftFromStartY);??float?z7?=?z6?-?imuShiftFromStartZ;??//繞x軸旋轉(zhuǎn)（imuPitchStart）??float?x8?=?x7;??float?y8?=?cos(imuPitchStart)?*?y7?-?sin(imuPitchStart)?*?z7;??float?z8?=?sin(imuPitchStart)?*?y7?+?cos(imuPitchStart)?*?z7;??//繞y軸旋轉(zhuǎn)（imuYawStart）??float?x9?=?cos(imuYawStart)?*?x8?+?sin(imuYawStart)?*?z8;??float?y9?=?y8;??float?z9?=?-sin(imuYawStart)?*?x8?+?cos(imuYawStart)?*?z8;??//然后變換到全局(w)坐標(biāo)系下的當(dāng)前幀終止時(shí)刻??//繞y軸旋轉(zhuǎn)（-imuYawLast）??float?x10?=?cos(imuYawLast)?*?x9?-?sin(imuYawLast)?*?z9;??float?y10?=?y9;??float?z10?=?sin(imuYawLast)?*?x9?+?cos(imuYawLast)?*?z9;??//繞x軸旋轉(zhuǎn)（-imuPitchLast）??float?x11?=?x10;??float?y11?=?cos(imuPitchLast)?*?y10?+?sin(imuPitchLast)?*?z10;??float?z11?=?-sin(imuPitchLast)?*?y10?+?cos(imuPitchLast)?*?z10;??//繞z軸旋轉(zhuǎn)（-imuRollLast）??po->x?=?cos(imuRollLast)?*?x11?+?sin(imuRollLast)?*?y11;??po->y?=?-sin(imuRollLast)?*?x11?+?cos(imuRollLast)?*?y11;??po->z?=?z11;??//只保留線號(hào)??po->intensity?=?int(pi->intensity);}

總結(jié)

感覺如果去掉IMU的話，整個(gè)代碼就很清晰，但是加上IMU部分就很容易讓人懵逼，下一篇文章中將會(huì)對(duì)一些laserOdometry中涉及到的坐標(biāo)變換和IMU修正進(jìn)行講解。

標(biāo)簽：