LOAM_velodyne學習(二)

LaserOdometry

這一模塊(節點)主要功能是:進行點云數據配準,完成運動估計

利用ScanRegistration中提取到的特征點,建立相鄰時間點云數據之間的關聯,由此推斷lidar的運動。我們依舊從主函數開始:

int main(int argc, char** argv)
{ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;/***** 訂閱6個節點,通過回調函數進行處理 *****/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);/**** 發布4個節點 *****/ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2);ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);//創建對象nav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";//創建TransformBroadcaster對象tf::TransformBroadcaster tfBroadcaster;//創建坐標變換對象tf::StampedTransform laserOdometryTrans;laserOdometryTrans.frame_id_ = "/camera_init";laserOdometryTrans.child_frame_id_ = "/laser_odom";

接下來是設置處理速度并進行初始化

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

在初始化之后,就能將相鄰兩個時刻的點云哪來進行配準了,具體的做法我們看看論文怎么說

在這里,由于需要高速的處理,論文并沒有采用一一對應的點云配準(事實上,對于點云來說,這也是很難做到的),而是在上一時刻尋找兩點確定一條當前時刻的角點距離最近直線作為該角點的配準對應,同理,在上一時刻尋找三點確定一個與當前時刻的平面點距離最近平面作為該平面點的配準對應。

作者給出了距離的計算公式,于是整個配準過程變成了優化過程——不斷迭代使得距離之和最小。我們來看看作者給出的距離公式

根據下圖能較容易推導出該公式

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

接下來是點到平面的距離

根據下圖能較容易推導出該公式

?

首先看分子:b和c叉乘,得到平面法向量設為h。即要求a和h的點乘的模。等于|a||h|cosα。再看分母:b和c叉乘的模,即法向量h的模|h|。因此,分子除以分母即為a在h上投影的長度|a|cosα,也就是a到平面的距離。

理解了原理,我們來看這一部分的代碼

 // 特征點的數量足夠多再開始匹配,cornerPointsSharpNum和cornerPointsSharp都是最新時刻t+1的數據if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);int cornerPointsSharpNum = cornerPointsSharp->points.size();// 多次迭代 LM求解前后時刻的位姿變化for (int iterCount = 0; iterCount < 25; iterCount++) {laserCloudOri->clear();coeffSel->clear();/******** 特征角點的處理 ********/for (int i = 0; i < cornerPointsSharpNum; i++) {// 每一次迭代都將特征點都要利用當前預測的坐標轉換轉換至k+1 sweep的初始位置處對應于函數 TransformToStart()// 每一次迭代都將特征點都要利用當前預測的坐標轉換轉換至k+1 sweep的結束位置處對應于函數 TransformToEnd()// pointSel是當前時刻t+1的cornerPointsSharp轉換到初始imu坐標系后的點坐標,對角點一個一個做處理,設為i點TransformToStart(&cornerPointsSharp->points[i], &pointSel);// 每5次循環尋找一次鄰域點,否則沿用上次循環的鄰域點if (iterCount % 5 == 0) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);// nearestKSearch是PCL中的K近鄰域搜索,搜索上一時刻LessCornor的K鄰域點// 搜索結果: pointSearchInd是索引; pointSearchSqDis是近鄰對應的平方距離(以25作為閾值)kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);/******** 在上一時刻t的LessSharp中用KD樹尋找和點i最近的點j ********/int closestPointInd = -1, minPointInd2 = -1;if (pointSearchSqDis[0] < 25) {closestPointInd = pointSearchInd[0];//最近鄰域點的所在層數int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);/******** 在上一時刻t的LessSharp中點j附近2層中最近的點l ********/float pointSqDis, minPointSqDis2 = 25;for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {// closestPointScan +(-) 2.5表示只接受(后)前2層(共4層)范圍內的數據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;}/******** 計算點i到jl的距離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));// 對x,y,z的偏導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);//增加權重,距離越遠,影響影子越小}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);}}}/******** 特征平面點的處理 ********/for (int i = 0; i < surfPointsFlatNum; i++) {/********當前時刻t+1轉換到imu初始坐標系下,對平面點一個一個做處理,設為點i ********/TransformToStart(&surfPointsFlat->points[i], &pointSel);if (iterCount % 5 == 0) {kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;/******** 上一時刻t的LessFlat中的距離點i最近的點j ********/if (pointSearchSqDis[0] < 25) {closestPointInd = pointSearchInd[0];int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);/******** 在附近4層尋找距離點j最近的兩點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;}/*******計算點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既為偏導函數的分子部分也為距離函數分母行列式內各方向分量值,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;//距離沒有取絕對值float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 平面偏導公式,似乎后面沒有用到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);}}}// 如果點云數量小于10,可以不用計算位姿矩陣了int pointSelNum = laserCloudOri->points.size();if (pointSelNum < 10) {continue;}似乎后面沒有用到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);}}}// 如果點云數量小于10,可以不用計算位姿矩陣了int pointSelNum = laserCloudOri->points.size();if (pointSelNum < 10) {continue;}

