cartographer 点云数据的预处理
目录
传感器数据的走向
体素滤波与之后的处理
3D情况下的激光雷达数据的预处理
初始位姿估计
位姿推测器的优缺点分析与总结
可能有问题的点
可能的改进建议
传感器数据的走向
传感器数据从 CollatedTrajectoryBuilder类的HandleCollatedSensorData函数 传递GlobalTrajectoryBuilder类 的相应函数
从这里开始, 传感器数据才真正进入到SLAM的前端与后端部分
运动畸变去除后的点云, 点的坐标相对于local_frame了, 点云依然围绕着tracking_frame
sensor::RangefinderPoint hit_in_local = range_data_poses[i] * sensor::ToRangefinderPoint(hit);
以点云的时间(也就是最后一个点的时间)预测出来的坐标做为点云的origin
accumulated_range_data_.origin = range_data_poses.back().translation()
计算从tracking_frame变换到local_frame原点并且变换后姿态为0的坐标变换
transform_to_gravity_aligned_frame = gravity_alignment.cast<float>() *
range_data_poses.back().inverse()
将点云进行平移与旋转, 点的坐标相对于local_frame, 点云围绕这local_frame坐标系原点
sensor::TransformRangeData(range_data, transform_to_gravity_aligned_frame)
进行z轴的过滤
sensor::CropRangeData(sensor::TransformRangeData(range_data,
transform_to_gravity_aligned_frame), options_.min_z(), options_.max_z())
单线雷达不能设置 大于0的min_z, 因为单线雷达的z为0
体素滤波与之后的处理
分别对 returns点云 与 misses点云 进行体素滤波
sensor::RangeData{ cropped.origin,
sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};对 returns点云 进行自适应体素滤波
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
options_.adaptive_voxel_filter_options())
将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云
TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d-
>cast<float>()) )
将 原点位于匹配后的位姿处的点云 返回到node.cc 中, node.cc将这个点云发布出去, 在rviz中可视化
3D情况下的激光雷达数据的预处理
进行多个雷达点云数据的时间同步
对点云进行第一次体素滤波
激光雷达数据运动畸变的校正, 同时将点云的相对于tracking_frame的点坐标 转成 相对于local slam坐标系的 点坐标
分别对 returns 与 misses 进行第二次体素滤波
将原点位于机器人当前位姿处的点云 转成 原点位于local坐标系原点处的点云
使用高分辨率进行自适应体素滤波 生成高分辨率点云
使用低分辨率进行自适应体素滤波 生成低分辨率点云
将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云
将 原点位于匹配后的位姿处的点云 返回到node.cc 中, node.cc将这个点云发布出去, 在rviz中可视化
初始位姿估计
基于IMU与里程计的位姿推测器
重要成员变量说明
姿态预测相关
imu_tracker_ 只在添加位姿时更新, 用于保存添加校位姿准时的姿态
odometry_imu_tracker_ 只在添加位姿时更新, 用于根据里程计数据计算线速度时姿态的预测
extrapolation_imu_tracker_ 只在添加位姿时更新, 用于位姿预测时的姿态预测
通过里程计计算的线速度与角速度
linear_velocity_from_odometry_ 只在添加里程计数据时更新, 用于位姿预测时的平移量预测
angular_velocity_from_odometry_ 只在添加里程计数据时更新, 用于不使用imu数据时的imu_tracker_的角 速度的更新
通过pose计算的线速度与角速度
linear_velocity_from_poses_ 只在添加位姿时更新, 用于位姿预测时 不使用里程计数据时 平移量的预测
angular_velocity_from_poses_ 只在添加位姿时更新, 用于 不使用里程计数据时 的imu_tracker_的角速度的更新
传感器数据队列的个数
imu_date_ 队列数据的个数最少是1个
odometry_data_ 队列数据的个数最少是2个, 所以, odometry_data_.size() < 2 就意味着不使用里程计
timed_pose_queue_ 队列数据的个数最少是2个
位姿推测器的优缺点分析与总结
预测位姿时的4种情况 都是匀速模型
使用imu, 使用里程计
平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过imu的角速度乘以时间
使用imu, 不使用里程计
平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过imu的角速度乘以时间
不使用imu, 使用里程计
平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过里程计数据队列开始和末尾的2个数据计算出的角速度乘以时间
不使用imu, 不是用里程计
平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过pose数据队列开始和末尾的2个数据计算出的角速度乘以时间
总结:
预测平移时: 有里程计就用里程计的线速度, 没有里程计就用pose计算的线速度进行预测
预测姿态时: 有IMU就用IMU的角速度, 没有IMU时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用
pose计算的角速度进行预测
预测的都是相对值, 要加上最后一个pose的位姿
可能有问题的点
计算pose的线速度与角速度时, 是采用的数据队列开始和末尾的2个数据计算的
计算里程计的线速度与角速度时, 是采用的数据队列开始和末尾的2个数据计算的
使用里程计, 不使用imu时, 计算里程计的线速度方向和姿态的预测时, 用的是里程计数据队列开始和末尾的2个
数据的平均角速度计算的, 时间长了就不准
不使用里程计, 不使用imu时, 用的是pose数据队列开始和末尾的2个数据的平均角速度计算的, 时间长了就不 准
添加位姿时, 没有用pose的姿态对imu_tracker_进行校准, 也没有对整体位姿预测器进行校准, 只计算了pose 的线速度与角速度
从代码上看, cartographer认为位姿推测器推测出来的位姿与姿态是准确的
可能的改进建议
pose的距离越小, 匀速模型越能代替机器人的线速度与角速度, 计算pose的线速度与角速度时, 可以考虑使用 最近的2个数据进行计算
里程计距离越短数据越准, 计算里程计的线速度与角速度时, 可以考虑使用最近的2个数据进行计算
使用里程计, 不使用imu时, 计算里程计的线速度方向时, 可以考虑使用里程计的角度进行计算
使用里程计, 不使用imu时, 进行姿态的预测时, 可以考虑使用里程计的角度进行预测
不使用里程计, 不使用imu时, 可以考虑用最近的2个pose计算线速度与角速度
使用pose对imu_tracker_的航向角进行校准
