五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

laserOdometry这个节点订阅了5个话题：有序点云、极大边线点、次极大边线点、极小平面点、次极小平面点。发布了4个话题：有序点云、上一帧的平面点、上一帧的边线点、当前帧位姿粗估计。主要功能是前端的激光里程计和位姿粗估计。

我们先看主函数：

int main(int argc, char **argv)
{ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;nh.param<int>("mapping_skip_frame", skipFrameNum, 2); //    设定里程计的帧率//if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. printf("Mapping %d Hz \n", 10 / skipFrameNum);// 从scanRegistration节点订阅的消息话题，分别为极大边线点  次极大边线点   极小平面点  次极小平面点 全部点云点ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);// 注册发布上一帧的边线点话题   ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);// 注册发布上一帧的平面点话题  ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);// 注册发布全部有序点云话题，就是从scanRegistration订阅来的点云，未经其他处理ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);// 注册发布帧间的位姿变换话题  ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);// 注册发布帧间的平移运动话题  ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);nav_msgs::Path laserPath;int frameCount = 0;// 循环频率ros::Rate rate(100);

下面是主函数的主循环，主要是帧间位姿估计的过程，目标是希望找到位姿变换T，使得第k帧点云左乘T得到第k+1帧点云，或者说左乘T得到k+1帧点云的误差最小。

 while (ros::ok()){ros::spinOnce();    // 只触发一次回调，所以每次都要调用一次；等待回调函数执行完毕，执行一次后续代码，参考https://www.cnblogs.com/liu-fa/p/5925381.html// 首先确保订阅的五个消息都有，有一个队列为空都不行if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&!fullPointsBuf.empty()){// 5个queue记录了最新的极大/次极大边线点，极小/次极小平面点，全部有序点云// 分别求出队列第一个时间，用来分配时间戳timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();// 因为同一帧的时间戳都是相同的，这里比较是否是同一帧if (timeCornerPointsSharp != timeLaserCloudFullRes ||timeCornerPointsLessSharp != timeLaserCloudFullRes ||timeSurfPointsFlat != timeLaserCloudFullRes ||timeSurfPointsLessFlat != timeLaserCloudFullRes){printf("unsync messeage!");ROS_BREAK();}// 分别将五个点云消息取出来，同时转成pcl的点云格式mBuf.lock();  //数据多个线程使用，这里先进行加锁，避免线程冲突cornerPointsSharp->clear();pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);cornerSharpBuf.pop();cornerPointsLessSharp->clear();pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);cornerLessSharpBuf.pop();surfPointsFlat->clear();pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);surfFlatBuf.pop();surfPointsLessFlat->clear();pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);surfLessFlatBuf.pop();laserCloudFullRes->clear();pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);fullPointsBuf.pop();mBuf.unlock(); //数据取出来后进行解锁TicToc t_whole; //计算整个激光雷达里程计的时间// initializing，一个什么也不干的初始化，没有延迟时间，主要用来跳过第一帧数据，直接作为第二帧的上一帧if (!systemInited){systemInited = true;std::cout << "Initialization finished \n";}else //特征点匹配、位姿估计{// 取出比较突出的特征点数量，极大边线点和极小平面点int cornerPointsSharpNum = cornerPointsSharp->points.size();int surfPointsFlatNum = surfPointsFlat->points.size();TicToc t_opt;  //计算优化的时间// 点到线以及点到面的非线性优化，迭代2次（选择当前优化位姿的特征点匹配，并优化位姿（4次迭代），然后重新选择特征点匹配并优化）for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter){corner_correspondence = 0;plane_correspondence = 0;//ceres::LossFunction *loss_function = NULL;// 定义一下ceres的核函数，使用Huber核函数来减少外点的影响，即去除outliersceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);// 由于旋转不满足一般意义的加法，因此这里使用ceres自带的local paramceres::LocalParameterization *q_parameterization =new ceres::EigenQuaternionParameterization();ceres::Problem::Options problem_options;ceres::Problem problem(problem_options);  //实例化求解最优化问题// 待优化的变量是帧间位姿，包括平移和旋转，这里旋转使用四元数来表示problem.AddParameterBlock(para_q, 4, q_parameterization);problem.AddParameterBlock(para_t, 3);pcl::PointXYZI pointSel;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;TicToc t_data;  //计算寻找关联点的时间

基于最近邻原理建立corner特征点（边线点）之间的关联，每一个极大边线点去上一帧的次极大边线点中找匹配；采用边线点匹配方法:假如在第k+1帧中发现了边线点i，通过KD-tree查询在第k帧中的最近邻点j，查询j的附近扫描线上的最近邻点l，j与l相连形成一条直线l-j，让点i与这条直线的距离最短。

构建一个非线性优化问题：以点i与直线lj的距离为代价函数，以位姿变换T(四元数表示旋转+位移t)为优化变量。下面是优化的程序。

                // find correspondence for corner featuresfor (int i = 0; i < cornerPointsSharpNum; ++i) //先进行线点的匹配{// 运动补偿去畸变          TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);// 在上一帧所有角点（次极大边线点）构成的kdtree中寻找距离当前帧最近的一个点kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1;//如果最近邻的corner特征点（次极大边线点）之间距离平方小于阈值，则最近邻点有效if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD){closestPointInd = pointSearchInd[0];    // 目标点对应的最近距离点的索引取出来// 找到其所在线束id，线束信息是intensity的整数部分int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;// search in the direction of increasing scan line// 在刚刚角点（次极大边线点）id附近扫描线分别继续寻找最邻近点for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j){// if in the same scan line, continue， 不找同一根线束的if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)continue;// if not in nearby scans, end the loop，即要求找到的线束距离当前线束不能太远if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))break;// 计算和当前找到的角点（次极大边线点）之间的距离double 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 (pointSqDis < minPointSqDis2){// find nearer point，寻找距离最小的角点（次极大边线点）及其索引，记录其索引minPointSqDis2 = pointSqDis;minPointInd2 = j;}}     // search in the direction of decreasing scan line// 同样另一个方向寻找对应角点（次极大边线点）for (int j = closestPointInd - 1; j >= 0; --j){// if in the same scan line, continue， 不找同一根线束的if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)continue;// if not in nearby scans, end the loop，即要求找到的线束距离当前线束不能太远if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))break;double 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 (pointSqDis < minPointSqDis2){// find nearer point，寻找距离最小的角点（次极大边线点）及其索引， 记录其索引minPointSqDis2 = pointSqDis;minPointInd2 = j;}}}// 如果特征点i的两个最近邻点j和m都有效，构建非线性优化问题if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid{// 取出当前点和上一帧的两个角点Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,cornerPointsSharp->points[i].y,cornerPointsSharp->points[i].z);Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,laserCloudCornerLast->points[closestPointInd].y,laserCloudCornerLast->points[closestPointInd].z);Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,laserCloudCornerLast->points[minPointInd2].y,laserCloudCornerLast->points[minPointInd2].z);double s;if (DISTORTION) //去运动畸变，这里没有做，kitii数据已经做了s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;elses = 1.0;ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);    //构建优化问题并求解corner_correspondence++;}}

下面采用平面点匹配方法：假如在第k+1帧中发现了平面点i，通过KD-tree查询在第k帧（上一帧）中的最近邻点j，查询j的附近扫描线上的最近邻点l和同一条扫描线的最近邻点m，这三点确定一个平面，让点i与这个平面的距离最短；

构建一个非线性优化问题：以点i与平面lmj的距离为代价函数，以位姿变换T(四元数表示旋转+t)为优化变量。

                  for (int i = 0; i < surfPointsFlatNum; ++i){TransformToStart(&(surfPointsFlat->points[i]), &pointSel); //运动补偿去畸变// 先寻找上一帧距离这个面点最近的面点kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;// 距离必须小于给定阈值if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD){// 取出找到的上一帧面点的索引closestPointInd = pointSearchInd[0];// get closest point's scan ID// 取出最近的面点在上一帧的第几根scan上面int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;// search in the direction of increasing scan line// 额外再寻找两个点，要求一个点和最近点同一个scan，另一个是不同scan，先升序遍历搜索点寻找这些点for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j){// if not in nearby scans, end the loop// 不能和当前找到的上一帧面点线束距离太远if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))break;// 计算和当前帧该点距离double 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 in the same or lower scan line// 如果是同一根scan且距离最近if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}// if in the higher scan line// 如果是其他线束点else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3){minPointSqDis3 = pointSqDis;minPointInd3 = j;}}// search in the direction of decreasing scan line// 同样的方式，按照降序方向寻找这两个点for (int j = closestPointInd - 1; j >= 0; --j){// if not in nearby scans, end the loopif (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))break;double 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 in the same or higher scan lineif (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3){// find nearer pointminPointSqDis3 = pointSqDis;minPointInd3 = j;}}// 如果另外找到的两个点是有效点，就取出他们的3d坐标if (minPointInd2 >= 0 && minPointInd3 >= 0){Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,surfPointsFlat->points[i].y,surfPointsFlat->points[i].z);Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,laserCloudSurfLast->points[closestPointInd].y,laserCloudSurfLast->points[closestPointInd].z);Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,laserCloudSurfLast->points[minPointInd2].y,laserCloudSurfLast->points[minPointInd2].z);Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,laserCloudSurfLast->points[minPointInd3].y,laserCloudSurfLast->points[minPointInd3].z);double s;if (DISTORTION) //去运动畸变，这里没有做，kitii数据已经做了s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;elses = 1.0;// 构建点到面的约束，构建cere的非线性优化问题。ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); plane_correspondence++;}}}// 输出寻找关联点消耗的时间//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);printf("data association time %f ms \n", t_data.toc());// 如果总的线约束和面约束太少，就打印一下  if ((corner_correspondence + plane_correspondence) < 10){printf("less correspondence! *************************************************\n");}// 调用ceres求解器求解 ，设定求解器类型，最大迭代次数，不输出过程信息，优化报告存入summaryTicToc t_solver;ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.max_num_iterations = 4;options.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);printf("solver time %f ms \n", t_solver.toc());}// 经过两次LM优化消耗的时间printf("optimization twice time %f \n", t_opt.toc());// 这里的w_curr 实际上是 w_last，即上一帧// 更新帧间匹配的结果，得到lidar odom位姿t_w_curr = t_w_curr + q_w_curr * t_last_curr;q_w_curr = q_w_curr * q_last_curr;}TicToc t_pub; //计算发布运行时间// 发布lidar里程计结果// publish odometry// 创建nav_msgs::Odometry消息类型，把信息导入，并发布nav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init"; //选择相机坐标系laserOdometry.child_frame_id = "/laser_odom"; laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);// 以四元数和平移向量发出去laserOdometry.pose.pose.orientation.x = q_w_curr.x();laserOdometry.pose.pose.orientation.y = q_w_curr.y();laserOdometry.pose.pose.orientation.z = q_w_curr.z();laserOdometry.pose.pose.orientation.w = q_w_curr.w();laserOdometry.pose.pose.position.x = t_w_curr.x();laserOdometry.pose.pose.position.y = t_w_curr.y();laserOdometry.pose.pose.position.z = t_w_curr.z();pubLaserOdometry.publish(laserOdometry);// geometry_msgs::PoseStamped消息是laserOdometry的部分内容geometry_msgs::PoseStamped laserPose;laserPose.header = laserOdometry.header;laserPose.pose = laserOdometry.pose.pose;laserPath.header.stamp = laserOdometry.header.stamp;laserPath.poses.push_back(laserPose);laserPath.header.frame_id = "/camera_init";pubLaserPath.publish(laserPath);// transform corner features and plane features to the scan end point//去畸变，没有调用if (0){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]);}int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++){TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}}// 位姿估计完毕之后，当前边线点和平面点就变成了上一帧的边线点和平面点，把索引和数量都转移过去pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';// kdtree设置当前帧，用来下一帧lidar odom使用kdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);// 控制后端节点的执行频率，降频后给后端发送，只有整除时才发布结果if (frameCount % skipFrameNum == 0){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);}printf("publication time %f ms \n", t_pub.toc());  printf("whole laserOdometry time %f ms \n \n", t_whole.toc());if(t_whole.toc() > 100)  //里程计超过100ms则有问题ROS_WARN("odometry process over 100ms");frameCount++;}rate.sleep();}return 0;
}

回调函数注释如下：

// 操作都是送去各自的队列中，加了线程锁以避免线程数据冲突
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{mBuf.lock();cornerSharpBuf.push(cornerPointsSharp2);mBuf.unlock();
}void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{mBuf.lock();cornerLessSharpBuf.push(cornerPointsLessSharp2);mBuf.unlock();
}void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{mBuf.lock();surfFlatBuf.push(surfPointsFlat2);mBuf.unlock();
}void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{mBuf.lock();surfLessFlatBuf.push(surfPointsLessFlat2);mBuf.unlock();
}//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{mBuf.lock();fullPointsBuf.push(laserCloudFullRes2);mBuf.unlock();
}

激光雷达在运动过程中存在一定的运动畸变，即同一帧点云中，各个点在采集时，LiDAR的位姿是不同的，就如同高速移动相机时拍摄的照片一样。

那么如何进行运动补偿呢？即将所有的点云补偿到某一个时刻。

常用的做法是补偿到起始时刻，如果有IMU，我们通过IMU得到的雷达高频位姿，可以求出每个点相对起始点的位姿，就可以补偿回去。

如果没有IMU，我们可以使用匀速模型假设，使⽤上⼀个帧间⾥程记的结果作为当前两帧之间的运动，假设当前帧也是匀速运动，可以估计出每个点相对起始时刻的位姿。

最后，当前点云中的点相对第一个点去除因运动产生的畸变，效果相当于静止扫描得到的点云。下面是去除运动畸变的函数。

// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{//interpolation ratiodouble s;// 由于kitti数据集上的lidar已经做过了运动补偿，因此这里就不做具体补偿了if (DISTORTION)s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;elses = 1.0;    // s = 1.0说明全部补偿到点云结束的时刻// 所有点的操作方式都是一致的，相当于从结束时刻补偿到起始时刻，相当于是一个匀速模型的假设Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);Eigen::Vector3d t_point_last = s * t_last_curr;Eigen::Vector3d point(pi->x, pi->y, pi->z);Eigen::Vector3d un_point = q_point_last * point + t_point_last;po->x = un_point.x();po->y = un_point.y();po->z = un_point.z();po->intensity = pi->intensity;
}

