家居企业网站建设效果,一个后台可以做几个网站,新网域名网站,老网站怎么做循环链接LaserOdometry
这一模块#xff08;节点#xff09;主要功能是#xff1a;进行点云数据配准#xff0c;完成运动估计
利用ScanRegistration中提取到的特征点#xff0c;建立相邻时间点云数据之间的关联#xff0c;由此推断lidar的运动。我们依旧从主函数开始#xff1…LaserOdometry
这一模块节点主要功能是进行点云数据配准完成运动估计
利用ScanRegistration中提取到的特征点建立相邻时间点云数据之间的关联由此推断lidar的运动。我们依旧从主函数开始
int main(int argc, char** argv)
{ros::init(argc, argv, laserOdometry);ros::NodeHandle nh;/***** 订阅6个节点通过回调函数进行处理 *****/ros::Subscriber subCornerPointsSharp nh.subscribesensor_msgs::PointCloud2(/laser_cloud_sharp, 2, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp nh.subscribesensor_msgs::PointCloud2(/laser_cloud_less_sharp, 2, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat nh.subscribesensor_msgs::PointCloud2(/laser_cloud_flat, 2, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat nh.subscribesensor_msgs::PointCloud2(/laser_cloud_less_flat, 2, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes nh.subscribesensor_msgs::PointCloud2 (/velodyne_cloud_2, 2, laserCloudFullResHandler);ros::Subscriber subImuTrans nh.subscribesensor_msgs::PointCloud2 (/imu_trans, 5, imuTransHandler);/**** 发布4个节点 *****/ros::Publisher pubLaserCloudCornerLast nh.advertisesensor_msgs::PointCloud2(/laser_cloud_corner_last, 2);ros::Publisher pubLaserCloudSurfLast nh.advertisesensor_msgs::PointCloud2(/laser_cloud_surf_last, 2);ros::Publisher pubLaserCloudFullRes nh.advertisesensor_msgs::PointCloud2 (/velodyne_cloud_3, 2);ros::Publisher pubLaserOdometry nh.advertisenav_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::PointCloudPointType::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;
在初始化之后就能将相邻两个时刻的点云哪来进行配准了具体的做法我们看看论文怎么说 在这里由于需要高速的处理论文并没有采用一一对应的点云配准事实上对于点云来说这也是很难做到的而是在上一时刻寻找两点确定一条与当前时刻的角点距离最近的直线作为该角点的配准对应同理在上一时刻寻找三点确定一个与当前时刻的平面点距离最近的平面作为该平面点的配准对应。
作者给出了距离的计算公式于是整个配准过程变成了优化过程——不断迭代使得距离之和最小。我们来看看作者给出的距离公式 根据下图能较容易推导出该公式 由三角形面积公式可知2Sc*dabsinC 则dabsinC/c|a×b|/c
接下来是点到平面的距离 根据下图能较容易推导出该公式 首先看分子b和c叉乘得到平面法向量设为h。即要求a和h的点乘的模。等于|a||h|cosα。再看分母b和c叉乘的模即法向量h的模|h|。因此分子除以分母即为a在h上投影的长度|a|cosα也就是a到平面的距离。
理解了原理我们来看这一部分的代码 // 特征点的数量足够多再开始匹配cornerPointsSharpNum和cornerPointsSharp都是最新时刻t1的数据if (laserCloudCornerLastNum 10 laserCloudSurfLastNum 100) {std::vectorint 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) {// 每一次迭代都将特征点都要利用当前预测的坐标转换转换至k1 sweep的初始位置处对应于函数 TransformToStart()// 每一次迭代都将特征点都要利用当前预测的坐标转换转换至k1 sweep的结束位置处对应于函数 TransformToEnd()// pointSel是当前时刻t1的cornerPointsSharp转换到初始imu坐标系后的点坐标对角点一个一个做处理设为i点TransformToStart(cornerPointsSharp-points[i], pointSel);// 每5次循环寻找一次邻域点否则沿用上次循环的邻域点if (iterCount % 5 0) {std::vectorint 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) {/********当前时刻t1转换到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[权重系数*(点到直线的距离/点到平面的距离)] 求解公式: AXB// 为了让左边满秩同乘At- At*A*X At*BmatA.atfloat(i, 0) arx;matA.atfloat(i, 1) ary;matA.atfloat(i, 2) arz;matA.atfloat(i, 3) atx;matA.atfloat(i, 4) aty;matA.atfloat(i, 5) atz;matB.atfloat(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[权重系数*(点到直线的距离/点到平面的距离)] 求解公式: AXB// 为了让左边满秩同乘At- At*A*X At*BmatA.atfloat(i, 0) arx;matA.atfloat(i, 1) ary;matA.atfloat(i, 2) arz;matA.atfloat(i, 3) atx;matA.atfloat(i, 4) aty;matA.atfloat(i, 5) atz;matB.atfloat(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.atfloat(0, i) eignThre[i]) {for (int j 0; j 6; j) {matV2.atfloat(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.atfloat(0, 0);transform[1] matX.atfloat(1, 0);transform[2] matX.atfloat(2, 0);transform[3] matX.atfloat(3, 0);transform[4] matX.atfloat(4, 0);transform[5] matX.atfloat(5, 0);for(int i0; i6; i){if(isnan(transform[i]))transform[i]0;}float deltaR sqrt(pow(rad2deg(matX.atfloat(0, 0)), 2) pow(rad2deg(matX.atfloat(1, 0)), 2) pow(rad2deg(matX.atfloat(2, 0)), 2));float deltaT sqrt(pow(matX.atfloat(3, 0) * 100, 2) pow(matX.atfloat(4, 0) * 100, 2) pow(matX.atfloat(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的作用是将k1时刻的less特征点转移至k1时刻的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]);}}/******* t1时刻数据成为上一个时刻的数据 ************/pcl::PointCloudPointType::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;
}
代码剩余的部分中回调函数的功能较为简单不再赘述。
最后同样附上框图方便理解
