devbox
笔触狂放9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

激光SLAM源码解析S-LOAM（二）激光里程计的计算_eigen::map t_last_curr(para_t);

作者：笔触狂放9 | 2024-02-18 00:50:02

赞

踩

eigen::map t_last_curr(para_t);

10Hz激光雷达点云帧，相临帧的时间间隔是0.1秒。在这0.1秒内激光雷达的位姿变化（平移和旋转）可由这两帧点云的配准计算出来。

以某一时刻为起点，累计帧间位姿变换，得到各帧时刻激光雷达相对起点的位姿（位置和方向），这就是里程计。

配准是求取帧间变换的关键，帧帧配准是指相邻帧点云的配准，比如，配对的双方分别是当前帧与上一帧。

点云共面算法是点云帧帧配准的主要方法。当一个点到一个平面的距离为0时，就认为这个点就与此平面共面。

lidarOdometry.cpp

代码注释较详细，如下所示。

  1. /**
  2.  * Created by haocaichao on 2021/8/29
  3.  *
  4.  * 激光里程计的计算（帧帧配准）
  5.  * （1）接收帧平面特征点云
  6.  * （2）计算当前帧与上一帧之间位姿变换（帧帧配准，ceres优化）
  7.  * （3）累计帧间变换，得到从起始帧开始的里程计
  8.  * （4）发布点云、里程计、轨迹、地图
  9.  */
  10.  
  11. #include "header.h"
  12.  
  13. typedef pcl::PointXYZI PointType;
  14.  
  15. int isDone=1;         //标识运算是否完成
  16. float planeMax=0.5;   //平面判断门槛值
  17. std::mutex mLock;      //多线程锁
  18. pcl::VoxelGrid<PointType> downSizeFilterMap;  //定义点云下采样对象，用于点云抽稀
  19.  
  20. /**
  21.  * 定义ceres优化中的代价函数
  22.  * 点到平面的距离计算用向量表示
  23.  * 在同一坐标系中，两点间(po,pa)向量值与平面法向量(norm)的点乘可得到距离
  24.  */
  25. struct PlaneFeatureCost{
  26.     const Eigen::Vector3d _po,_pa,_norm;
  27.  
  28.     PlaneFeatureCost(Eigen::Vector3d po,Eigen::Vector3d pa,Eigen::Vector3d norm):
  29.             _po(po),_pa(pa),_norm(norm){}
  30.  
  31.     template <typename T>
  32.     bool operator()(const T* q,const T* t,T* residual) const {
  33.         Eigen::Matrix<T,3,1> po_curr{T(_po[0]),T(_po[1]),T(_po[2])};
  34.         Eigen::Matrix<T,3,1> pa_last{T(_pa[0]),T(_pa[1]),T(_pa[2])};
  35.         Eigen::Matrix<T,3,1> p_norm{T(_norm[0]),T(_norm[1]),T(_norm[2])};
  36.         Eigen::Quaternion<T> q_last_curr{q[3],q[0],q[1],q[2]};   //用于坐标系变换统一
  37.         Eigen::Matrix<T,3,1> t_last_curr{t[0],t[1],t[2]};        //用于坐标系变换统一
  38.         Eigen::Matrix<T,3,1> po_last;
  39.         po_last=q_last_curr*po_curr+t_last_curr;
  40.         residual[0]=((po_last-pa_last).dot(p_norm));
  41.         return true;
  42.     }
  43. };
  44.  
  45. ros::Subscriber sub_plane_frame_cloud;      //接收平面特征点云
  46. ros::Publisher pub_plane_frame_cloud;       //发布平面特征点云
  47. ros::Publisher pub_frame_odometry;          //发布激光雷达里程计，由帧帧配准计算得到
  48. ros::Publisher pub_frame_odom_path;         //发布激光雷达运动轨迹
  49. ros::Publisher pub_sum_lidar_odom_cloud;    //发布拼接后的点云地图
  50. //存储当前帧点云
  51. pcl::PointCloud<PointType>::Ptr lastFramePlanePtr(new pcl::PointCloud<PointType>());
  52. //存储上一帧点云
  53. pcl::PointCloud<PointType>::Ptr currFramePlanePtr(new pcl::PointCloud<PointType>());
  54. //存储拼接后总点云
  55. pcl::PointCloud<PointType>::Ptr sumPlaneCloudPtr(new pcl::PointCloud<PointType>());
  56. std::queue<sensor_msgs::PointCloud2> planeQueue;  //定义点云消息队列
  57. nav_msgs::Path lidarPathInOdom;        //定义激光雷达运动轨迹
  58. std_msgs::Header currHead;             //定义ros消息头变量
  59. double timePlane=0;                    //定义平面点云帧时间戳变量
  60. int numFrame=0;                        //定义帧计数变量
  61. int flagStart=0;                       //定义是否开始标志
  62. double para_q[4]={0,0,0,1};            //定义长度为4的数组，用于构成四元数
  63. double para_t[3]={0,0,0};              //定义长度为3的数组，用于构成位移
  64. Eigen::Quaterniond q_0_curr(1,0,0,0);  //起点到当前帧，四元数
  65. Eigen::Vector3d t_0_curr(0,0,0);       //起点到当前帧，位移
  66. Eigen::Quaterniond q_0_last(1,0,0,0);  //起点到上一帧，四元数
  67. Eigen::Vector3d t_0_last(0,0,0);       //起点到上一帧，位移
  68. //上一帧到当前帧，四元数
  69. Eigen::Quaterniond q_last_curr=Eigen::Map<Eigen::Quaterniond>(para_q);
  70. //上一帧到当前帧，位移
  71. Eigen::Vector3d t_last_curr=Eigen::Map<Eigen::Vector3d>(para_t);
  72.  
  73. //将当前点坐标变换到上一帧坐标系中
  74. void transformToLast(PointType const *const pi,PointType *const po){
  75.     Eigen::Vector3d curr_point(pi->x,pi->y,pi->z);
  76.     Eigen::Vector3d proj_point;
  77.     proj_point=q_last_curr*curr_point+t_last_curr;
  78.     po->x=proj_point.x();
  79.     po->y=proj_point.y();
  80.     po->z=proj_point.z();
  81.     po->intensity=pi->intensity;
  82. }
  83.  
  84. //发布点云、里程计、轨迹、地图
  85. void publishResult(){
  86.     //累计帧间变换，得到从起点开始的里程计
  87.     q_0_curr = q_0_last * q_last_curr ;
  88.     t_0_curr = t_0_last + q_0_last * t_last_curr;
  89.     q_0_last = q_0_curr;
  90.     t_0_last = t_0_curr;
  91.     //发布里程计
  92.     nav_msgs::Odometry lidarOdometry;
  93.     lidarOdometry.header.frame_id = "map";
  94.     lidarOdometry.child_frame_id = "map_child";
  95.     lidarOdometry.header.stamp = currHead.stamp;
  96.     lidarOdometry.pose.pose.position.x = t_0_curr[0];
  97.     lidarOdometry.pose.pose.position.y = t_0_curr[1];
  98.     lidarOdometry.pose.pose.position.z = t_0_curr[2];
  99.     lidarOdometry.pose.pose.orientation.x = q_0_curr.x();
  100.     lidarOdometry.pose.pose.orientation.y = q_0_curr.y();
  101.     lidarOdometry.pose.pose.orientation.z = q_0_curr.z();
  102.     lidarOdometry.pose.pose.orientation.w = q_0_curr.w();
  103.     pub_frame_odometry.publish(lidarOdometry);
  104.     //发布里轨迹
  105.     geometry_msgs::PoseStamped pose_stamped;
  106.     pose_stamped.header.stamp = lidarOdometry.header.stamp;
  107.     pose_stamped.header.frame_id = "map";
  108.     pose_stamped.pose = lidarOdometry.pose.pose;
  109.     lidarPathInOdom.poses.push_back(pose_stamped);
  110.     lidarPathInOdom.header.stamp=lidarOdometry.header.stamp;
  111.     lidarPathInOdom.header.frame_id="map";
  112.     pub_frame_odom_path.publish(lidarPathInOdom);
  113.     //发布平面特征点云
  114.     sensor_msgs::PointCloud2 plane_frame_cloud_msgs;
  115.     pcl::toROSMsg(*currFramePlanePtr, plane_frame_cloud_msgs);
  116.     plane_frame_cloud_msgs.header.stamp = lidarOdometry.header.stamp;
  117.     plane_frame_cloud_msgs.header.frame_id = "map";
  118.     pub_plane_frame_cloud.publish(plane_frame_cloud_msgs);
  119.     //发布拼接点云地图
  120. //    if(numFrame % 10 == 0){
  121. //        double r,p,y;
  122. //        tf::Quaternion tq(q_0_curr.x(),q_0_curr.y(),q_0_curr.z(),q_0_curr.w());
  123. //        tf::Matrix3x3(tq).getRPY(r,p,y);
  124. //        Eigen::Affine3d transCurd ;
  125. //        pcl::getTransformation(t_0_curr.x(), t_0_curr.y(), t_0_curr.z(), r,p,y,transCurd);
  126. //        pcl::PointCloud<PointType>::Ptr cloud_res(new pcl::PointCloud<PointType>());
  127. //        pcl::transformPointCloud(*currFramePlanePtr, *cloud_res, transCurd);
  128. //        *sumPlaneCloudPtr += *cloud_res;
  129. //        pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
  130. //        downSizeFilterMap.setInputCloud(sumPlaneCloudPtr);
  131. //        downSizeFilterMap.filter(*cloud_temp);
  132. //        sensor_msgs::PointCloud2 res_cloud_msgs;
  133. //        pcl::toROSMsg(*cloud_temp, res_cloud_msgs);
  134. //        res_cloud_msgs.header.stamp = lidarOdometry.header.stamp;
  135. //        res_cloud_msgs.header.frame_id = "map";
  136. //        pub_sum_lidar_odom_cloud.publish(res_cloud_msgs);
  137. //    }
  138. }
  139.  
  140. //计算帧帧配准，得到帧间位姿变换
  141. void frameRegistration(){
  142.     //定义ceres优化对象与参数
  143.     ceres::LossFunction *loss_function=new ceres::HuberLoss(0.1);
  144.     ceres::LocalParameterization *q_parameterization=new ceres::EigenQuaternionParameterization();
  145.     ceres::Problem problem;
  146.     problem.AddParameterBlock(para_q,4,q_parameterization);
  147.     problem.AddParameterBlock(para_t,3);
  148.  
  149.     pcl::KdTreeFLANN<PointType> kdTreePlanLast;
  150.     int last_plane_num=lastFramePlanePtr->points.size();
  151.     int curr_plane_num=currFramePlanePtr->points.size();
  152.     if(last_plane_num>10){
  153.         kdTreePlanLast.setInputCloud(lastFramePlanePtr);
  154.         for(int i_opt=0;i_opt<2;i_opt++){
  155.             for (int i = 0; i < curr_plane_num; ++i) {    //遍历当前帧各平面点
  156.                 PointType pointSeed;
  157.                 //将当前帧此平面点坐标变换到上一帧坐标系中
  158.                 transformToLast(&currFramePlanePtr->points[i],&pointSeed);
  159.                 std::vector<float> pointSearchSqDis1;
  160.                 std::vector<int> indx1;
  161.                 //将变换后的此点作为种子点，查找上一帧中距离此点最近点的索引
  162.                 kdTreePlanLast.nearestKSearch(pointSeed,1,indx1,pointSearchSqDis1);
  163.                 int p_ind_a=indx1[0];
  164.                 std::vector<float> pointSearchSqDis2;
  165.                 std::vector<int> indx2;
  166.                 //将上面最近点作为种子点，查找上一帧中距离最近点的索引和距离
  167.                 kdTreePlanLast.nearestKSearch(lastFramePlanePtr->points[p_ind_a],30,indx2,pointSearchSqDis2);
  168.                 std::vector<int> v_indx5;
  169.                 std::vector<int> v_indx_row;
  170.                 int p_row=-1;
  171.                 if(indx2.size()<5) continue;
  172.                 int n=5;
  173.                 //挑选5个最近点，尽量满足有2个点不属于同一扫描线
  174.                 for (size_t i_kd = 0; i_kd < indx2.size(); ++i_kd) {
  175.                     float f_indx=lastFramePlanePtr->points[indx2[i_kd]].intensity;
  176.                     int i_indx=int(f_indx);
  177.                     int row=100*(f_indx-i_indx+0.002);   //获取点索引
  178.                     if(i_kd==0){
  179.                         p_row=row;
  180.                     }
  181.                     if(i_kd<5){       //先将最近5个点选入
  182.                         v_indx5.push_back(indx2[i_kd]);
  183.                     }else{            //从第6个点开始，寻找与记录不同线的最近2个点
  184.                         if(row != p_row && row>=0 && row <=63){
  185.                             v_indx_row.push_back(indx2[i_kd]);
  186.                             n=i_kd;
  187.                             if(v_indx_row.size()>=2){
  188.                                 break;
  189.                             }
  190.                         }
  191.                     }
  192.                 }
  193.                 if(v_indx_row.size()==1){       //如果不同线的只有1个点
  194.                     v_indx5[4]=v_indx_row[0];
  195.                 }
  196.                 if(v_indx_row.size()==2){       //如果不同线的有2个点
  197.                     v_indx5[3]=v_indx_row[0];
  198.                     v_indx5[4]=v_indx_row[1];
  199.                 }
  200.  
  201.                 if(pointSearchSqDis2[n]<1){     //如果5个点中最远的点小于1米，则利用5点估算平面法线
  202.                     Eigen::Matrix<float, 5, 3> matA0;
  203.                     Eigen::Matrix<float, 5, 1> matB0;
  204.                     Eigen::Vector3f matX0;
  205.                     matA0.setZero();
  206.                     matB0.fill(-1);
  207.                     matX0.setZero();
  208.                     for (int j = 0; j < 5; ++j) {
  209.                         matA0(j,0)=lastFramePlanePtr->points[v_indx5[j]].x;
  210.                         matA0(j,1)=lastFramePlanePtr->points[v_indx5[j]].y;
  211.                         matA0(j,2)=lastFramePlanePtr->points[v_indx5[j]].z;
  212.                     }
  213.                     matX0=matA0.colPivHouseholderQr().solve(matB0);
  214.                     matX0.normalize();  //norm
  215.                     bool planeValid = true;
  216.                     for (int k = 0; k < 4; ++k) {   //利用法向量计算各点到平面的距离
  217.                         Eigen::Vector3d v_temp(
  218.                                 lastFramePlanePtr->points[v_indx5[k]].x-lastFramePlanePtr->points[v_indx5[k+1]].x,
  219.                                 lastFramePlanePtr->points[v_indx5[k]].y-lastFramePlanePtr->points[v_indx5[k+1]].y,
  220.                                 lastFramePlanePtr->points[v_indx5[k]].z-lastFramePlanePtr->points[v_indx5[k+1]].z
  221.                         );
  222.                         if(fabs(matX0(0)*v_temp[0]+matX0(1)*v_temp[1]+matX0(2)*v_temp[2])>planeMax){
  223.                             planeValid=false;       //如果有点到平面的距离太大，则说明此5点不共面
  224.                             break;
  225.                         }
  226.                     }
  227.                     Eigen::Vector3d po(currFramePlanePtr->points[i].x,currFramePlanePtr->points[i].y,currFramePlanePtr->points[i].z);
  228.                     Eigen::Vector3d pa(lastFramePlanePtr->points[p_ind_a].x,lastFramePlanePtr->points[p_ind_a].y,lastFramePlanePtr->points[p_ind_a].z);
  229.                     Eigen::Vector3d norm(matX0[0],matX0[1],matX0[2]);
  230.                     if(planeValid){                 //当找到了共面点，就利用种子点、最近点、平面法向量，构造点与平面共面的优化条件
  231.                         problem.AddResidualBlock(new ceres::AutoDiffCostFunction<PlaneFeatureCost,1,4,3>
  232.                                                          (new PlaneFeatureCost(po,pa,norm)),loss_function,para_q,para_t);
  233.                     }
  234.                 }
  235.             }
  236.         }
  237.         //设置优化参数，并优化
  238.         ceres::Solver::Options options;
  239.         options.linear_solver_type=ceres::DENSE_QR;
  240.         options.max_num_iterations=8;
  241.         options.minimizer_progress_to_stdout=false;
  242.         ceres::Solver::Summary summary;
  243.         ceres::Solve(options,&problem,&summary);
  244.         //得到优化结果，构成帧间变换的方向四元数与位移
  245.         q_last_curr=Eigen::Map<Eigen::Quaterniond>(para_q);
  246.         t_last_curr=Eigen::Map<Eigen::Vector3d>(para_t);
  247.     }
  248. }
  249.  
  250. //接收平面特征点云，添加到消息队列中
  251. void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &cldMsg) {
  252.     mLock.lock();
  253.     planeQueue.push(*cldMsg);
  254.     mLock.unlock();
  255. }
  256.  
  257. //点云处理多线程函数
  258. void cloudThread(){
  259.     ros::Rate rate(10);                //控制处理频率为10hz，为最终频率
  260.     while(ros::ok()){
  261.         rate.sleep();
  262.         ros::Rate rate2(50);           //内层处理频率为50hz
  263.         if(isDone==0) continue;
  264.         while (planeQueue.size()>0){
  265.             if(isDone==0) continue;
  266.             isDone=0;
  267.             numFrame++;
  268.             rate2.sleep();
  269.  
  270.             mLock.lock();               //锁线程，取数据
  271.             currFramePlanePtr->clear();
  272.             currHead=planeQueue.front().header;
  273.             timePlane=planeQueue.front().header.stamp.toSec();
  274.             pcl::fromROSMsg(planeQueue.front(),*currFramePlanePtr);
  275.             planeQueue.pop();
  276.             mLock.unlock();
  277.  
  278.             if(flagStart==0){          //标志起始帧
  279.                 flagStart=1;
  280.             }else{                     //从第二帧开始计算帧帧配准
  281.                 frameRegistration();
  282.                 publishResult();
  283.             }
  284.             *lastFramePlanePtr=*currFramePlanePtr;
  285.             isDone=1;
  286.         }
  287.     }
  288. }
  289.  
  290. int main(int argc, char **argv) {
  291.     if(N_SCAN_ROW==16){
  292.         planeMax=0.15;
  293.     }
  294.     if(N_SCAN_ROW==64){
  295.         planeMax=0.05;
  296.     }
  297.     downSizeFilterMap.setLeafSize(0.4,0.4,0.4);
  298.  
  299.     ros::init(argc, argv, "LidarOdometry");
  300.     ros::NodeHandle nh;
  301.     sub_plane_frame_cloud = nh.subscribe<sensor_msgs::PointCloud2>("/plane_frame_cloud1", 10, cloudHandler);
  302.     pub_plane_frame_cloud=nh.advertise<sensor_msgs::PointCloud2>("/plane_frame_cloud2",100);
  303.     pub_frame_odometry = nh.advertise<nav_msgs::Odometry>("/frame_odom2", 100);
  304.     pub_frame_odom_path = nh.advertise<nav_msgs::Path>("/frame_odom_path2", 100);
  305.     pub_sum_lidar_odom_cloud = nh.advertise<sensor_msgs::PointCloud2>("/sum_lidar_odom_cloud2", 10);
  306.     ROS_INFO("\033[1;32m----> LidarOdometry Started.\033[0m");
  307.     std::thread t_cloud_thread(cloudThread);
  308.     ros::spin();
  309.     t_cloud_thread.join();
  310.     return 0;
  311. }

代码地址为（GitHub - haocaichao/S-LOAM: S-LOAM(Simple LOAM) 是一种简单易学的激光SLAM算法，整个程序只有几百行代码，十分方便学习与试验分析。）。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/笔触狂放9/article/detail/102983
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号