這是LOAM第二部分Lidar laserOdometry雷達(dá)里程計(jì)。
在第一章提取完特征點(diǎn)后，需要對(duì)特征點(diǎn)云進(jìn)行關(guān)聯(lián)匹配，之后估計(jì)姿態(tài)。
主要分為兩部分：

特征點(diǎn)關(guān)聯(lián)使用scan-to-scan方式t和t+1時(shí)刻相鄰兩幀的點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)，分為邊緣點(diǎn)匹配和平面點(diǎn)匹配兩部分。計(jì)算點(diǎn)到直線的距離和點(diǎn)到平面的距離。
姿態(tài)解算根據(jù)匹配的特征點(diǎn)云使用LM算法估計(jì)接收端位姿。

這部分代碼完全是放在main函數(shù)。
首先介紹前面的參數(shù)。后面出現(xiàn)的會(huì)重點(diǎn)標(biāo)注下。

//一個(gè)點(diǎn)云周期 const float scanPeriod = 0.1;//接收到邊緣點(diǎn)和平面點(diǎn) pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>()); //上一幀點(diǎn)云的邊緣點(diǎn)和平面點(diǎn) pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>()); //保存前一個(gè)節(jié)點(diǎn)發(fā)過(guò)來(lái)的未經(jīng)處理過(guò)的特征點(diǎn)和權(quán)重 pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>()); //全部點(diǎn)云 pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>()); //imu信息 pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>()); //上一幀邊緣點(diǎn)、平面點(diǎn)構(gòu)建的KD-tree pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>()); pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());//當(dāng)前幀相對(duì)上一幀的狀態(tài)轉(zhuǎn)移量，局部坐標(biāo)系 float transform[6] = {0}; //當(dāng)前幀相對(duì)于第一幀的狀態(tài)轉(zhuǎn)移量，全局坐標(biāo)系 float transformSum[6] = {0};//點(diǎn)云第一個(gè)點(diǎn)的RPY、最后一個(gè)點(diǎn)的RPY float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0; //點(diǎn)云最后一個(gè)點(diǎn)相對(duì)于第一個(gè)點(diǎn)由于加減速產(chǎn)生的畸變位移、速度 float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0; float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

0、預(yù)處理

訂閱節(jié)點(diǎn)，發(fā)布消息

創(chuàng)建里程計(jì)對(duì)象、坐標(biāo)轉(zhuǎn)換對(duì)象

同步作用，確保同時(shí)收到同一個(gè)點(diǎn)云的特征點(diǎn)以及IMU信息才進(jìn)入

6個(gè)sub訂閱器分別訂閱scanRegistration節(jié)點(diǎn)發(fā)布的6個(gè)消息：/laser_cloud_sharp、/laser_cloud_less_sharp、pubSurfPointFlat、pubSurfPointLessFlat、/velodyne_cloud_2、/imu_trans消息。
調(diào)用6個(gè)Handler回調(diào)函數(shù)處理這些邊特征、面特征、全部點(diǎn)云和IMU數(shù)據(jù)，把他們從ROS::Msg類型轉(zhuǎn)換成程序可以處理的pcl點(diǎn)云類型；
4個(gè)pub發(fā)布器發(fā)布/laser_cloud_corner_last、/laser_cloud_surf_last、/velodyne_cloud_3、/laser_odom_to_init消息。

