前言
在之前的博客中对ALOAM的前端和后端做了代码详细的梳理
这里先对前几篇后端的内容做一个总结
- 这篇博客中，首先对订阅的前端的数据做了处理
- 这篇博客中，对地图进行栅格处理并根据当前帧位姿提取局部地图
- 这篇博客中，对当前帧中的角点和面点与局部地图的角点和面点构建Ceres约束
- 这篇博客中，通过Ceres进行位姿优化
- 这篇博客中，进行里程计到地图位姿更新维护
本篇为ALOAM后端的最终篇，地图更新及消息发布。
地图更新原因：
- 当地图调整之后，栅格有些空着的，需要进行填充
- 保障地图的实时更新
- 当前帧的点云加到地图中去，下帧会有更多的匹配点
代码解析
for (int i = 0; i < laserCloudCornerStackNum; i++)
{遍历角点
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);将该点转到地图坐标系下
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
//负数的四舍五入
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;计算该点的栅格索引
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{ 判断索引满足条件后
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;根据 xyz 的索引计算一维数组中的索引
laserCloudCornerArray[cubeInd]->push_back(pointSel);将该点加入 角点的 地图 对应的栅格索引 的 数组中
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}对于面点也是同样的操作
printf("add points time %f ms\n", t_add.toc());终端输出 添加点的时间
下面要做的是把 当前帧涉及到的局部地图的栅格做一个下采样
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];//栅格索引
// 对该栅格索引内的角点点云进行 体素 滤波
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
// 对该栅格索引内的面点点云进行 体素 滤波
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}遍历当前帧涉及的栅格个数
laserCloudValidNum 就是当前帧涉及的栅格个数，在上面通过取局部地图栅格的时候在for循环里做的累加
局部地图的选取规则就是通过判断当前帧位置的索引，然后在I和J方向前后取2个，K方向前后1个
当前帧所在栅格和局部地图，局部地图为了方便看在J方向上前后取了1个，实际为2个
因为当前帧大概率会落在这几个栅格中，所以仅对这几个栅格做滤波处理即可，在这几个栅格之外的点是很稀疏的，可以不处理，这样提高计算效率
printf("filter time %f ms \n", t_filter.toc());//终端输出滤波时间终端输出滤波时间
至此完成了整体的地图更新
下面要做的就是点云地图的发布
局部地图每隔5帧进行要给发布
//后端每处理5帧发布一个
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();//局部地图清空
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
//把 局部地图 角点和面点 都加入局部地图中
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
// 转成ROS的格式发布
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}局部地图清空
把 局部地图 角点和面点 都加入局部地图中
转成ROS的格式发布
//每隔 20帧 发布全量地图
if (frameCount % 20 == 0)
{
pcl::PointCloud<PointType> laserCloudMap;
// 21*21*11
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}每隔 20帧 发布全量地图
//当前帧发布
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);当前帧转到地图坐标系后发布
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);建图后里程计的发布
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);建图后path的发布
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));建图后tf的发布
总结
后端的地图更新总结
- 将该帧的所有角点转到地图坐标系下根据位置计算栅格索引，存入该栅格的点云数组中
- 将该帧的所有面点转到地图坐标系下根据位置计算栅格索引，存入该栅格的点云数组中
- 对添加点后的局部地图进行下采用
- 发布ROS的消息格式
后端消息发布总结
这时候可以根据main函数里面的advertise进行一个总结
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);- 局部地图 后端没处理 5 帧发布一次，当前帧的栅格索引的I、J方向前后2个，K方向前后一个
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);- 整体的栅格地图 后端每处理20帧发布一次。21 21 11的栅格尺寸，一个格子边长50m
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);- 发布当前帧到map 下的位姿 这个位姿是 准确的 是通过 优化 求得的 频率是 后端处理完一帧后即分布一帧的频率
pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);- 发布当前帧转到map坐标系下的位姿 高频率的位姿 通过上一帧的优化值，估计的
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);- 发布 path ，该 路径是准确的，通过 优化后求得的， 频率是 后端处理完一帧后即分布一帧的频率
通过在以往博客中搭建的仿真场景,进行相关消息的可视化
gazebo场景
频率为 大约 2hz
整体栅格地图
频率大约为 0.5hz 也就是2s更新一次
当前帧转到地图坐标系的