之后則是通過構建雅克比矩陣,完成LM求解,從而估計運動,我們仔細閱讀論文了解其原理

接下來看看代碼

          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));/****** 構建雅可比矩陣,求解 *******/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對rx,ry,rz,tx,ty,tz求解偏導,不是很明白怎么得到的代碼 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的偏導]; B=[權重系數*(點到直線的距離/點到平面的距離)] 求解公式: 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的偏導]; B=[權重系數*(點到直線的距離/點到平面的距離)] 求解公式: 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

接下來有一個迭代第一步的處理,猜測是出現退化進行修正。然后更新位姿之后進行收斂判斷

 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); // 計算矩陣的特征向量E及特征向量的反對稱陣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小的特征值則出現退化} 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;}

最為艱難的計算過程結束了,接下來就是坐標轉換了。。。。。這部分其實也讓我很頭疼,各個坐標系有點暈,仔細理一理

先看代碼

float rx, ry, rz, tx, ty, tz;/******** 旋轉角的累計變化量 ********/// AccumulateRotation作用將局部旋轉坐標轉換至全局旋轉坐標AccumulateRotation(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;tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);// 插入imu旋轉,更新位姿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轉化為四元數發布 ********/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時刻的less特征點轉移至k+1時刻的sweep的結束位置處的雷達坐標系下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時刻數據成為上一個時刻的數據 ************/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發布一次? ————————————*/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;
}

代碼剩余的部分中,回調函數的功能較為簡單,不再贅述。

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

?

本文來自互聯網用戶投稿,該文觀點僅代表作者本人,不代表本站立場。本站僅提供信息存儲空間服務,不擁有所有權,不承擔相關法律責任。
如若轉載,請注明出處:http://www.pswp.cn/news/253948.shtml
繁體地址,請注明出處:http://hk.pswp.cn/news/253948.shtml
英文地址,請注明出處:http://en.pswp.cn/news/253948.shtml

如若內容造成侵權/違法違規/事實不符,請聯系多彩編程網進行投訴反饋email:809451989@qq.com,一經查實,立即刪除!

相關文章

戶外穿越

晚上很早就睡了&#xff0c;并且&#xff0c;太過激動&#xff0c;所以早上四點五十分就被驚醒&#xff0c;然后到早上鬧鐘響。 早上匆匆忙吃過早餐&#xff0c;就趕去坐車&#xff0c;到登山之前&#xff0c;坐了大巴車&#xff0c;又坐了景區的車&#xff0c;景區的路是山路十…

【oracle】關于創建表時用default指定默認值的坑

剛開始學create table的時候沒注意&#xff0c;學到后面發現可以指定默認值。于是寫了如下語句&#xff1a; 當我查詢的時候發現&#xff0c;查出來的結果是這樣的。。 很納悶有沒有&#xff0c;我明明指定默認值了呀&#xff0c;為什么創建出來的表還是空的呢&#xff1f;又跑…

Makefile中用宏定義進行條件編譯(gcc -D)/在Makefile中進行宏定義-D

在源代碼里面如果這樣是定義的&#xff1a; #ifdef MACRONAME //可選代碼 #endif 那在makefile里面 gcc -D MACRONAMEMACRODEF 或者 gcc -D MACRONAME 這樣就定義了預處理宏&#xff0c;編譯的時候可選代碼就會被編譯進去了。 對于GCC編譯器&#xff0c;有如下選項&…

python安裝與配置

首先下載python地址&#xff1a; https://www.python.org/downloads/release/python-361/下載頁面中有多個版本&#xff1a; web-based installer 是需要通過聯網完成安裝的 executable installer 是可執行文件(*.exe)方式安裝 embeddable zip file 嵌入式版本&#xff0c;可…

[OpenGL ES 03]3D變換:模型,視圖,投影與Viewport

[OpenGL ES 03]3D變換&#xff1a;模型&#xff0c;視圖&#xff0c;投影與Viewport 羅朝輝 (http://blog.csdn.net/kesalin) 本文遵循“署名-非商業用途-保持一致”創作公用協議 系列文章&#xff1a;[OpenGL ES 01]OpenGL ES之初體驗[OpenGL ES 02]OpenGL ES渲染管線與著色器…

LOAM_velodyne學習(三)

終于到第三個模塊了&#xff0c;我們先來回顧下之前的工作&#xff1a;點云數據進來后&#xff0c;經過前兩個節點的處理可以完成一個完整但粗糙的里程計&#xff0c;可以概略地估計出Lidar的相對運動。如果不受任何測量噪聲的影響&#xff0c;這個運動估計的結果足夠精確&…

監控視頻線種類 視頻信號傳輸介紹及各種視頻接口的傳輸距離

一.視頻信號接口 監控視頻線種類介紹&#xff1a; 按照材料區分有SYV及SYWV兩種&#xff0c;絕緣層的物理材料結構不同&#xff0c;SYV是實心聚乙烯電纜&#xff0c;SYWV是高物理發泡電纜&#xff0c;物理發泡電纜傳輸性能優于聚乙烯。 S--同軸電纜 Y--聚乙烯 V--聚氯乙烯 W…