int main(int argc, char** argv) {/****************0、預(yù)處理************************/ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;// 1.訂閱節(jié)點(diǎn)，發(fā)布消息// 訂閱scanRegistration發(fā)布的六個(gè)節(jié)點(diǎn),調(diào)用六個(gè)回調(diào)函數(shù)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);// imu隊(duì)列長(zhǎng)度5// 發(fā)布四個(gè)節(jié)點(diǎn)，發(fā)布消息ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);// 隊(duì)列長(zhǎng)度2ros::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);// 隊(duì)列長(zhǎng)度5// 2.創(chuàng)建里程計(jì)對(duì)象、坐標(biāo)轉(zhuǎn)換對(duì)象// 創(chuàng)建里程計(jì)對(duì)象 laserOdometrynav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";tf::TransformBroadcaster tfBroadcaster;// 創(chuàng)建坐標(biāo)變換對(duì)象 laserOdometryTranstf::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)*/, tripod1, tripod2, tripod3/*特征點(diǎn)的對(duì)應(yīng)點(diǎn)*/, pointProj/*unused*/, coeff;//退化標(biāo)志bool isDegenerate = false;//P矩陣，預(yù)測(cè)矩陣cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));int frameCount = skipFrameNum;ros::Rate rate(100);bool status = ros::ok();while (status) {// 調(diào)用一次回調(diào)函數(shù)，訂閱當(dāng)前時(shí)刻msg和發(fā)布上一時(shí)刻處理后的msgros::spinOnce();// 3.同步作用，確保同時(shí)收到同一個(gè)點(diǎn)云的特征點(diǎn)以及IMU信息才進(jìn)入if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) { newCornerPointsSharp = false;newCornerPointsLessSharp = false;newSurfPointsFlat = false;newSurfPointsLessFlat = false;newLaserCloudFullRes = false;newImuTrans = false;

接下來(lái)分為：初始化、點(diǎn)云處理、構(gòu)建Jacobian矩陣估計(jì)位姿、坐標(biāo)轉(zhuǎn)換。

1、初始化

將第一個(gè)點(diǎn)云數(shù)據(jù)集發(fā)送給laserMapping,從下一個(gè)點(diǎn)云數(shù)據(jù)開(kāi)始配準(zhǔn)。

將第一個(gè)點(diǎn)云訂閱的數(shù)據(jù)保存為上一時(shí)刻數(shù)據(jù)

從ROS轉(zhuǎn)化為pcl，再發(fā)布出去

記住原點(diǎn)的翻滾角和俯仰角為imu

T平移量的初值賦值為imu加減速的位移量，為其梯度下降的方向

//********************1、初始化：將第一個(gè)點(diǎn)云數(shù)據(jù)集發(fā)送給laserMapping,從下一個(gè)點(diǎn)云數(shù)據(jù)開(kāi)始配準(zhǔn)****************if (!systemInited) {// 1.將訂閱的數(shù)據(jù)保存為上一時(shí)刻數(shù)據(jù)。// 將邊緣點(diǎn)和上一幀邊緣點(diǎn)交換，保存當(dāng)前幀邊緣點(diǎn)值cornerPointsLessSharp下輪使用pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;//將平面點(diǎn)和上一幀平面點(diǎn)交換，，保存當(dāng)前幀平面點(diǎn)surfPointsLessFlat的值下輪使用laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;//使用上一幀的特征點(diǎn)（邊緣+平面）構(gòu)建kd-treekdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的邊沿點(diǎn)集合kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面點(diǎn)集合// 2.將上一時(shí)刻數(shù)據(jù)（邊緣+平面）從ROS轉(zhuǎn)化為pcl，再發(fā)布出去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);//3.記住原點(diǎn)的翻滾角和俯仰角transformSum[0] += imuPitchStart;transformSum[2] += imuRollStart;systemInited = true;continue;}//4.T平移量的初值賦值為加減速的位移量，為其梯度下降的方向//（沿用上次轉(zhuǎn)換的T（一個(gè)sweep勻速模型），同時(shí)在其基礎(chǔ)上減去勻速運(yùn)動(dòng)位移，即只考慮加減速的位移量）transform[3] -= imuVeloFromStartX * scanPeriod;transform[4] -= imuVeloFromStartY * scanPeriod;transform[5] -= imuVeloFromStartZ * scanPeriod;

2、點(diǎn)云配準(zhǔn)

點(diǎn)云配準(zhǔn)前言：首先保證上一時(shí)刻特征點(diǎn)（邊緣+平面）數(shù)量足夠再開(kāi)始進(jìn)行匹配，其次，后面需要25次迭代LM算法求解位姿。
重要參數(shù)：

laserCloudOri：點(diǎn)云過(guò)濾：保留距離小，權(quán)重大的點(diǎn)；舍棄距離為零的點(diǎn)。
coeffSel：權(quán)重

if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {//上一時(shí)刻特征點(diǎn)數(shù)量足夠// 當(dāng)前時(shí)刻特征點(diǎn)（邊緣+平面）數(shù)量std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);int cornerPointsSharpNum = cornerPointsSharp->points.size();int surfPointsFlatNum = surfPointsFlat->points.size();//多次迭代LM算法求解前后位姿變化，這里25次for (int iterCount = 0; iterCount < 25; iterCount++) {laserCloudOri->clear();coeffSel->clear();

針對(duì)邊緣點(diǎn)和平面點(diǎn)兩部分分別進(jìn)行匹配關(guān)系處理。
以邊緣點(diǎn)為例：

kd-tree查找一個(gè)最近距離點(diǎn)j，Ind為索引，SqDis為距離平方

尋找最鄰近點(diǎn)j附近的次臨近點(diǎn)l：分為scanID增加和減小兩個(gè)方向分別進(jìn)行尋找。

計(jì)算點(diǎn)到線的距離，根據(jù)距離分配權(quán)重

邊緣點(diǎn)處理：

/********************邊緣點(diǎn)處理************///從上個(gè)點(diǎn)云中邊緣點(diǎn)中找兩個(gè)最近距離點(diǎn)j和l//一個(gè)點(diǎn)j使用kd-tree查找，在最鄰近點(diǎn)j附近(向上下三條掃描線以內(nèi))找到次臨近點(diǎn)lfor (int i = 0; i < cornerPointsSharpNum; i++) {// 0、t+1時(shí)刻的邊緣點(diǎn)轉(zhuǎn)換到初始imu坐標(biāo)系的點(diǎn)坐標(biāo) pointSelTransformToStart(&cornerPointsSharp->points[i], &pointSel);// ？？？每迭代五次，重新查找最近點(diǎn)（降采樣）if (iterCount % 5 == 0) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);// 1、kd-tree查找一個(gè)最近距離點(diǎn)j，Ind為索引，SqDis為距離平方kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1;//在最鄰近點(diǎn)j附近(向上下三條掃描線以內(nèi))找到次臨近點(diǎn)l//再次提醒：velodyne是2度一線，scanID相鄰并不代表線號(hào)相鄰，相鄰線度數(shù)相差2度，也即線號(hào)scanID相差2if (pointSearchSqDis[0] < 25) {//找到的最近點(diǎn)距離的確很近的話closestPointInd = pointSearchInd[0];// 最近點(diǎn)j//提取最近點(diǎn)線號(hào)int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);float pointSqDis, minPointSqDis2 = 25;//初始門檻值5米，可大致過(guò)濾掉scanID相鄰，但實(shí)際線不相鄰的值// 2、尋找最鄰近點(diǎn)j附近的次臨近點(diǎn)l// 向scanID增大的方向查找for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {//非相鄰線結(jié)束，距離超過(guò)三條線跳出if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {break;}// 計(jì)算所有點(diǎn)和最近鄰點(diǎn)j距離平方pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {//確保兩個(gè)點(diǎn)不在同一條scan上（相鄰線查找應(yīng)該可以用scanID == closestPointScan +/- 1 來(lái)做）if (pointSqDis < minPointSqDis2) {//距離更近，要小于初始值5米//更新最小距離與點(diǎn)序minPointSqDis2 = pointSqDis;minPointInd2 = j;}}}//向scanID減小的方向查找，向下三條線for (int j = closestPointInd - 1; j >= 0; j--) {if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {break;}pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) {if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}}}}//記住組成線的點(diǎn)序pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距離點(diǎn)j，-1表示未找到滿足的點(diǎn)pointSearchCornerInd2[i] = minPointInd2;//另一個(gè)最近的l，-1表示未找到滿足的點(diǎn)}// 3、計(jì)算點(diǎn)到線的距離，根據(jù)距離分配權(quán)重if (pointSearchCornerInd2[i] >= 0) {//次臨近點(diǎn)l大于等于0，不等于-1，說(shuō)明兩個(gè)點(diǎn)都找到了tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];// 最鄰近點(diǎn)tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];// 次臨近點(diǎn)//三個(gè)點(diǎn)的坐標(biāo)：t+1時(shí)刻點(diǎn)i，t時(shí)刻的j和lfloat x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = tripod1.x;float y1 = tripod1.y;float z1 = tripod1.z;float x2 = tripod2.x;float y2 = tripod2.y;float z2 = tripod2.z;//向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）//分子的模：向量OA OB的向量積(即叉乘)為：//| i j k |//|x0-x1 y0-y1 z0-z1|//|x0-x2 y0-y2 z0-z2|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)));//分母AB的模：兩個(gè)最近距離點(diǎn)之間的距離float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));//點(diǎn)到線的距離，d = |向量OA 叉乘 向量OB|/|AB|float ld2 = a012 / l12;//AB方向的單位向量與OAB平面的單位法向量的向量積在各軸上的分量（d的方向）//x軸分量ifloat la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;//y軸分量jfloat lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;//z軸分量kfloat lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;//unusedpointProj = pointSel;pointProj.x -= la * ld2;pointProj.y -= lb * ld2;pointProj.z -= lc * ld2;//權(quán)重計(jì)算，距離越大權(quán)重越小，距離越小權(quán)重越大，得到的權(quán)重范圍<=1float s = 1;if (iterCount >= 5) {//5次迭代之后開(kāi)始增加權(quán)重因素s = 1 - 1.8 * fabs(ld2);}//考慮權(quán)重coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1 && ld2 != 0) {//只保留權(quán)重大的，也即距離比較小的點(diǎn)，同時(shí)也舍棄距離為零的laserCloudOri->push_back(cornerPointsSharp->points[i]);coeffSel->push_back(coeff);}}}

同理，平面點(diǎn)也是相同步驟

/********************平面點(diǎn)處理************///對(duì)本次接收到的平面點(diǎn),從上次接收到的點(diǎn)云曲率比較小的點(diǎn)中找三點(diǎn)組成平面，一個(gè)使用kd-tree查找，//另外一個(gè)在同一線上查找滿足要求的，第三個(gè)在不同線上查找滿足要求的for (int i = 0; i < surfPointsFlatNum; i++) {TransformToStart(&surfPointsFlat->points[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);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;}pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) {//如果點(diǎn)的線號(hào)小于等于最近點(diǎn)的線號(hào)(應(yīng)該最多取等，也即同一線上的點(diǎn))if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}} else {//如果點(diǎn)處在大于該線上if (pointSqDis < minPointSqDis3) {minPointSqDis3 = pointSqDis;minPointInd3 = j;}}}//同理for (int j = closestPointInd - 1; j >= 0; j--) {if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) {break;}pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) {if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}} else {if (pointSqDis < minPointSqDis3) {minPointSqDis3 = pointSqDis;minPointInd3 = j;}}}}pointSearchSurfInd1[i] = closestPointInd;//kd-tree最近距離點(diǎn),-1表示未找到滿足要求的點(diǎn)pointSearchSurfInd2[i] = minPointInd2;//同一線號(hào)上的距離最近的點(diǎn)，-1表示未找到滿足要求的點(diǎn)pointSearchSurfInd3[i] = minPointInd3;//不同線號(hào)上的距離最近的點(diǎn)，-1表示未找到滿足要求的點(diǎn)}if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {//找到了三個(gè)點(diǎn)tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//A點(diǎn)tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//B點(diǎn)tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//C點(diǎn)//向量AB = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)//向量AC = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)//向量AB AC的向量積（即叉乘），得到的是法向量//x軸方向分向量ifloat pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);//y軸方向分向量jfloat pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);//z軸方向分向量kfloat pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);//法向量的模float ps = sqrt(pa * pa + pb * pb + pc * pc);//pa pb pc為法向量各方向上的單位向量pa /= ps;pb /= ps;pc /= ps;pd /= ps;//點(diǎn)到面的距離：向量OA與與法向量的點(diǎn)積除以法向量的模float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;//unusedpointProj = pointSel;pointProj.x -= pa * pd2;pointProj.y -= pb * pd2;pointProj.z -= pc * pd2;//同理計(jì)算權(quán)重float s = 1;if (iterCount >= 5) {s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));}//考慮權(quán)重coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1 && pd2 != 0) {//保存原始點(diǎn)與相應(yīng)的系數(shù)laserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}

3、構(gòu)建Jacobian矩陣估計(jì)位姿

LM 的計(jì)算流程為：

計(jì)算matA,matB矩陣，可以Ax=B

最小二乘法計(jì)算（QR分解），為了讓左邊滿秩，同乘At-> AtAX = At*B

出現(xiàn)退化用預(yù)測(cè)矩陣P

/*****************************3、構(gòu)建Jacobian矩陣估計(jì)位姿 ********************/cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(pointSelNum, 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));// 1.計(jì)算matA,matB矩陣// A=[J的偏導(dǎo)]; B=[權(quán)重系數(shù)*(點(diǎn)到直線的距離/點(diǎn)到平面的距離)] 求解公式: AX=B// 為了讓左邊滿秩，同乘At-> At*A*X = At*Bfor (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];// J對(duì)rx,ry,rz,tx,ty,tz求解偏導(dǎo)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;}// 2.最小二乘法計(jì)算（QR分解）cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;//求解matAtA * matX = matAtBcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// 3.出現(xiàn)退化用預(yù)測(cè)矩陣Pif (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)的特征向量置為0matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}//計(jì)算P矩陣matP = matV.inv() * matV2;}if (isDegenerate) {//如果發(fā)生退化，只使用預(yù)測(cè)矩陣P計(jì)算cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}//累加每次迭代的旋轉(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;}}}

4、坐標(biāo)轉(zhuǎn)換

求解的是點(diǎn)云間的相對(duì)運(yùn)動(dòng)，但他們是在這兩幀點(diǎn)云的局部坐標(biāo)系下的，我們需要把它轉(zhuǎn)換到世界坐標(biāo)系下，因此需要進(jìn)行轉(zhuǎn)換。

局部坐標(biāo)系轉(zhuǎn)移到全局坐標(biāo)系

根據(jù)IMU修正旋轉(zhuǎn)量，插入imu旋轉(zhuǎn)，更新位姿

歐拉角轉(zhuǎn)換成四元數(shù)，再發(fā)布出去四元數(shù)和平移量

對(duì)k+1時(shí)刻的less特征點(diǎn)（邊緣+平面）轉(zhuǎn)移至k+1時(shí)刻的sweep的結(jié)束位置處的雷達(dá)坐標(biāo)系下

更新，畸變校正之后的點(diǎn)作為last點(diǎn)保存等下個(gè)點(diǎn)云進(jìn)來(lái)進(jìn)行匹配

按照跳幀數(shù)publich邊沿點(diǎn)，平面點(diǎn)以及全部點(diǎn)給laserMapping(每隔一幀發(fā)一次)

1.局部坐標(biāo)系轉(zhuǎn)移到全局坐標(biāo)系
分為兩步驟，先把旋轉(zhuǎn)累積量全局化，再把平移量全局化。

// 1.局部坐標(biāo)系轉(zhuǎn)移到全局坐標(biāo)系float rx, ry, rz, tx, ty, tz;// 1)旋轉(zhuǎn)角累計(jì)變化量相對(duì)于原點(diǎn)rx,ry,rzAccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) - sin(rz) * (transform[4] - imuShiftFromStartY);float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) + cos(rz) * (transform[4] - imuShiftFromStartY);float z1 = transform[5] * 1.05 - imuShiftFromStartZ;float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;// 2)平移量相對(duì)于原點(diǎn)tx,ty,tztx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

其中涉及到的旋轉(zhuǎn)累積量函數(shù)。
AccumulateRotation()
相對(duì)于第一個(gè)點(diǎn)云即原點(diǎn)，積累旋轉(zhuǎn)量.

//相對(duì)于第一個(gè)點(diǎn)云即原點(diǎn)，積累旋轉(zhuǎn)量 void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz) {float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);ox = -asin(srx);float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));oy = atan2(srycrx / cos(ox), crycrx / cos(ox));float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); }

2.根據(jù)IMU修正旋轉(zhuǎn)量，插入imu旋轉(zhuǎn)，更新位姿

PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);// 得到世界坐標(biāo)系下的轉(zhuǎn)移矩陣TtransformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;

