LaserOdometry

這一模塊（節(jié)點(diǎn)）主要功能是：進(jìn)行點(diǎn)云數(shù)據(jù)配準(zhǔn)，完成運(yùn)動(dòng)估計(jì)

利用ScanRegistration中提取到的特征點(diǎn)，建立相鄰時(shí)間點(diǎn)云數(shù)據(jù)之間的關(guān)聯(lián)，由此推斷l(xiāng)idar的運(yùn)動(dòng)。我們依舊從主函數(shù)開(kāi)始：

int main(int argc, char** argv) {ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;/***** 訂閱6個(gè)節(jié)點(diǎn)，通過(guò)回調(diào)函數(shù)進(jìn)行處理 *****/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);/**** 發(fā)布4個(gè)節(jié)點(diǎn) *****/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);//創(chuàng)建對(duì)象nav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";//創(chuàng)建TransformBroadcaster對(duì)象tf::TransformBroadcaster tfBroadcaster;//創(chuàng)建坐標(biāo)變換對(duì)象tf::StampedTransform laserOdometryTrans;laserOdometryTrans.frame_id_ = "/camera_init";laserOdometryTrans.child_frame_id_ = "/laser_odom";

接下來(lái)是設(shè)置處理速度并進(jìn)行初始化

ros::Rate rate(100);bool status = ros::ok(); while (status) {// 進(jìn)行一次回調(diào)函數(shù)的調(diào)用（完成當(dāng)前時(shí)刻的msg的訂閱和上一時(shí)刻的處理后的msg的發(fā)布）ros::spinOnce();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;/*********初始化 ***********/if (!systemInited) { /********保存上一時(shí)刻的數(shù)據(jù)******/// exchange cornerPointsLessSharp & laserCloudCornerLast pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;// exchange surfPointsLessFlat & laserCloudSurfLastlaserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;// kd二叉樹(shù) 為了方便尋找最近的點(diǎn)kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);// 發(fā)布 laserCloudCornerLast 上一時(shí)刻sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);// 發(fā)布 laserCloudSurfLast 上一時(shí)刻sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);// transformSum累計(jì)變換了的角度transformSum[0] += imuPitchStart;transformSum[2] += imuRollStart;systemInited = true;continue; //初始化后開(kāi)始接受下一時(shí)刻的訂閱數(shù)據(jù)}laserCloudOri->clear();coeffSel->clear();//更新位姿中的坐標(biāo)transform[3] -= imuVeloFromStartX * scanPeriod;transform[4] -= imuVeloFromStartY * scanPeriod;transform[5] -= imuVeloFromStartZ * scanPeriod;

在初始化之后，就能將相鄰兩個(gè)時(shí)刻的點(diǎn)云哪來(lái)進(jìn)行配準(zhǔn)了，具體的做法我們看看論文怎么說(shuō)

在這里，由于需要高速的處理，論文并沒(méi)有采用一一對(duì)應(yīng)的點(diǎn)云配準(zhǔn)（事實(shí)上，對(duì)于點(diǎn)云來(lái)說(shuō)，這也是很難做到的），而是在上一時(shí)刻尋找兩點(diǎn)確定一條與當(dāng)前時(shí)刻的角點(diǎn)距離最近的直線作為該角點(diǎn)的配準(zhǔn)對(duì)應(yīng)，同理，在上一時(shí)刻尋找三點(diǎn)確定一個(gè)與當(dāng)前時(shí)刻的平面點(diǎn)距離最近的平面作為該平面點(diǎn)的配準(zhǔn)對(duì)應(yīng)。

作者給出了距離的計(jì)算公式，于是整個(gè)配準(zhǔn)過(guò)程變成了優(yōu)化過(guò)程——不斷迭代使得距離之和最小。我們來(lái)看看作者給出的距離公式

根據(jù)下圖能較容易推導(dǎo)出該公式

由三角形面積公式可知2S=c*d=absinC 則d=absinC/c=|a×b|/c

接下來(lái)是點(diǎn)到平面的距離

根據(jù)下圖能較容易推導(dǎo)出該公式

?

首先看分子：b和c叉乘，得到平面法向量設(shè)為h。即要求a和h的點(diǎn)乘的模。等于|a||h|cosα。再看分母：b和c叉乘的模，即法向量h的模|h|。因此，分子除以分母即為a在h上投影的長(zhǎng)度|a|cosα，也就是a到平面的距離。

理解了原理，我們來(lái)看這一部分的代碼

// 特征點(diǎn)的數(shù)量足夠多再開(kāi)始匹配，cornerPointsSharpNum和cornerPointsSharp都是最新時(shí)刻t+1的數(shù)據(jù)if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);int cornerPointsSharpNum = cornerPointsSharp->points.size();// 多次迭代 LM求解前后時(shí)刻的位姿變化for (int iterCount = 0; iterCount < 25; iterCount++) {laserCloudOri->clear();coeffSel->clear();/******** 特征角點(diǎn)的處理 ********/for (int i = 0; i < cornerPointsSharpNum; i++) {// 每一次迭代都將特征點(diǎn)都要利用當(dāng)前預(yù)測(cè)的坐標(biāo)轉(zhuǎn)換轉(zhuǎn)換至k+1 sweep的初始位置處對(duì)應(yīng)于函數(shù) TransformToStart()// 每一次迭代都將特征點(diǎn)都要利用當(dāng)前預(yù)測(cè)的坐標(biāo)轉(zhuǎn)換轉(zhuǎn)換至k+1 sweep的結(jié)束位置處對(duì)應(yīng)于函數(shù) TransformToEnd()// pointSel是當(dāng)前時(shí)刻t+1的cornerPointsSharp轉(zhuǎn)換到初始imu坐標(biāo)系后的點(diǎn)坐標(biāo)，對(duì)角點(diǎn)一個(gè)一個(gè)做處理，設(shè)為i點(diǎn)TransformToStart(&cornerPointsSharp->points[i], &pointSel);// 每5次循環(huán)尋找一次鄰域點(diǎn)，否則沿用上次循環(huán)的鄰域點(diǎn)if (iterCount % 5 == 0) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);// nearestKSearch是PCL中的K近鄰域搜索，搜索上一時(shí)刻LessCornor的K鄰域點(diǎn)// 搜索結(jié)果: pointSearchInd是索引; pointSearchSqDis是近鄰對(duì)應(yīng)的平方距離(以25作為閾值)kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);/******** 在上一時(shí)刻t的LessSharp中用KD樹(shù)尋找和點(diǎn)i最近的點(diǎn)j ********/int closestPointInd = -1, minPointInd2 = -1;if (pointSearchSqDis[0] < 25) {closestPointInd = pointSearchInd[0];//最近鄰域點(diǎn)的所在層數(shù)int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);/******** 在上一時(shí)刻t的LessSharp中點(diǎn)j附近2層中最近的點(diǎn)l ********/float pointSqDis, minPointSqDis2 = 25;for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {// closestPointScan +(-) 2.5表示只接受(后)前2層(共4層)范圍內(nèi)的數(shù)據(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;}}}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;}}}}pointSearchCornerInd1[i] = closestPointInd;pointSearchCornerInd2[i] = minPointInd2;}/******** 計(jì)算點(diǎn)i到j(luò)l的距離de 理想情況下ijl共線 ********/if (pointSearchCornerInd2[i] >= 0) {tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];float 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;// 公式分子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)));// 公式分母float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));// 對(duì)x,y,z的偏導(dǎo)float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float ld2 = a012 / l12;pointProj = pointSel;pointProj.x -= la * ld2;pointProj.y -= lb * ld2;pointProj.z -= lc * ld2;float s = 1;if (iterCount >= 5) {s = 1 - 1.8 * fabs(ld2);//增加權(quán)重，距離越遠(yuǎn)，影響影子越小}coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1 && ld2 != 0) {laserCloudOri->push_back(cornerPointsSharp->points[i]);coeffSel->push_back(coeff);}}}/******** 特征平面點(diǎn)的處理 ********/for (int i = 0; i < surfPointsFlatNum; i++) {/********當(dāng)前時(shí)刻t+1轉(zhuǎn)換到imu初始坐標(biāo)系下，對(duì)平面點(diǎn)一個(gè)一個(gè)做處理，設(shè)為點(diǎn)i ********/TransformToStart(&surfPointsFlat->points[i], &pointSel);if (iterCount % 5 == 0) {kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;/******** 上一時(shí)刻t的LessFlat中的距離點(diǎn)i最近的點(diǎn)j ********/if (pointSearchSqDis[0] < 25) {closestPointInd = pointSearchInd[0];int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);/******** 在附近4層尋找距離點(diǎn)j最近的兩點(diǎn)l,m ********/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) {if (pointSqDis < minPointSqDis2) {minPointSqDis2 = pointSqDis;minPointInd2 = j;}} else {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;pointSearchSurfInd2[i] = minPointInd2;pointSearchSurfInd3[i] = minPointInd3;}/*******計(jì)算點(diǎn)i到平面jlm的距離dh 理想情況下ijlm共面 *******/if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];// pa,pb,pc既為偏導(dǎo)函數(shù)的分子部分也為距離函數(shù)分母行列式內(nèi)各方向分量值，ps為分母部分float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);float 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 /= ps;pb /= ps;pc /= ps;pd /= ps;//距離沒(méi)有取絕對(duì)值float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 平面偏導(dǎo)公式，似乎后面沒(méi)有用到pointProj = pointSel;pointProj.x -= pa * pd2;pointProj.y -= pb * pd2;pointProj.z -= pc * pd2;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));//加上影響因子}coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1 && pd2 != 0) {laserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}// 如果點(diǎn)云數(shù)量小于10，可以不用計(jì)算位姿矩陣了int pointSelNum = laserCloudOri->points.size();if (pointSelNum < 10) {continue;}似乎后面沒(méi)有用到pointProj = pointSel;pointProj.x -= pa * pd2;pointProj.y -= pb * pd2;pointProj.z -= pc * pd2;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));//加上影響因子}coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1 && pd2 != 0) {laserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}// 如果點(diǎn)云數(shù)量小于10，可以不用計(jì)算位姿矩陣了int pointSelNum = laserCloudOri->points.size();if (pointSelNum < 10) {continue;}

之后則是通過(guò)構(gòu)建雅克比矩陣，完成LM求解，從而估計(jì)運(yùn)動(dòng)，我們仔細(xì)閱讀論文了解其原理

接下來(lái)看看代碼

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));/****** 構(gòu)建雅可比矩陣，求解 *******/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];// 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;// A=[J的偏導(dǎo)]; B=[權(quán)重系數(shù)*(點(diǎn)到直線的距離/點(diǎn)到平面的距離)] 求解公式: AX=B// 為了讓左邊滿秩，同乘At-> At*A*X = At*BmatA.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;cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);//QR分解得到X不是很明白怎么得到的代碼 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;// A=[J的偏導(dǎo)]; B=[權(quán)重系數(shù)*(點(diǎn)到直線的距離/點(diǎn)到平面的距離)] 求解公式: AX=B// 為了讓左邊滿秩，同乘At-> At*A*X = At*BmatA.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;cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);//QR分解得到X

