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);
扫一扫
3582
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
