作者：K.Fire

寫在前面

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

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

整體框架

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

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

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

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

LIDAR(激光雷達坐標系l)：x軸向前，y軸向左，z軸向上

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

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

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

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

可以看到節(jié)點圖和論文中的框架是一一對應的，這幾個模塊的功能如下：

scanRegistration：對原始點云進行預處理，計算曲率，提取特征點

laserOdometry：對當前sweep與上一次sweep進行特征匹配，計算一個快速(10Hz)但粗略的位姿估計

laserMapping：對當前sweep與一個局部子圖進行特征匹配，計算一個慢速(1Hz)比較精確的位姿估計

transformMaintenance：對兩個模塊計算出的位姿進行融合，得到最終的精確地位姿估計

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

接收特征點話題、全部點云話題、IMU話題，并保存到對應的變量中

將當前sweep與上一次sweep進行特征匹配，得到edge point匹配對應的直線以及planar point匹配對應的平面

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

累積位姿變換，并用IMU修正，得到當前幀到初始幀的累積位姿變換

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

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

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

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

intmain(intargc,char**argv)
{
ros::init(argc,argv,"laserOdometry");
ros::NodeHandlenh;

ros::SubscribersubCornerPointsSharp=nh.subscribe
("/laser_cloud_sharp",2,laserCloudSharpHandler);

ros::SubscribersubCornerPointsLessSharp=nh.subscribe
("/laser_cloud_less_sharp",2,laserCloudLessSharpHandler);

ros::SubscribersubSurfPointsFlat=nh.subscribe
("/laser_cloud_flat",2,laserCloudFlatHandler);

ros::SubscribersubSurfPointsLessFlat=nh.subscribe
("/laser_cloud_less_flat",2,laserCloudLessFlatHandler);

ros::SubscribersubLaserCloudFullRes=nh.subscribe
("/velodyne_cloud_2",2,laserCloudFullResHandler);

ros::SubscribersubImuTrans=nh.subscribe
("/imu_trans",5,imuTransHandler);

ros::PublisherpubLaserCloudCornerLast=nh.advertise
("/laser_cloud_corner_last",2);

ros::PublisherpubLaserCloudSurfLast=nh.advertise
("/laser_cloud_surf_last",2);

ros::PublisherpubLaserCloudFullRes=nh.advertise
("/velodyne_cloud_3",2);

ros::PublisherpubLaserOdometry=nh.advertise("/laser_odom_to_init",5);
nav_msgs::OdometrylaserOdometry;
laserOdometry.header.frame_id="/camera_init";
laserOdometry.child_frame_id="/laser_odom";

tf::TransformBroadcastertfBroadcaster;
tf::StampedTransformlaserOdometryTrans;
laserOdometryTrans.frame_id_="/camera_init";
laserOdometryTrans.child_frame_id_="/laser_odom";

std::vectorpointSearchInd;//搜索到的點序
std::vectorpointSearchSqDis;//搜索到的點平方距離

PointTypepointOri,pointSel;/*選中的特征點*/
PointTypetripod1,tripod2,tripod3;/*特征點的對應點*/
PointTypepointProj;/*unused*/
PointTypecoeff;/*直線或平面的系數(shù)*/

//退化標志
boolisDegenerate=false;
//P矩陣，預測矩陣，用來處理退化情況
cv::MatmatP(6,6,CV_32F,cv::all(0));

intframeCount=skipFrameNum;//skipFrameNum=1
ros::Raterate(100);
boolstatus=ros::ok();

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

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

對于接收到點云之后都是如下操作：

記錄時間戳

保存到相應變量中

濾除無效點

將接收標志設置為true

voidlaserCloudSharpHandler(constsensor_msgs::PointCloud2ConstPtr&cornerPointsSharp2)
{
timeCornerPointsSharp=cornerPointsSharp2->header.stamp.toSec();

cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerPointsSharp2,*cornerPointsSharp);
std::vectorindices;
pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp,indices);
newCornerPointsSharp=true;
}

voidlaserCloudLessSharpHandler(constsensor_msgs::PointCloud2ConstPtr&cornerPointsLessSharp2)
{
timeCornerPointsLessSharp=cornerPointsLessSharp2->header.stamp.toSec();

cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerPointsLessSharp2,*cornerPointsLessSharp);
std::vectorindices;
pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp,indices);
newCornerPointsLessSharp=true;
}

