2.2laserOdometry.cpp
包含以下函数
-
void TransformToStart //将当前帧点位位姿转换到此帧的初始位姿
-
void TransformToEnd //将当前帧点位位姿转换到此帧的结束位姿
-
void laserCloudSharpHandler
-
void laserCloudLessSharpHandler
-
void laserCloudFlatHandler
-
void laserCloudLessFlatHandler
-
void laserCloudFullResHandler //多线程数据订阅
-
int main //主函数
1、设置节点 laserOdometry
ros::init(argc,argv."laserOdometry"); ros::NodeHandle nh; nh.param<int>("mapping_skip_frame",skipFrameNum,2);
2、调用3-7的回调函数,订阅提取出来的 大角点,一般角点,大面点,一般面点和所有点云
/订阅提取出来的点云,角点、面点 ros::Subscriber subCornerPointsSharp = nh.subsribe<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);
3、发布上一帧的角点,面点,所有点,里程计,轨迹等消息
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); 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);
4、进入while循环
while (ros::ok())
1)触发一次回调,订阅当前帧的角点,面点和所有点
ros::spinOnce();
2)将五个点云消息分别取出来,同时转换成PCL格式
//首先确保订阅的五个消息都有,有一个队列为箜都不行 if(!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() && !surfFlatBuf.empty() && !surfLessFlatBuf.empty() && !fullPointsBuf.empty()) { //分别求出队列第一个时间 timeCornerPointsSharp = cornerSharpBuf.front()->healder.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(timeCornerPointsLessSharp != timeLaserCloudFullRes || timeCornerPointsLessSharp != timeLaserCloudFullRes || timeSurfPointsFlat != timeLaserCloudFullRes || timeSurfPointsLessFlat != timeLaserCloudFullRes) { printf("unsyn messeage!"); ROS_BREAK; } //将五个点云消息扽别取出来,同时转换成PCL格式 //同时进行 mBuf.lock(); // cornerPointsSharp ->clear(); //上面定义的Pcl形式的指针 pcl::fromROSMsg(*cornerSharpBuf.front() , *cornerPointsSharp); // conerSharpBuf.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();
3)取出比较突出的特征,特征提取中的2个角点和2个面点 :当前帧比较明显的角点和面点和上一帧比较明显的角点和面点 建立约束[]
//lidar里程计的求解 //取出比较突出的特征,特征提取中的2个角点和2个面点 :当前帧比较明显的角点和面点和上一帧比较明显的角点和面点 建立约束 int cornerPointsSharpNum = cornerPointsSharp->points.size(); int surfPointsFlatNum = surfPointsFlat->points.size(); TicToc t_opt; //进行两次迭代 for( size_t opti_counter = 0; opti_counter < 2; ++opti_counter) { corner_correspondence = 0; //关联点 plane_correspondence = 0; //ceres::LossFunction *loss_function =FULL; //定义一个ceres核函数 ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); //当残差大于0.1则降低其权重 //由于旋转不满足一般意义的加法,这里使用ceres自带的local_para ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization(); //自定义四元数加法 ceres::Problem::Options problem_options; //定义ceres的问题 即初始化 ceres::Problem problem(problem_options); //待优化的变量是帧间位姿,平移和旋转,这里旋转使用四元数表示 //添加参数块 problem.AddParameterBlock(para_q,4,q_parameterization); //用这个参数告诉ceres优化器我们需要优化的参数 ,数组首指针,参数长度4 problem.AddParwmeterBlock(para_t,3); pcl::PointXYZI pointSel; std::vector<int> PointSearchInd; std::vector<float> pointSearchSqDis;
4)寻找角点约束
寻找不同线束上的最近两个角点,计算残差
for(int i=0; i < cornerPointsSharpNum; ++i) { //运动补偿 TransformToStart(&cornerPointsSharp->points[i],&pointSel); //把它补偿到帧的开始时刻,并输出新的坐标 //在上一帧所有的角点构成的kdtree中寻找距离当前帧最近的一个点 id 和 距离 kdtreeCornerLast->nearestKSearch(pointSel, 1, PointSearchInd, pointSearchSqDis); int closestPointInd =-1; minPointInd = -1; //只有小于给定门限才认为是有效约束 if(pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) //DISTANCE_SQ_THRESHOLD=25距离阈值 帧间距离 { cloestPointInd = PointSearchInd[0]; //对应的最近距离的索引 //找到其所在的线束id,,线束信息隐藏在intensity的整数部分 int closestPointScanID = int(laserCloudCornerLast->points[cloestPointInd].intensity); double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; //寻找角点 ,在刚刚角点id上下继续寻找,目的是找到最近的角点,由于其按照线束进行排序,所以向上找 for (int j=closestPointScanID +1; j<(int)laserCloudCornerLast->points.size(); ++j) { //不找同一根线束的 if( int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) continue; //要求找到的线束距离当前线束不能太远 NEARBY_SCAN=2.5 上下四条线 if(int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) break; //计算和当前找到的角点之间的距离 double pointSqDis =(laserCloudCornerLast->points[j].x-pointSet.x)* (laserCloudCornerLast->points[j].x-pointSet.x) + (laserCloudCornerLast->points[j].y-pointSet.y)* (laserCloudCornerLast->points[j].y-pointSet.y) + (laserCloudCornerLast->points[j].z-pointSet.z) * (laserCloudCornerLast->points[j].z-pointSet.z); //寻找距离最小的角点及其索引 if(PointSqDis < minPointSqDis2) { //记录其索引 minPointSqDis2 = pointSqDis; minPointInd2 = j; } } //往另一个方向寻找 for (int j=closestPointScanID -1; j (int)laserCloudCornerLast->points.size(); --j) { if( int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) continue; //要求找到的线束距离当前线束不能太远 if(int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) break; double pointSqDis =(laserCloudCornerLast->points[j].x-pointSet.x)* (laserCloudCornerLast->points[j].x-pointSet.x) + (laserCloudCornerLast->points[j].y-pointSet.y)* (laserCloudCornerLast->points[j].y-pointSet.y) + (laserCloudCornerLast->points[j].z-pointSet.z) * (laserCloudCornerLast->points[j].z-pointSet.z); //寻找距离最小的角点及其索引 if(PointSqDis < minPointSqDis2) { //记录其索引 minPointSqDis2 = pointSqDis; minPointInd2 = j; } } } // 如果这个角点是一个有效的角点 if (minPointInd2 >=0) { //取出当前点和上一帧的两个角点 Eigen::Vector curr_point(conerPointsSharp->points[i].x, conerPointsSharp->points[i].y, conerPointsSharp->points[i].z); Eigen::Vector last_point_a(cornerPointsSharp ->points[closestPointInd].x, cornerPointsSharp ->points[closestPointInd].y,cornerPointsSharp ->points[closestPointInd].z ); Eigen::Vector last_point_b(cornerPointsSharp ->points[minPointInd2].x, cornerPointsSharp ->points[minPointInd2].y, cornerPointsSharp ->points[minPointInd2].z); double s; if (DISTORTION) s = (cornerPointsSharp->points[i].intensity - int(cornerPointSharp->points[i].intensity))/SCAN_PERIOD; else s=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 ++; } }
5)寻找面点的约束
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 int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity); double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD; // search in the direction of increasing scan line 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 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 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 higher scan line if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) { // find nearer point minPointSqDis3 = pointSqDis; minPointInd3 = j; } } if (minPointInd2 >= 0 && minPointInd3 >= 0) { Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, urfPointsFlat->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) s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; else s = 1.0; 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++; } } }
6)位姿求解
//调用ceres求解器求解 TicToc t_solver; ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; options.max_num_iterations = 4; //这里还是前端 options.miimizer_progress_to_stdout = false; ceres::Slover::Summary summary; ceres::Slover(options,&problem,&summary); } //w_curr=w_last 算出帧间的位姿变化 t_w_curr = t_w_curr + q_w_curr * t_last_curr; q_w_curr = q_w_curr * q_last_curr; }
7)发布里程计信息(旋转和平移),角点,面点和所有点,并建立当前帧角点面点的kdtree
//这里发布的odom坐标系下的位姿(直接得到的是lidar坐标系下的)
//后面发布的是map坐标系下的位姿
需要的是curr_to_odom odom_to_map
// wmap_T_odom * odom_T_curr = wmap_T_curr; // transformation between odom's world and map's world frame Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0); Eigen::Vector3d t_wmap_wodom(0, 0, 0); Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0); Eigen::Vector3d t_wodom_curr(0, 0, 0);