免費節假日API 更新新功能了 新增農歷信息返回

感謝大家對免費節假日API的支持.最近看了別家的api于是增加了一些新功能即獲取日期的農歷信息. 這個新功能還處于測試階段如有問題歡迎反饋 檢查一個日期是詳細信息 https://tool.bitefu.net/jiari/?d20180101&info1 返回值 {"status": 1,"type": 1,…

新手算法學習之路----二叉樹(二叉樹最大路徑和)

摘抄自&#xff1a;https://segmentfault.com/a/1190000003554858#articleHeader2 題目&#xff1a; Given a binary tree, find the maximum path sum. The path may start and end at any node in the tree. For example: Given the below binary tree, 1/ \2 3Return 6. 思…

Ajax工作原理

詳見&#xff1a;http://blog.yemou.net/article/query/info/tytfjhfascvhzxcyt238 在這篇文章中&#xff0c;我將從10個方面來對AJAX技術進行系統的講解。 1、ajax技術的背景 不可否認&#xff0c;ajax技術的流行得益于google的大力推廣&#xff0c;正是由于google earth、go…

各種視頻信號格式及端子介紹/VGA DVI HDMI區別

視頻信號是我們接觸最多的顯示信號&#xff0c;但您并不一定對各種視頻信號有所了解。因為國內用到的視頻信號格式和端子非常有限&#xff0c;一般就是復合視頻和S端子&#xff0c;稍高級一些的就是色差及VGA。對于那些經常接觸國外電器和二手設備的朋友&#xff0c;就會遇到各…

LOAM_velodyne學習(四)

TransformMaintenance 來到了最后一個模塊&#xff0c;代碼不是很長&#xff0c;我們在看完代碼之后&#xff0c;再詳細說明這個模塊的功能 依然主函數開始 int main(int argc, char** argv) {ros::init(argc, argv, "transformMaintenance");ros::NodeHandle nh;…

PHP數據庫類

<?phpclass Db{//私有靜態屬性存儲實例化對象自身private static $instance;//存儲PDO類的實例化private $pdo;//PDOStatement類private $stmt;//禁止外部實例化對象&#xff0c;鏈接數據庫private function __construct($config,$port,$charset){try{$this->pdo new P…

oracle參數文件、控制文件、數據文件、日志文件的位置及查詢方法

參數文件&#xff1a;所有參數文件一般在 $ORACLE_HOME/dbs 下 sqlplus查詢語句&#xff1a;show parameter spfile; 網絡連接文件&#xff1a; $ORACLE_HOME/dbs/network/admin 目錄中 控制文件&#xff1a;select * from v$controlfile; 數據文件&#xff1a;一般在oracleda…

Bishops Alliance—— 最大上升子序列

原題鏈接&#xff1a;http://codeforces.com/gym/101147/problem/F 題意&#xff1a;n*n的棋盤&#xff0c;給m個主教的坐標及其私有距離p&#xff0c;以及常數C&#xff0c;求位于同一對角線上滿足條件&#xff1a;dist(i, j) > p[i]^2 p[j]^2 C 的主教集合的元素個數最…

LeGO-LOAM學習

前言 在學習了LOAM之后&#xff0c;了解到LeGO-LOAM&#xff08;面向復雜情況的輕量級優化地面的雷達里程計&#xff09;&#xff0c;進行了一個學習整理。 Github&#xff1a;https://github.com/RobustFieldAutonomyLab/LeGO-LOAM 論文&#xff1a;https://github.com/Robu…

char data[0]用法總結

struct MyData { int nLen; char data[0]; }; 開始沒有理解紅色部分的內容&#xff0c;上網搜索下&#xff0c;發現用處很大&#xff0c;記錄下來。 在結構中&#xff0c;data是一個數組名&#xff1b;但該數組沒有元素&#xff1b;該數組…

(一)低功耗設計目的與功耗的類型

一、低功耗設計的目的 1.便攜性設備等需求 電子產品在我們生活中扮演了極其重要的作用&#xff0c;便攜性的電子設備便是其中一種。便攜性設備需要電池供電、需要消耗電池的能量。在同等電能提供下&#xff0c;低功耗設計的產品就能夠工作更長的時間。時間的就是生命&#xff…

(轉)徹底學會使用epoll(一)——ET模式實現分析

注&#xff1a;之前寫過兩篇關于epoll實現的文章&#xff0c;但是感覺懂得了實現原理并不一定會使用&#xff0c;所以又決定寫這一系列文章&#xff0c;希望能夠對epoll有比較清楚的認識。是請大家轉載務必注明出處&#xff0c;算是對我勞動成果的一點點尊重吧。另外&#xff0…

MFC的消息映射有什么作用

絕對以下這三個解釋的比較簡潔&#xff0c;特此做個記錄&#xff01;以感謝回答的這些人&#xff01; MFC的消息映射有什么作用: Windows操作系統主要是有消息來處理的&#xff0c;每個程序都有自己的消息隊列&#xff0c;并且這些消息是有優先級的&#xff0c;也就是誰會先…