voidlaserCloudFlatHandler(constsensor_msgs::PointCloud2ConstPtr&surfPointsFlat2)
{
timeSurfPointsFlat=surfPointsFlat2->header.stamp.toSec();

surfPointsFlat->clear();
pcl::fromROSMsg(*surfPointsFlat2,*surfPointsFlat);
std::vectorindices;
pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat,indices);
newSurfPointsFlat=true;
}

voidlaserCloudLessFlatHandler(constsensor_msgs::PointCloud2ConstPtr&surfPointsLessFlat2)
{
timeSurfPointsLessFlat=surfPointsLessFlat2->header.stamp.toSec();

surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfPointsLessFlat2,*surfPointsLessFlat);
std::vectorindices;
pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat,indices);
newSurfPointsLessFlat=true;
}

//接收全部點
voidlaserCloudFullResHandler(constsensor_msgs::PointCloud2ConstPtr&laserCloudFullRes2)
{
timeLaserCloudFullRes=laserCloudFullRes2->header.stamp.toSec();

laserCloudFullRes->clear();
pcl::fromROSMsg(*laserCloudFullRes2,*laserCloudFullRes);
std::vectorindices;
pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes,indices);
newLaserCloudFullRes=true;
}

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

//接收imu消息
voidimuTransHandler(constsensor_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;
}

二、特征匹配

2.1 初始化

接收到第一幀點云數(shù)據(jù)時，先進行一次初始化，因為第一幀點云沒法匹配..

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

//初始化：將第一個點云數(shù)據(jù)集發(fā)送給laserMapping,從下一個點云數(shù)據(jù)開始處理
if(!systemInited){
//將cornerPointsLessSharp與laserCloudCornerLast交換,目的保存cornerPointsLessSharp的值到laserCloudCornerLast中下輪使用
pcl::PointCloud::PtrlaserCloudTemp=cornerPointsLessSharp;
cornerPointsLessSharp=laserCloudCornerLast;
laserCloudCornerLast=laserCloudTemp;

//將surfPointLessFlat與laserCloudSurfLast交換，目的保存surfPointsLessFlat的值到laserCloudSurfLast中下輪使用
laserCloudTemp=surfPointsLessFlat;
surfPointsLessFlat=laserCloudSurfLast;
laserCloudSurfLast=laserCloudTemp;

//使用上一幀的特征點構(gòu)建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的邊沿點集合
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面點集合

//將cornerPointsLessSharp和surfPointLessFlat點也即邊沿點和平面點分別發(fā)送給laserMapping
sensor_msgs::PointCloud2laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast,laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp=ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id="/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

sensor_msgs::PointCloud2laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast,laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp=ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id="/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

//記住原點的翻滾角和俯仰角
transformSum[0]+=imuPitchStart;//IMU的y方向
transformSum[2]+=imuRollStart;//IMU的x方向

systemInited=true;
continue;
}

2.2 TransformToStart()函數(shù)

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

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

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

首先要明確：transform中保存的變量是上一幀相對于當前幀的變換，也就是

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

那么問題如下：

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

需要求解的是當前sweep初始時刻坐標系的點云。

推導過程：

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

而為YXZ變換順序(解釋：當前幀相對于上一幀的變換順序為ZXY，呢么反過來，這里是上一幀相當于當前幀的變換，就變成了YXZ)，所以，代入得：

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