下面介绍下ceres优化问题如何求解：定义一个模板类，重写优化问题，在这个类中定义残差，重载（）运算符，（）运算符函数前⼏个参数是参数块的起始指针，最后⼀个参数是残差块的指针，（）运算符负责计算残差。下面是优化边线点的模板类程序。

struct LidarEdgeFactor
{LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,Eigen::Vector3d last_point_b_, double s_): curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}template <typename T>bool operator()(const T *q, const T *t, T *residual) const{// 将double数组转成eigen的数据结构，注意这里必须都写成模板Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 计算的是上一帧到当前帧的位姿变换，因此根据匀速模型，计算该点对应的位姿// 这里暂时不考虑畸变，因此这里不做任何变换q_last_curr = q_identity.slerp(T(s), q_last_curr);Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};Eigen::Matrix<T, 3, 1> lp;// 把当前点根据当前计算的帧间位姿变换到上一帧lp = q_last_curr * cp + t_last_curr;Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);    // 模是三角形的面积Eigen::Matrix<T, 3, 1> de = lpa - lpb;// 残差的模是该点到底边的垂线长度// 这里感觉不需要定义三维residual[0] = nu.x() / de.norm();residual[1] = nu.y() / de.norm();residual[2] = nu.z() / de.norm();return true;}static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,const Eigen::Vector3d last_point_b_, const double s_){return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));}Eigen::Vector3d curr_point, last_point_a, last_point_b;double s;
};

