作者：K.Fire

寫在前面

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

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

整體框架

LOAM多牛逼就不用多說(shuō)了，直接開始

先貼一下我詳細(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è)向量通過(guò)可以變換到A中的向量。首先對(duì)照ros的節(jié)點(diǎn)圖和論文中提到的算法框架來(lái)看一下：

可以看到節(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的過(guò)程，可以得到高頻率但精度略低的位姿變換，它的具體功能如下：

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

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

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

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

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

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

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

其中有6個(gè)訂閱者，分別看一下它們的回調(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;//搜索到的點(diǎn)序
std::vectorpointSearchSqDis;//搜索到的點(diǎn)平方距離

PointTypepointOri,pointSel;/*選中的特征點(diǎn)*/
PointTypetripod1,tripod2,tripod3;/*特征點(diǎn)的對(duì)應(yīng)點(diǎn)*/
PointTypepointProj;/*unused*/
PointTypecoeff;/*直線或平面的系數(shù)*/

//退化標(biāo)志
boolisDegenerate=false;
//P矩陣，預(yù)測(cè)矩陣，用來(lái)處理退化情況
cv::MatmatP(6,6,CV_32F,cv::all(0));

intframeCount=skipFrameNum;//skipFrameNum=1
ros::Raterate(100);
boolstatus=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)變量中

濾除無(wú)效點(diǎn)

將接收標(biāo)志設(shè)置為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;
}

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

//接收imu消息
voidimuTransHandler(constsensor_msgs::PointCloud2ConstPtr&imuTrans2)
{
timeImuTrans=imuTrans2->header.stamp.toSec();

imuTrans->clear();
pcl::fromROSMsg(*imuTrans2,*imuTrans);

//根據(jù)發(fā)來(lái)的消息提取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 初始化

接收到第一幀點(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::PtrlaserCloudTemp=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::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);

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

systemInited=true;
continue;
}

2.2 TransformToStart()函數(shù)

在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)行更新(迭代更新過(guò)程在特征匹配和非線性最小二乘優(yōu)化中才進(jìn)行)，所以這里使用的transform變量與上上幀相對(duì)于上一幀的變換相同，這也就是作者假設(shè)了每個(gè)掃描周期車輛都進(jìn)行了完全相同的運(yùn)動(dòng)，用數(shù)學(xué)公式表示如下：

那么問(wèn)題如下：

現(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)過(guò)程：

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

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

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

//將該次sweep中每個(gè)點(diǎn)通過(guò)插值，變換到初始時(shí)刻start
voidTransformToStart(PointTypeconst*constpi,PointType*constpo)
{
//插值系數(shù)計(jì)算，云中每個(gè)點(diǎn)的相對(duì)時(shí)間/點(diǎn)云周期10
floats=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
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坐標(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）
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中每個(gè)點(diǎn)通過(guò)插值，變換到初始時(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)，就按照論文中所說(shuō)，需要查找上一幀中，與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í)就是在求解非線性最小二乘問(wèn)題時(shí)的核函數(shù)，s的本質(zhì)定義如下：

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

//論文中提到的Levenberg-Marquardt算法(L-Mmethod)，其實(shí)是高斯牛頓算法
for(intiterCount=0;iterCountclear();
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(inti=0;ipoints[i],&pointSel);

//每迭代五次，重新查找最近點(diǎn)，相當(dāng)于每個(gè)匹配迭代優(yōu)化4次，ALOAM代碼中的Ceres代碼的最大迭代次數(shù)為4
if(iterCount%5==0){
std::vectorindices;
pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast,indices);
//kd-tree查找一個(gè)最近距離點(diǎn)，邊沿點(diǎn)未經(jīng)過(guò)體素柵格濾波，一般邊沿點(diǎn)本來(lái)就比較少，不做濾波
kdtreeCornerLast->nearestKSearch(pointSel,1,pointSearchInd,pointSearchSqDis);
intclosestPointInd=-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]points[closestPointInd].intensity);

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

2.3 planar point匹配

尋找匹配平面：

將該次sweep中每個(gè)點(diǎn)通過(guò)插值，變換到初始時(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)，就按照論文中所說(shuō)，需要查找上一幀中，與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í)就是在求解非線性最小二乘問(wèn)題時(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(inti=0;ipoints[i],&pointSel);

if(iterCount%5==0){
//kd-tree最近點(diǎn)查找，在經(jīng)過(guò)體素柵格濾波之后的平面點(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
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 計(jì)算雅可比矩陣并求解增量

在代碼中，作者是純手推的高斯牛頓算法，這種方式相比于使用Ceres等工具，會(huì)提高運(yùn)算速度，但是計(jì)算雅克比矩陣比較麻煩，需要清晰的思路和扎實(shí)的數(shù)學(xué)功底，下面我們一起來(lái)推導(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)過(guò)的坐標(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è)軸的分量就是前面求解出來(lái)的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(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》)中有詳細(xì)論述。

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

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

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;
}
}

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

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

3.3 迭代更新

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

//累加每次迭代的旋轉(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;
??????????}
??????????//計(jì)算旋轉(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é)一下整個(gè)流程：

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

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

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

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

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

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

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

	4.1 發(fā)布話題

	這一部分首先獎(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::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)之后的坐標(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é)束位置
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++;
//點(diǎn)云全部點(diǎn)，每間隔一個(gè)點(diǎn)云數(shù)據(jù)相對(duì)點(diǎn)云最后一個(gè)點(diǎn)進(jìn)行畸變校正
if(frameCount>=skipFrameNum+1){//skipFrameNum=1
intlaserCloudFullResNum=laserCloudFullRes->points.size();
for(inti=0;ipoints[i],&laserCloudFullRes->points[i]);
}
}

//畸變校正之后的點(diǎn)作為last點(diǎn)保存等下個(gè)點(diǎn)云進(jìn)來(lái)進(jìn)行匹配
pcl::PointCloud::PtrlaserCloudTemp=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::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 將點(diǎn)云變換到結(jié)束時(shí)刻end--TransformToEnd()函數(shù)

	這里對(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ù)的過(guò)程，將當(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.上面的過(guò)程，總體上看的結(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)通過(guò)測(cè)量得到的imuRPYLast變換到全局(w)坐標(biāo)系下的當(dāng)前幀終止時(shí)刻，并且對(duì)應(yīng)了imuRPYLast姿態(tài)角：

	

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

	

	
//將上一幀點(diǎn)云中的點(diǎn)相對(duì)結(jié)束位置去除因勻速運(yùn)動(dòng)產(chǎn)生的畸變，效果相當(dāng)于得到在點(diǎn)云掃描結(jié)束位置靜止掃描得到的點(diǎn)云
voidTransformToEnd(PointTypeconst*constpi,PointType*constpo)
{
//插值系數(shù)計(jì)算
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];

//將點(diǎn)curr系下的點(diǎn)，變換到初始時(shí)刻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;//求出了相對(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）
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)生的運(yùn)動(dòng)畸變，然后變換到全局(w)坐標(biāo)系下
//平移后繞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)坐標(biāo)系下的當(dāng)前幀終止時(shí)刻
//繞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;
//只保留線號(hào)
po->intensity=int(pi->intensity);
}


	總結(jié)

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

	編輯：黃飛