cartographer模块分析1: 前端激光里程计实现
前端里程计
- LocalTrajectoryBuilder2D类
-
- 1. LocalTrajectoryBuilder2D::AddRangeData
-
- 1.1 AddAccumulatedRangeData()
-
- 1.1.2 ScanMatch()
- 1.1.3 submap的更新
LocalTrajectoryBuilder2D类
该类是局部激光里程计的实现类.
该类有3个主要的函数,如下
// 添加激光雷达的数据 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 关键
std::unique_ptr<MatchingResult> AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& range_data);
// 添加IMU数据
void AddImuData(const sensor::ImuData& imu_data);
// 添加里程计数据
void AddOdometryData(const sensor::OdometryData& odometry_data);
上面3个函数是给外部调用的传感器数据处理的接口函数。
AddRangeData() 通过传入当前激光数据实现激光里程计。
AddImuData() 通过添加IMU数据更新位姿插值器。
AddOdometryData() 通过添加轮式里程计数据更新位姿插值器。
IMU与轮速计只是对位姿插值器造成影响，与激光里程计是隔离的，首先只考虑纯激光的情况, 后续再考虑IMU与编码器的融合, 因此 AddImuData(), AddOdometryData() 就先不看。
主要的成员有:
/***************************************************************私有成员********************************************/
// 设置器对象
const proto::LocalTrajectoryBuilderOptions2D options_;
// Submap ???????????????????????????????????????????
ActiveSubmaps2D active_submaps_;
// 运动滤波器 ??????????????????????????????????????????
MotionFilter motion_filter_;
// 相关匹配方法对象
scan_matching::RealTimeCorrelativeScanMatcher2D
real_time_correlative_scan_matcher_;
// 优化匹配方法对象
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
// 位姿外插器
std::unique_ptr<PoseExtrapolator> extrapolator_;
// 用于频率控制
int num_accumulated_ = 0;
// ??????????????????????????????????????????
sensor::RangeData accumulated_range_data_;
// ??????????????????????????????????
absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
absl::optional<double> last_thread_cpu_time_seconds_;
absl::optional<common::Time> last_sensor_time_;
// ????????????????????????????????????????????????
RangeDataCollator range_data_collator_;
下面进行总结.
1. LocalTrajectoryBuilder2D::AddRangeData
对激光数据进行处理求解里程计的核心函数
/**
* @brief 添加激光数据的处理
* @details 流程:
* 1. 多传感器时间同步
* 2. 运动畸变去除
* 3. 无效数据剔除
* 4. 调用 AddAccumulatedRangeDate()
* @param unsynchronized_data 激光雷达的原始数据
* @param sensor_id
* @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data)
主要完成的事是:
1、 进行多个传感器的数据同步，只有一个传感器的时候，可以直接忽略
2、 运动畸变的去除
3、 对激光进行距离滤波
4、根据重力求解当前激光雷达的姿态，然后将激光数据投影到世界坐标系，并对超过z轴范围的点进行滤除，最后进行匹配。
下面是详细过程：
该函数输入的点云的类型是 sensor::TimedPointCloudData, 声明在 timed_point_cloud_data.h 中
/**
* @brief 点云数据时间和系统时间放在一起的结构体
*/
struct TimedPointCloudData {
common::Time time; // 帧时间
Eigen::Vector3f origin; // 激光雷达位置
TimedPointCloud ranges; // 每个点的时间与XY位置
};
ranges 即激光的测量, 类型是 TimedPointCloud,
using TimedPointCloud = std::vector
而 TimedRangefinderPoint 的也是cartographer 内部定义的,如下
// Stores 3D position of a point with its relative measurement time.
// See point_cloud.h for more details.
struct TimedRangefinderPoint {
Eigen::Vector3f position;
float time;
};
即 ranges 保存了所有激光点的 3D位置以及时间戳信息.
- 当有多个激光雷达时进行
// 进行多个传感器的数据同步 只有一个传感器的话 就直接忽略
auto synchronized_data =
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
if (synchronized_data.ranges.empty()) {
LOG(INFO) << "Range data collator filling buffer.";
return nullptr;
}
range_data_collator_.AddRangeData()函数返回的数据类型是： TimedPointCloudOriginData
struct TimedPointCloudOriginData {
//点云+时间+原始序号
struct RangeMeasurement {
TimedRangefinderPoint point_time;
size_t origin_index;
};
//系统时间
common::Time time;
std::vector<Eigen::Vector3f> origins;
std::vector<RangeMeasurement> ranges;
};
其中 TimedRangefinderPoint：
//点云+时间(浮点型)
struct TimedRangefinderPoint {
Eigen::Vector3f position;
float time;
};
所以这种点云数据类型，包含每个点的位置以及相对时间戳。
- 如果不使用IMU的话,同时为第一帧数据， 在这里初始化外插器
// Initialize extrapolator now if we do not ever use an IMU.
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
}
外插器定义为 std::unique_ptr
初始化中主要是两步:
// 构造插值器
1. extrapolator_ = absl::make_unique<PoseExtrapolator>(
// 时间参数，两次预测估计最小时间差
::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
.constant_velocity()
.pose_queue_duration()),
// 重力常数
options_.pose_extrapolator_options()
.constant_velocity()
.imu_gravity_time_constant());
// 设置初始pose
2. extrapolator_->AddPose(time, transform::Rigid3d::Identity());
-
激光的运动补偿
分析见后文。 -
调用 AddAccumulatedRangeData()进行匹配
首先用外插器通过重力估计姿态：
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
然后匹配
return AddAccumulatedRangeData(
time,
// 按重力对齐 + 滤波
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment, sensor_duration);
1.1 AddAccumulatedRangeData()
AddAccumulatedRangeData()函数的声明如下
/**
* @brief 这里进行激光的匹配
*/
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data, // 激光数据
const transform::Rigid3d& gravity_alignment, // 重力方向
const absl::optional<common::Duration>& sensor_duration)
其中输入的激光数据,是由函数
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast() * range_data_poses.back().inverse(),
accumulated_range_data_)
得到的, 该函数将输入的激光点与重力方向对齐, 以及滤波,细节如下:
/**
* @brief 重力对齐 + 范围滤波 + 体素滤波
*/
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const {
// 按重力对齐 + 范围滤波
const sensor::RangeData cropped =
sensor::CropRangeData(sensor::TransformRangeData(
range_data, transform_to_gravity_aligned_frame),
options_.min_z(), options_.max_z());
// 进行滤波
return sensor::RangeData{
cropped.origin, // 激光雷达的坐标
sensor::VoxelFilter(cropped.returns, // pointcloud类型的数据
options_.voxel_filter_size()), // 对障碍点云滤波
sensor::VoxelFilter(cropped.misses,
options_.voxel_filter_size())}; // 非障碍物点云滤波
}
具体细节见 cartographer核心问题1 传感器数据处理
继续回到AddAccumulatedRangeData函数
该函数内部核心的几步是:
- 用插值器去预测当前时刻的pose
// 预测6Dof的位姿
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
// 将上面的6自由度的位姿投影为 3自由度的位姿 (2D)
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
插值器的总结在 cartographer核心问题1 轮速记,IMU的引入–位姿插值器
-
对输入点云进行降采样, 这里使用的是自适应降采样
AdaptivelyVoxelFiltered()
细节在 cartographer核心问题1 传感器数据处理 -
调用 ScanMatch函数进行匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
- 之后就是将匹配结果转回到6Dof Pose, 然后更新外插器, 以及更新submap, 代码如下:
// 更具匹配的结果以及重力的方向 转换为 6Dof的位姿
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
// 将结果加入到 外插器中 用于状态的更新
extrapolator_->AddPose(time, pose_estimate);
// 将点云转换到 submap坐标系中
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
// 将点云插入到submap中
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
可以看到这个函数中主要实现了点云的匹配, submap的更新 , 以及位姿插值器的更新.
下面学习上面最重要的两个过程 点云的匹配ScanMatch 以及 submap的更新.
1.1.2 ScanMatch()
/**
* @brief 匹配的核心函数
* @param[in] time
* @param[in] pose_prediction 预测位姿
* @param[in] filtered_gravity_aligned_point_cloud 点云
*/
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud)
流程如下:
疑问: 下面是啥意思 ???
// ????????????????????????????????????????????/
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
const double residual_angle =
std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
这个函数调用的两个核心匹配算法的总结在:
cartographer核心问题2 前端核心匹配算法