//將該次sweep中每個點通過插值，變換到初始時刻start
voidTransformToStart(PointTypeconst*constpi,PointType*constpo)
{
//插值系數(shù)計算，云中每個點的相對時間/點云周期10
floats=10*(pi->intensity-int(pi->intensity));

//線性插值：根據(jù)每個點在點云中的相對位置關系，乘以相應的旋轉(zhuǎn)平移系數(shù)
//這里的transform數(shù)值上和上次一樣，這里體現(xiàn)了勻速運動模型，就是每次時間間隔下，相對于上一次sweep的變換都是一樣的
//而在使用該函數(shù)之前進行了一次操作：transform[3]-=imuVeloFromStartX*scanPeriod;
//這個操作是在勻速模型的基礎上，去除了由于加減速造成的畸變

//R_curr_start=R_YXZ
floatrx=s*transform[0];
floatry=s*transform[1];
floatrz=s*transform[2];
floattx=s*transform[3];
floatty=s*transform[4];
floattz=s*transform[5];

/*pi是在curr坐標系下p_curr，需要變換到當前sweep的初始時刻start坐標系下
*現(xià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）
floatx1=cos(rz)*(pi->x-tx)+sin(rz)*(pi->y-ty);
floaty1=-sin(rz)*(pi->x-tx)+cos(rz)*(pi->y-ty);
floatz1=(pi->z-tz);

//繞x軸旋轉(zhuǎn)（-rx）
floatx2=x1;
floaty2=cos(rx)*y1+sin(rx)*z1;
floatz2=-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;
}

2.2 edge point匹配

尋找匹配直線：

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

然后先試用PCL中的KD-tree查找當前點在上一幀中的最近鄰點，對應論文中提到的j點，最近鄰在上一幀中的索引保存在pointSearchInd中，距離保存在pointSearchSqDis中。

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

closestPointInd對應論文中的j點，minPointInd2對應論文中的l點

int closestPointInd = -1, minPointInd2 = -1;

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

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

計算直線參數(shù)：

當找到了j點和l點，就可以進行特征匹配，計算匹配直線的系數(shù)，對應論文中的公式為：

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

代碼中變量代表的含義：

x0：i點

x1：j點

x2：l點

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

la、lb、lc：i點到jl線垂線方向的向量(jl方向的單位向量與ijl平面的單位法向量的叉乘得到)在xyz三個軸上的分量

ld2：點到直線的距離，即

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

最后將權重大(>0.1)的，即距離比較小，且距離不為零的點放入laserCloudOri，對應的直線系數(shù)放入coeffSel。

//論文中提到的Levenberg-Marquardt算法(L-Mmethod)，其實是高斯牛頓算法
for(intiterCount=0;iterCountclear();
coeffSel->clear();

//處理當前點云中的曲率最大的特征點,從上個點云中曲率比較大的特征點中找兩個最近距離點，一個點使用kd-tree查找，另一個根據(jù)找到的點在其相鄰線找另外一個最近距離的點
for(inti=0;ipoints[i],&pointSel);

//每迭代五次，重新查找最近點，相當于每個匹配迭代優(yōu)化4次，ALOAM代碼中的Ceres代碼的最大迭代次數(shù)為4
if(iterCount%5==0){
std::vectorindices;
pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast,indices);
//kd-tree查找一個最近距離點，邊沿點未經(jīng)過體素柵格濾波，一般邊沿點本來就比較少，不做濾波
kdtreeCornerLast->nearestKSearch(pointSel,1,pointSearchInd,pointSearchSqDis);
intclosestPointInd=-1,minPointInd2=-1;
//closestPointInd對應論文中的j點，minPointInd2對應論文中的l點

//循環(huán)是尋找相鄰線最近點l
//再次提醒：velodyne是2度一線，scanID相鄰并不代表線號相鄰，相鄰線度數(shù)相差2度，也即線號scanID相差2
if(pointSearchSqDis[0]points[closestPointInd].intensity);

floatpointSqDis,minPointSqDis2=25;//初始門檻值25米，可大致過濾掉scanID相鄰，但實際線不相鄰的值
//尋找距離目標點最近距離的平方和最小的點
for(intj=closestPointInd+1;jpoints[j].intensity)>closestPointScan+2.5){//非相鄰線
break;
}
...

2.3 planar point匹配

尋找匹配平面：

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

然后先試用PCL中的KD-tree查找當前點在上一幀中的最近鄰點，對應論文中提到的j點，最近鄰在上一幀中的索引保存在pointSearchInd中，距離保存在pointSearchSqDis中。

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

closestPointInd對應論文中的j點、minPointInd2對應論文中的l點、minPointInd3對應論文中的m點。

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

如果查找到了最近鄰的j點，就按照論文中所說，需要查找上一幀中，與j線號scanID相同且j的最近鄰點l，然后查找一個與j線號scanID相鄰且最近鄰的點m。

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

計算平面參數(shù)：

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

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

代碼中變量代表的含義：

tripod1：j點

tripod2：l點

tripod3：m點

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

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

ps：法向量的模

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

下面這個s可以看做一個權重，對應于論文中的這一部分

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

最后將權重大(>0.1)的，即距離比較小，且距離不為零的點放入laserCloudOri，對應的平面系數(shù)放入coeffSel。

//對本次接收到的曲率最小的點,從上次接收到的點云曲率比較小的點中找三點組成平面，一個使用kd-tree查找，另外一個在同一線上查找滿足要求的，第三個在不同線上查找滿足要求的
for(inti=0;ipoints[i],&pointSel);

if(iterCount%5==0){
//kd-tree最近點查找，在經(jīng)過體素柵格濾波之后的平面點中查找，一般平面點太多，濾波后最近點查找數(shù)據(jù)量小
kdtreeSurfLast->nearestKSearch(pointSel,1,pointSearchInd,pointSearchSqDis);

//closestPointInd對應論文中的j、minPointInd2對應論文中的l、minPointInd3對應論文中的m
intclosestPointInd=-1,minPointInd2=-1,minPointInd3=-1;
if(pointSearchSqDis[0]points[closestPointInd].intensity);

floatpointSqDis,minPointSqDis2=25,minPointSqDis3=25;
for(intj=closestPointInd+1;jpoints[j].intensity)>closestPointScan+2.5){
break;
}

...

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

3.1 計算雅可比矩陣并求解增量

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

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

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

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

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

根據(jù)鏈式法則，f(x)對X求導有：

對其中每一項進行計算：

D對求導的結(jié)果其實就是 進行歸一化后的點到直線向量，它在xyz三個軸的分量就是前面求解出來的la、lb、lc變量。

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

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

//滿足要求的特征點至少10個，特征匹配數(shù)量太少棄用此幀數(shù)據(jù)，不再進行優(yōu)化步驟
if(pointSelNumpoints[i];
coeff=coeffSel->points[i];

floats=1;

floatsrx=sin(s*transform[0]);
floatcrx=cos(s*transform[0]);
floatsry=sin(s*transform[1]);
floatcry=cos(s*transform[1]);
floatsrz=sin(s*transform[2]);
floatcrz=cos(s*transform[2]);
floattx=s*transform[3];
floatty=s*transform[4];
floattz=s*transform[5];

floatarx=(-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;

floatary=((-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;

floatarz=((-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;

floatatx=-s*(cry*crz-srx*sry*srz)*coeff.x+s*crx*srz*coeff.y
-s*(crz*sry+cry*srx*srz)*coeff.z;

floataty=-s*(cry*srz+crz*srx*sry)*coeff.x-s*crx*crz*coeff.y
-s*(sry*srz-cry*crz*srx)*coeff.z;

floatatz=s*crx*sry*coeff.x-s*srx*coeff.y-s*crx*cry*coeff.z;

floatd2=coeff.intensity;

matA.at(i,0)=arx;
matA.at(i,1)=ary;
matA.at(i,2)=arz;
matA.at(i,3)=atx;
matA.at(i,4)=aty;
matA.at(i,5)=atz;
matB.at(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);

3.2 退化處理

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

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

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

if(iterCount==0){
//特征值1*6矩陣
cv::MatmatE(1,6,CV_32F,cv::all(0));
//特征向量6*6矩陣
cv::MatmatV(6,6,CV_32F,cv::all(0));
cv::MatmatV2(6,6,CV_32F,cv::all(0));

//求解特征值/特征向量
cv::eigen(matAtA,matE,matV);
matV.copyTo(matV2);

isDegenerate=false;
//特征值取值門檻
floateignThre[6]={10,10,10,10,10,10};
for(inti=5;i>=0;i--){//從小到大查找
if(matE.at(0,i)(i,j)=0;
}
isDegenerate=true;
}else{
break;
}
}

//計算P矩陣
matP=matV.inv()*matV2;//論文中對應的Vf^-1*V_u`
}

if(isDegenerate){//如果發(fā)生退化，只使用預測矩陣P計算
cv::MatmatX2(6,1,CV_32F,cv::all(0));
matX.copyTo(matX2);
matX=matP*matX2;
}

3.3 迭代更新

最后進行迭代更新，如果更新量很小則終止迭代。

//累加每次迭代的旋轉(zhuǎn)平移量
transform[0]+=matX.at(0,0);
transform[1]+=matX.at(1,0);
transform[2]+=matX.at(2,0);
transform[3]+=matX.at(3,0);
transform[4]+=matX.at(4,0);
transform[5]+=matX.at(5,0);

for(inti=0;i<6;?i++){
????????????if(isnan(transform[i]))//判斷是否非數(shù)字
??????????????transform[i]=0;
??????????}
??????????//計算旋轉(zhuǎn)平移量，如果很小就停止迭代
??????????float?deltaR?=?sqrt(
??????????????????????????????pow(rad2deg(matX.at(0,0)),2)+
pow(rad2deg(matX.at(1,0)),2)+
pow(rad2deg(matX.at(2,0)),2));
floatdeltaT=sqrt(
pow(matX.at(3,0)*100,2)+
pow(matX.at(4,0)*100,2)+
pow(matX.at(5,0)*100,2));

if(deltaR

	

	

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

	一共迭代25次，每次分為edge point和planar point兩個處理過程

	每迭代5次時，重新尋找最近點

	其他情況時，根據(jù)找到的最近點和待匹配點，計算匹配得到的直線和平面方程

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

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

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

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

	4.1 發(fā)布話題

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

	/laser_cloud_corner_last：曲率較大的點--less edge point

	/laser_cloud_surf_last：曲率較小的點--less planar point

	/velodyne_cloud_3：全部點云--full cloud point

	/laser_odom_to_init：里程計坐標系下，當前時刻end相對于初始時刻的坐標變換

	

	
//得到世界坐標系下的轉(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::QuaterniongeoQuat=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)之后的坐標系(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);

//對點云的曲率比較大和比較小的點投影到掃描結(jié)束位置
intcornerPointsLessSharpNum=cornerPointsLessSharp->points.size();
for(inti=0;ipoints[i],&cornerPointsLessSharp->points[i]);
}

intsurfPointsLessFlatNum=surfPointsLessFlat->points.size();
for(inti=0;ipoints[i],&surfPointsLessFlat->points[i]);
}

frameCount++;
//點云全部點，每間隔一個點云數(shù)據(jù)相對點云最后一個點進行畸變校正
if(frameCount>=skipFrameNum+1){//skipFrameNum=1
intlaserCloudFullResNum=laserCloudFullRes->points.size();
for(inti=0;ipoints[i],&laserCloudFullRes->points[i]);
}
}

//畸變校正之后的點作為last點保存等下個點云進來進行匹配
pcl::PointCloud::PtrlaserCloudTemp=cornerPointsLessSharp;
cornerPointsLessSharp=laserCloudCornerLast;
laserCloudCornerLast=laserCloudTemp;

laserCloudTemp=surfPointsLessFlat;
surfPointsLessFlat=laserCloudSurfLast;
laserCloudSurfLast=laserCloudTemp;

laserCloudCornerLastNum=laserCloudCornerLast->points.size();
laserCloudSurfLastNum=laserCloudSurfLast->points.size();
//點足夠多就構(gòu)建kd-tree，否則棄用此幀，沿用上一幀數(shù)據(jù)的kd-tree
if(laserCloudCornerLastNum>10&&laserCloudSurfLastNum>100){
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
}

//按照跳幀數(shù)publich邊沿點，平面點以及全部點給laserMapping(每隔一幀發(fā)一次)
if(frameCount>=skipFrameNum+1){
frameCount=0;

sensor_msgs::PointCloud2laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast,laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp=ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id="/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

sensor_msgs::PointCloud2laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast,laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp=ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id="/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

sensor_msgs::PointCloud2laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes,laserCloudFullRes3);
laserCloudFullRes3.header.stamp=ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id="/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
}

status=ros::ok();
rate.sleep();
}


	

	

	4.2 將點云變換到結(jié)束時刻end--TransformToEnd()函數(shù)

	這里對應于論文中體到的，變換到下圖所示的將變換到。

	

	在勻速運動假設的前提下，從Start時刻到End時刻，點云都將保持imuRPYStart的位置姿態(tài)，然后對其中里程計坐標系下curr時刻的點進行以下操作：

	1.首先進行了一個TransformToStart()函數(shù)的過程，將當前點變換到里程計坐標系下start時刻坐標系下，得到x3、y3、z3：

	

	2.然后變換到里程計坐標系下end時刻坐標系下，再次提醒，得到x6、y6、z6：

	

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

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

	

	4.然后將所有點通過測量得到的imuRPYLast變換到全局(w)坐標系下的當前幀終止時刻，并且對應了imuRPYLast姿態(tài)角：

	

	我理解的這個過程如下：

	

	
//將上一幀點云中的點相對結(jié)束位置去除因勻速運動產(chǎn)生的畸變，效果相當于得到在點云掃描結(jié)束位置靜止掃描得到的點云
voidTransformToEnd(PointTypeconst*constpi,PointType*constpo)
{
//插值系數(shù)計算
floats=10*(pi->intensity-int(pi->intensity));

//R_curr_start
floatrx=s*transform[0];
floatry=s*transform[1];
floatrz=s*transform[2];
floattx=s*transform[3];
floatty=s*transform[4];
floattz=s*transform[5];

//將點curr系下的點，變換到初始時刻start
//平移后繞z軸旋轉(zhuǎn)（-rz）
floatx1=cos(rz)*(pi->x-tx)+sin(rz)*(pi->y-ty);
floaty1=-sin(rz)*(pi->x-tx)+cos(rz)*(pi->y-ty);
floatz1=(pi->z-tz);

//繞x軸旋轉(zhuǎn)（-rx）
floatx2=x1;
floaty2=cos(rx)*y1+sin(rx)*z1;
floatz2=-sin(rx)*y1+cos(rx)*z1;

//繞y軸旋轉(zhuǎn)（-ry）
floatx3=cos(ry)*x2-sin(ry)*z2;
floaty3=y2;
floatz3=sin(ry)*x2+cos(ry)*z2;//求出了相對于起始點校正的坐標

//R_end_start=R_YXZ
rx=transform[0];
ry=transform[1];
rz=transform[2];
tx=transform[3];
ty=transform[4];
tz=transform[5];

//變換到end坐標系
//繞y軸旋轉(zhuǎn)（ry）
floatx4=cos(ry)*x3+sin(ry)*z3;
floaty4=y3;
floatz4=-sin(ry)*x3+cos(ry)*z3;

//繞x軸旋轉(zhuǎn)（rx）
floatx5=x4;
floaty5=cos(rx)*y4-sin(rx)*z4;
floatz5=sin(rx)*y4+cos(rx)*z4;

//繞z軸旋轉(zhuǎn)（rz），再平移
floatx6=cos(rz)*x5-sin(rz)*y5+tx;
floaty6=sin(rz)*x5+cos(rz)*y5+ty;
floatz6=z5+tz;

//使用IMU去除由于加減速產(chǎn)生的運動畸變，然后變換到全局(w)坐標系下
//平移后繞z軸旋轉(zhuǎn)（imuRollStart）
floatx7=cos(imuRollStart)*(x6-imuShiftFromStartX)
-sin(imuRollStart)*(y6-imuShiftFromStartY);
floaty7=sin(imuRollStart)*(x6-imuShiftFromStartX)
+cos(imuRollStart)*(y6-imuShiftFromStartY);
floatz7=z6-imuShiftFromStartZ;

//繞x軸旋轉(zhuǎn)（imuPitchStart）
floatx8=x7;
floaty8=cos(imuPitchStart)*y7-sin(imuPitchStart)*z7;
floatz8=sin(imuPitchStart)*y7+cos(imuPitchStart)*z7;

//繞y軸旋轉(zhuǎn)（imuYawStart）
floatx9=cos(imuYawStart)*x8+sin(imuYawStart)*z8;
floaty9=y8;
floatz9=-sin(imuYawStart)*x8+cos(imuYawStart)*z8;

//然后變換到全局(w)坐標系下的當前幀終止時刻
//繞y軸旋轉(zhuǎn)（-imuYawLast）
floatx10=cos(imuYawLast)*x9-sin(imuYawLast)*z9;
floaty10=y9;
floatz10=sin(imuYawLast)*x9+cos(imuYawLast)*z9;

//繞x軸旋轉(zhuǎn)（-imuPitchLast）
floatx11=x10;
floaty11=cos(imuPitchLast)*y10+sin(imuPitchLast)*z10;
floatz11=-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;
//只保留線號
po->intensity=int(pi->intensity);
}


	總結(jié)

	感覺如果去掉IMU的話，整個代碼就很清晰，但是加上IMU部分就很容易讓人懵逼。

	編輯：黃飛