其中涉及到的IMU修正函數(shù)。
PluginIMURotation()
利用IMU修正旋轉(zhuǎn)量，根據(jù)起始?xì)W拉角，當(dāng)前點(diǎn)云的歐拉角修正

void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, float alx, float aly, float alz, float &acx, float &acy, float &acz) {float sbcx = sin(bcx);float cbcx = cos(bcx);float sbcy = sin(bcy);float cbcy = cos(bcy);float sbcz = sin(bcz);float cbcz = cos(bcz);float sblx = sin(blx);float cblx = cos(blx);float sbly = sin(bly);float cbly = cos(bly);float sblz = sin(blz);float cblz = cos(blz);float salx = sin(alx);float calx = cos(alx);float saly = sin(aly);float caly = cos(aly);float salz = sin(alz);float calz = cos(alz);float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);acx = -asin(srx);float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);acy = atan2(srycrx / cos(acx), crycrx / cos(acx));float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*cblx*salz*sblz);float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - calx*calz*cblx*sblz);acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx)); }

3.歐拉角轉(zhuǎn)換成四元數(shù)，再發(fā)布出去四元數(shù)和平移量

geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);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);// tf廣播新的平移旋轉(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);

4.對(duì)k+1時(shí)刻的less特征點(diǎn)（邊緣+平面）轉(zhuǎn)移至k+1時(shí)刻的sweep的結(jié)束位置處的雷達(dá)坐標(biāo)系下

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) {int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++) {TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}}

