前言
A-LOAM的cpp有四个,其中 kittiHelper.cpp 的作用是将kitti数据集转为rosbag
剩下的三个是作为 slam 的 部分,分别是:
- laserMappin.cpp ++++ 当前帧到地图的优化
- laserOdometry.cpp ++++ 帧间里程计
- scanRegistration.cpp ++++ 前端lidar点预处理及特征提取
本篇主要解读 帧间里程计部分的代码
对于前端的lidar点预处理及特征提取 在前面分析过了,链接:A-LOAM :前端lidar点特征提取部分代码解读
这部分在最后发布了5种topic.
- 所有的点云
- 角点
- 弱角点
- 面点
- 弱面点
帧间里程计则订阅这5种topic,并根据相邻两帧的特征点,优化出两帧间的位姿.
帧间里程计的代码在laserOdometry.cpp中
帧间里程计在计算相邻帧位姿的时候是通过ceres优化的,关于这部分优化的内容在这篇博客中:ALOAM:Ceres 优化部分及代码解析
激光雷达里程计原理
tk是第k帧lidar的开始时间
点云在这一帧结束的过程中,逐渐接收点 形成点云Pk
tk+1 是 第k帧雷达结束的时间 ,将k帧里面的所有点都投影到tk+1时刻的点上,形成点云 ^Pk
在下一帧点云(k+1)帧形成的时候,^Pk用来和新接受的点云(第k+1帧点云 Pk+1)一起,估计lidar的运动
假设 ^Pk 和 Pk+1 都是可用的了,然后开始找出特征点的匹配对.
对于Pk+1,找出边缘点和面点,用上一节曲率的方法. 用Ek+1和Hk+1来代表 边缘点集合和面点集合
我们将会找出^Pk 中的与之对应的边缘点和面点
在第k+1帧开始的时候,Pk+1还是空的,点在之后逐渐接收
lidar里程计在k+1帧开始接收的时候,递归的估计6自由度的运动,
在每一次迭代,用当前估计的变换,将Ek+1和Hk+1投影到tk+1时刻的坐标系中,用 ^Ek+1 和 ^Hk+1 表示投影后的点集
对于在^Ek+1 和 ^Hk+1中的每个点,需要找到在 ^Pk 中的距离最近的点 ,通过3d KD-tree的方法 . ^Pk 存在一个KD-tree中
代码解读
帧间里程计是一个单独的节点,所以整个代码可以从main函数开始
int main(int argc, char **argv)
{
//初始化节点 laserOdometry
ros::init(argc, argv, "laserOdometry");
//声明句柄
ros::NodeHandle nh;ros的基本操作
初始化节点
声明句柄
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//从服务器读取 参数 mapping_skip_frame 下采样的频率, 向后端发送数据的频率 launch文件中设置 设置为1
// 如果为 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -
nh.param<int>("mapping_skip_frame", skipFrameNum, 2);
//打印 建图的频率
printf("Mapping %d Hz \n", 10 / skipFrameNum);
从服务器读取 参数 mapping_skip_frame 下采样的频率 launch文件中设置 设置为1
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 订阅一些点云的topic 100是队列
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);//所有的点
订阅一些点云的topic 100是队列
角点
弱角点
面点
弱面点
所有的点
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//这个节点要发布的topic
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);这个节点要发布的topic
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
while (ros::ok())//整个while非常大
{
ros::spinOnce();//触发一次回调 spin是很个消息都及时处理 这样会丢帧开始整个while,非常大
首先调用spinOnce 触发一次回调 spin是很个消息都及时处理 这样会丢帧
下面看下各topic的回调函数
//下面的回调函数 都是将接收的点云存入 各自的队列当中
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();
}都是将接收的点云存入 各自的队列当中
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;存入的队列都是标准的 std::queue
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//判断五个消息的队列是否为空 为空则不进行整个功能的运行 进行按while的频率运行spinonce
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
//取出队列第一个的时间
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);//转成pcl的点云格式
cornerSharpBuf.pop();//去掉队列里面的第一个
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);//转成pcl的点云格式
cornerLessSharpBuf.pop();//去掉队列里面的第一个
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);//转成pcl的点云格式
surfFlatBuf.pop();//去掉队列里面的第一个
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);//转成pcl的点云格式
surfLessFlatBuf.pop();//去掉队列里面的第一个
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);//转成pcl的点云格式
fullPointsBuf.pop();//去掉队列里面的第一个
mBuf.unlock();分别将五个点云取出来,同时转成pcl的点云格式
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// initializing 进行初始化 等有两帧了才行
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{//初始化完成 继续做真正的工作进行初始化 等有两帧了才行
初始化完成 继续做真正的工作
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
在上面完成了将从上一个节点订阅的信息取出来,转成pcl形式,并完初始化工作,初始化工作就是等有两帧数据
然后继续看else里面的内容
//取出角点和面点的 特征点 数量 相当于约束的大小,一个特征点一个约束 ceres一共6个约束
int cornerPointsSharpNum = cornerPointsSharp->points.size();//一般为2
int surfPointsFlatNum = surfPointsFlat->points.size();//一般为4然后一个for循环进行2次的迭代求解
//进行两次迭代求解
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0;//初始化 角点匹配的点对的数量
plane_correspondence = 0;//初始化 面点匹配的点对的数量初始化 角点\面点匹配的点对的数量
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//ceres::LossFunction *loss_function = NULL;
//定义 ceres 的 损失函数 0.1代表 残差大于0.1的点 ,则权重降低 小于0.1 则认为正常,不做特殊的处理
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
//由于旋转不满足一般的加法,所以要用 ceres自带的 local Param
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
//声明ceres 的 Problem::Options
ceres::Problem::Options problem_options;
//声明ceres 的problem
ceres::Problem problem(problem_options);
//待优化的变量是帧间位姿,平移和旋转 ,这里旋转使用四元数来表示
//para_q是一个数组的指针 后面跟参数的的长度 不符合普通加法则再设置 local Param
problem.AddParameterBlock(para_q, 4, q_parameterization);// 添加四元数的参数块
problem.AddParameterBlock(para_t, 3);//添加平移的参数块定义 ceres 的 损失函数
由于旋转不满足一般的加法,所以要用 ceres自带的 local Param
声明ceres 的 Problem::Options
声明ceres 的problem
添加四元数/平移的参数块
pcl::PointXYZI pointSel;//畸变校正后的点云
std::vector<int> pointSearchInd;//kdtree找到的最近邻点的id
std::vector<float> pointSearchSqDis;//kdtree找到的最近邻点的距离声明这三个变量
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
然后for寻循环,遍历每个角点,在里面完成角点的约束
for (int i = 0; i < cornerPointsSharpNum; ++i)
{ //运动补偿
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
//在上一帧所有角点(弱角点)构成的kdtree中寻找距离当前帧最近的一个点 因为前面有初始化的判断 所有 第二帧肯定有上一帧
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);将该帧的点进行运动补偿
然后在上一帧的弱角点中寻找最近邻点
//只有小于给定门限才认为是有效约束 DISTANCE_SQ_THRESHOLD 是25
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{判断下找到的最近距离,距离满足要求才去找那两个点
//找到的最近邻点的 id 索引取出来
closestPointInd = pointSearchInd[0];
//取出这个点的 scan id 存在intensity的整数部分 因为 下一个点 的 scanid 不能和 这个一样
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
//
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;找到的最近邻点的 id 索引取出来
取出这个点的 scan id 存在intensity的整数部分 因为 下一个点 的 scanid 不能和 这个一样
//寻找下一个角点,在刚刚角点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;//记录点的索引id
}
}寻找下一个角点,在刚刚角点id上下分别寻找,目的是找到最近的角点,由于其按照线束进行排序,所以就是向上找
要求找到的线束与当前线束不能太远
计算找到的点和 当前帧角点的距离
距离满足要求 那么就更新需要的最小距离, 最后得到的就是满足要求的 距离最小的点
//同样另一个方向寻找第2近的点
// 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;
}
}同样另一个方向寻找第2近的点
如果找到了有效的两个点,则进行ceres的角点约束的添加
//如果找到了有效的两个点
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);
//上一帧的a点
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
//上一帧的b点
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;//当前点的时间戳占比
else
s = 1.0;
//添加ceres的约束项 就是定义 CostFuction 代价函数
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
//给problem 添加 残差项
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
//角点的约束次数 加 1
corner_correspondence++;
}把当前帧的角点和上一帧角点里面找到点a和点b取出来
构建Ceres的代价函数,其中代价函数是核心内容,这部分拿出来单说
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// find correspondence for plane features
// 进行面点的约束
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,
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)
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++;
}
}
}面点的约束添加和角点差不多,不展开细说了
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
添加完角点和面点的约束后,则进行ceres的相关配置,然后进行优化了
//上面完成了ceres的约束添加
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);在进行了两次的优化后
进行位姿的累加
//把计算的当前帧和上一帧的变换累加,形成相对第一帧的位姿变换,也就是世界坐标系下的位姿
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;把计算的当前帧和上一帧的变换累加,形成相对第一帧的位姿变换,也就是世界坐标系下的位姿
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
之后发布里程计信息
// publish 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);用世界坐标系下的位姿发布里程计
/*发布path*/
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);用里程计的信息发布path
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
下面要做的就是为下一帧来做准备
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;//把当前帧弱角点保存为上一帧弱角点,为下一帧做准备
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;//把当前帧弱面点保存为上一帧弱面点,为下一帧做准备把当前帧弱角点保存为上一帧弱角点,为下一帧做准备
把当前帧弱面点保存为上一帧弱面点,为下一帧做准备
//统计 角点和面点数量
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();统计 角点和面点数量
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);设置 kdtree 为下次搜索做准备
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
下面要做的就是 发布 ROS的点云,给后面的点图优化订阅.
通过skipFrameNum控制发布的频率
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);
}这就是ALOAM的帧间里程计的完整内容.