下面是优化面点的模板类程序。

struct LidarPlaneFactor
{LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_): curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),last_point_m(last_point_m_), s(s_){// 求出平面单位法向量ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);ljm_norm.normalize();}template <typename T>bool operator()(const T *q, const T *t, T *residual) const{Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};// 根据时间戳进行插值q_last_curr = q_identity.slerp(T(s), q_last_curr);Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};Eigen::Matrix<T, 3, 1> lp;lp = q_last_curr * cp + t_last_curr;// 点到平面的距离residual[0] = (lp - lpj).dot(ljm);return true;}static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,const double s_){return (new ceres::AutoDiffCostFunction<LidarPlaneFactor, 1, 4, 3>(new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));}Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;Eigen::Vector3d ljm_norm;double s;
};

后面再详细讲解ceres优化器的使用，如何设计参数等。

下一节讲解laserMapping.cpp。

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计相关推荐

六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十九.激光和惯导LIO-SLAM框架学习之项目工程代码介绍---代码框架和一些文件解释
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
激光SLAM入门学习笔记
激光SLAM入门学习笔记激光SLAM入门学习笔记一.推荐阅读书籍二.推荐公众号.知乎.博客 1.公众号 2.知乎 3.博客三.推荐阅读论文&代码(参考泡泡机器人) 2D激光SLAM 3 ...
激光slam课程学习笔记－－第2课：2D激光slam
前言:这系列笔记是学习曾书格老师的激光slam课程所得,这里分享只是个人理解,有误之处,望大佬们赐教.这节课介绍的是2D激光slam. 1. 2d激光slam的介绍激光slam的输入:IMU数据,里 ...
LOAM：实时的雷达里程计和建图
之前对视觉SLAM主要的开源框架,ORB-SLAM2.SVO等进行了介绍,然后疫情期间对VINS-Mono进行了详细的源码解析,接下来考虑到工作原因需要用到激光雷达.GNSS.IMU等多传感器融合,所 ...
彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总！
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计相关推荐

最新文章

热门文章