其中，涉及到將當(dāng)前時(shí)刻特征點(diǎn)投影到sweep結(jié)束位置的雷達(dá)坐標(biāo)系TransformToEnd（）。這里其實(shí)一起來(lái)講，有當(dāng)前幀點(diǎn)云P(k+1) TransformToStart（）和上一幀點(diǎn)云P(k) TransformToEnd（）兩個(gè)函數(shù)，為了將兩幀點(diǎn)云去除畸變，統(tǒng)一到同一坐標(biāo)系下計(jì)算。

TransformToStart()
當(dāng)前點(diǎn)云中的點(diǎn)相對(duì)第一個(gè)點(diǎn)去除因勻速運(yùn)動(dòng)產(chǎn)生的畸變，效果相當(dāng)于得到在點(diǎn)云掃描開(kāi)始位置靜止掃描得到的點(diǎn)云

void TransformToStart(PointType const * const pi, PointType * const po) {//插值系數(shù)計(jì)算，云中每個(gè)點(diǎn)的相對(duì)時(shí)間/點(diǎn)云周期10float s = 10 * (pi->intensity - int(pi->intensity));//線性插值：根據(jù)每個(gè)點(diǎn)在點(diǎn)云中的相對(duì)位置關(guān)系，乘以相應(yīng)的旋轉(zhuǎn)平移系數(shù)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];//平移后繞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; }

TransformToEnd()
當(dāng)前點(diǎn)云中的點(diǎn)相對(duì)第一個(gè)點(diǎn)去除因勻速運(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));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];//平移后繞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)rx = transform[0];ry = transform[1];rz = transform[2];tx = transform[3];ty = transform[4];tz = transform[5];//繞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;//平移后繞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;//繞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); }

5.更新，畸變校正之后的點(diǎn)作為last點(diǎn)保存等下個(gè)點(diǎn)云進(jìn)來(lái)進(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-treeif (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);}

6.按照跳幀數(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();

總結(jié)

以上是生活随笔為你收集整理的LOAM源码解析2——laserOdometry的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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