接下來(lái)有一個(gè)迭代第一步的處理，猜測(cè)是出現(xiàn)退化進(jìn)行修正。然后更新位姿之后進(jìn)行收斂判斷

if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV); // 計(jì)算矩陣的特征向量E及特征向量的反對(duì)稱陣VmatV.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]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true; // 存在比10小的特征值則出現(xiàn)退化} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}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]))transform[i]=0;}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;}

最為艱難的計(jì)算過(guò)程結(jié)束了，接下來(lái)就是坐標(biāo)轉(zhuǎn)換了。。。。。這部分其實(shí)也讓我很頭疼，各個(gè)坐標(biāo)系有點(diǎn)暈，仔細(xì)理一理

先看代碼

float rx, ry, rz, tx, ty, tz;/******** 旋轉(zhuǎn)角的累計(jì)變化量 ********/// AccumulateRotation作用將局部旋轉(zhuǎn)坐標(biāo)轉(zhuǎn)換至全局旋轉(zhuǎn)坐標(biāo)AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);/******** 接著轉(zhuǎn)移到世界坐標(biāo)系下 *******/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;tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);// 插入imu旋轉(zhuǎn)，更新位姿PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;/******** rx,ry,rz轉(zhuǎn)化為四元數(shù)發(fā)布 ********/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);/********* laserOdometryTrans 是用于tf廣播 ********/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);// TransformToEnd的作用是將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]);}/***** 這樣if條件的意義 *******/frameCount++;if (frameCount >= skipFrameNum + 1) {int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++) {TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}}/******* t+1時(shí)刻數(shù)據(jù)成為上一個(gè)時(shí)刻的數(shù)據(jù) ************/pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);}/*———————————— 2HZ發(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();}return 0; }

代碼剩余的部分中，回調(diào)函數(shù)的功能較為簡(jiǎn)單，不再贅述。

最后同樣附上框圖方便理解

?

總結(jié)

以上是生活随笔為你收集整理的LOAM_velodyne学习（二）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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