前言
ALOAM方法实现了低的漂移,并且计算的复杂度低,实时性很好.并且不需要高精度的lidar和惯导
这个方法的核心思想就是把SLAM问题进行了拆分,通过两个算法来进行.一个是执行高频率的前端里程计但是低精度的运动估计（定位）,另一个算法在比定位低一个数量级的频率执行后端建图（建图和校正里程计）.
这个两个算法都需要提特征点,就是经典的角点和面点,然后进行配准.在前端的那个算法中也就是里程计算法,特征点的提取会用到快速计算的方法.在建图的后端算法中,相互关联的特征点是通过特征值和特征向量来获得的.
本篇主要分析后端laserMapping的代码结构与数据处理
ALOAM 后端laserMapping
建图算法 运行 的频率要比里程计低
每一帧 执行 一次
在k+1帧的最后,激光里程计得到了经过畸变校正的点云 ^Pk+1,并且同时得到了 一个位姿变换 Tk+1.
建图算法就是将 ^Pk+1 配准到世界坐标系中.
Qk 是 已存在地图中的点云 是前k帧的累积
Tw 是 地图中 最后一帧 k 的位姿 在tk+1时刻
用来自激光里程计的输出,建图算法 将 ^Pk+1 投影到世界坐标系中,形成 ^Qk+1
然后就是配准 ^Qk+1 和Qk, 这个过程会优化lidar的位姿 Twk+1
订阅四种消息
- 当前帧全部点云(经过一次降采样)
- 上一帧的边线点集合
- 上一帧的平面点集合
- 当前帧的位姿粗估计
发布六种消息
- 附近5帧组成的降采样子地图 for rviz
- 所有帧组成的点云地图
- 经过Map to Map精估计优化后当前帧位姿精估计
- 当前帧原始点云（从velodyne_cloud_3订阅来的点云未经其他处理）
- 里程计坐标系位姿转化到世界坐标系位姿（地图坐标系），mapping输出的1Hz位姿，odometry输出的10Hz位姿，整合成10hz作为最终结果
- 经过Map to Map 精估计优化后的当前帧平移
代码解析
ALOAM的后端的模块是laserMapping .代码就是在laserMapping.cpp里面
laserMapping.cpp也是一个ROS的node.所以可以从main函数看起
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;ros的基本操作
//配置线特征和面特征点云的分辨率,之后通过体素滤波进行下采样
float lineRes = 0;//声明线特征分辨率
float planeRes = 0;//声明面特征分辨率
nh.param<float>("mapping_line_resolution", lineRes, 0.4);//从参数服务器读取线特征分辨率
nh.param<float>("mapping_plane_resolution", planeRes, 0.8);//从参数服务器读取面特征分辨率
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);//打印设置的分辨率
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);//设置线特征的分辨率
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);//设置面特征的分辨率配置线特征和面特征点云的分辨率,之后通过体素滤波进行下采样
然后订阅了4种消息
ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);- 上一帧的角点
- 上一帧的面点
- 前端里程计位姿
- 所有的点云
可以先看下各回调函数做了什么事情
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();//线程锁 加锁
cornerLastBuf.push(laserCloudCornerLast2);//存入Buf
mBuf.unlock();//线程锁 解锁
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}三个点云的回调函数处理很简单,就是存入到各自的数据Buf中
注意:回调函数要加线程锁,因为一个回调函数就是一个线程,主函数里还有一个线程,两个线程都会访问Buf信息,避免线程冲突,在存入数据前加锁,写入后解锁
然后来看里程计的回调函数做的事情
//前端激光雷达里程计回调函数
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{ mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();首先和点云一样写入Buf中
之后需要发布最新的位姿信息(这部分需要计算了).
前端里程计会定期向后端发送位姿,这个位姿是当前帧和里程计初始位姿直接的位姿(可以叫做T_odom_curr),在后端mapping模块中,需要以map坐标系为初始位姿,然后得到当前帧和map的位姿变换(可以叫做T_map_curr),如何进行这个两个位姿的转换呢?
后端mapping模块就是解决这个问题,它会估计出里程计初始位姿和map的初始位姿的位姿变换(可以叫做T_map_odom),然后
T_map_curr = T_map_odom×T_odom_curr
上面就是计算的原理
再补充一句,里程计坐标系和map坐标系在初始时刻是对齐的,然后里程计是累加转的时间坐标系下的,所以会随着时间的推移,越来越不准,通过后端的处理,map坐标系下的位姿会相对准一些
回到代码,就是的上面位姿变换的处理,收到一个odom的位姿,就发布一个map下的位姿
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;将ros的消息,转成eigen的格式
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 这个就是上面的公式,将odom的位姿转成map的位姿
q_wmap_wodom和t_wmap_wodom 后端优化的odom到map的位姿变换
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
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();
pubOdomAftMappedHighFrec.publish(odomAftMapped);转成ROS的里程计信息 发布出去
继续回到main函数中之后是定义本节点发布的消息(6种)
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);定义本节点发布的5种消息
//地图的数组进行 重置 分配地址
for (int i = 0; i < laserCloudNum; i++)
{
//后端地图的数组 用栅格的形式
laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
}地图的数组进行 重置 分配地址
std::thread mapping_process{process};主线程
process是一个 while(1)的循环
void process()
{
while(1)
{主循环一直运行. 后端的优化部分也就是在这主循环里一直执行.
这个就是ALOAM后端的整体代码结构
