admin管理员组文章数量:1660165
本次阅读的源码为 release-1.0 版本的代码
https://github/googlecartographer/cartographer_ros/tree/release-1.0
https://github/googlecartographer/cartographer/tree/release-1.0
也可以下载我上传的 全套工作空间的代码,包括 protobuf, cartographer, cartographer_ros, ceres,
https://download.csdn/download/tiancailx/11224156
第二篇文章分析传感器数据的处理流程,这篇文章将进行如何使用 传感器数据以及扫描匹配的处理过程。
GlobalTrajectoryBuilder类是连接 local SLAM 与 后端优化的桥梁,它将 传感器信息 和 local_slam匹配的结果 传入到pose_graph中。
cartographer/mapping/internal/global_trajectory_builder
template <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
public:
// Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no
// 'TimedPointCloudData' may be added in that case.
GlobalTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
const int trajectory_id, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback)
: trajectory_id_(trajectory_id),
pose_graph_(pose_graph),
local_trajectory_builder_(std::move(local_trajectory_builder)),
local_slam_result_callback_(local_slam_result_callback) {}
~GlobalTrajectoryBuilder() override {}
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
CHECK(local_trajectory_builder_)
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
}
kLocalSlamMatchingResults->Increment();
std::unique_ptr<InsertionResult> insertion_result;
if (matching_result->insertion_result != nullptr) {
kLocalSlamInsertionResults->Increment();
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
CHECK_EQ(node_id.trajectory_id, trajectory_id_);
insertion_result = common::make_unique<InsertionResult>(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
matching_result->insertion_result->insertion_submaps.begin(),
matching_result->insertion_result->insertion_submaps.end())});
}
if (local_slam_result_callback_) {
local_slam_result_callback_(
trajectory_id_, matching_result->time, matching_result->local_pose,
std::move(matching_result->range_data_in_local),
std::move(insertion_result));
}
}
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) override {
if (fixed_frame_pose.pose.has_value()) {
CHECK(fixed_frame_pose.pose.value().IsValid())
<< fixed_frame_pose.pose.value();
}
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
}
void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override {
pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
"local_trajectory_builder_ present.";
local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
}
private:
const int trajectory_id_;
PoseGraph* const pose_graph_;
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
LocalSlamResultCallback local_slam_result_callback_;
};
可以看到,通过重载函数AddSensorData() 将 lidar imu odom数据 传入到 local_trajectory_builder_2d.h 下的 对应方法中AddImuData() .
void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) 方法将雷达数据传入local slam ,得到匹配的位姿之后,在进行后端优化。
cartographer/mapping/internal/2d/local_trajectory_builder_2d
通过global_trajectory_builder 对 AddImuData() 和 AddOdometryData() 进行调用,将imu 和 odom 数据传入 pose extrapolator 进行位资预测。
LocalTrajectoryBuilder2D::AddRangeData() 方法中存在一个变量 num_accumulated_ ,这个变量对应着配置文件的 TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 2 参数,就是将几帧数据合成一组点云数据。
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(...)
{
// ...
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
num_accumulated_ = 0;
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
// TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
// 'time'.
accumulated_range_data_.origin = range_data_poses.back().translation();
return AddAccumulatedRangeData(
time,
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment);
}
}
之后进入LocalTrajectoryBuilder2D::AddAccumulatedRangeData() ,首先使用 Extrapolator 先根据之前时刻的线速度和角速度 乘以时间 来对下一时刻的位姿进行预测,然后将这个预测的位姿输入到scanMatch()中,对其进行最优化求解。
位姿预测
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment) {
if (gravity_aligned_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr;
}
// Computes a gravity aligned pose prediction.
// 计算重力对齐的姿势预测。
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
// local map frame <- gravity-aligned frame
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, gravity_aligned_range_data);
可以看到,在进行 PoseExtrapolator 初始化的时候将估计pose的时间间隔固定在了0.001s,也就是最小1ms进行一次姿态预测(对不对???)
// cartographer/mapping/internal/2d/local_trajectory_builder_2d
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
if (extrapolator_ != nullptr) {
return;
}
// We derive velocities from poses which are at least 1 ms apart for numerical
// stability. Usually poses known to the extrapolator will be further apart
// in time and thus the last two are used.
// 我们从姿势中获得速度,这些姿势相对于数值稳定性至少间隔1 ms。
// 通常,外推器已知的姿势将在时间上进一步分开,因此使用最后两个。
constexpr double kExtrapolationEstimationTimeSec = 0.001;
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
extrapolator_ = common::make_unique<PoseExtrapolator>(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
options_.imu_gravity_time_constant());
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}
// cartographer/mapping/pose_extrapolator.h
// Keep poses for a certain duration to estimate linear and angular velocity.
// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
// available to improve the extrapolation.
//保持姿势一定时间以估计线性和角速度。
//使用速度来推断运动。 如果可用,使用IMU和/或测距数据来改进外推。
class PoseExtrapolator {
public:
explicit PoseExtrapolator(common::Duration pose_queue_duration,
double imu_gravity_time_constant);
}
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
CHECK_GE(time, newest_timed_pose.time);
if (cached_extrapolated_pose_.time != time) {
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
const Eigen::Quaterniond rotation =
newest_timed_pose.pose.rotation() *
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
cached_extrapolated_pose_ =
TimedPose{time, transform::Rigid3d{translation, rotation}};
}
return cached_extrapolated_pose_.pose;
}
使用 ExtrapolatePose(const common::Time time) 类获得预测后的位姿。这个方法调用了 ExtrapolateRotation() ExtrapolateTranslation() 2个方法。
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
}
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
const double extrapolation_delta =
common::ToSeconds(time - newest_timed_pose.time);
if (odometry_data_.size() < 2) {
return extrapolation_delta * linear_velocity_from_poses_;
}
return extrapolation_delta * linear_velocity_from_odometry_;
}
ExtrapolateTranslation() 计算平移的预测是 平移的线速度乘以时间求得的。如果有odom 就使用odom的线速度,如果没有odom就是用匹配得到的2个pose的平移差除以时间得到 线速度。
ExtrapolateRotation() 是使用imu_tracker 进行角度的预测,也是用角速度乘以时间。当没有imu时,将用odom计算的角速度来代替,如果还没有odom,那就用匹配得到的2个pose的平移差除以时间得到 角速度。
ScanMatch
这样就得到了初始位姿的预测,再将这个预测输入到最小二乘的目标函数中,进行最优化求解。
其中,online correlative scan matcher 将会对这个初始位姿预测进行二次确认,这个优化是可选的,优化方式比较简单,就是在之前预测位姿附近开一个三维窗口,然后在这个三维窗口内,均匀播撒候选粒子,对每个粒子计算匹配得分,选取最高得分的粒子作为优化的位姿,再传入ceres中。
扫描匹配的过程:
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data) {
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
transform::Rigid2d initial_ceres_pose = pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
if (options_.use_online_correlative_scan_matching()) {
CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
proto::GridOptions2D_GridType_PROBABILITY_GRID);
double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
*static_cast<const ProbabilityGrid*>(matching_submap->grid()),
&initial_ceres_pose);
kFastCorrelativeScanMatcherScoreMetric->Observe(score);
}
auto pose_observation = common::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
double residual_angle = std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
return pose_observation;
}
之后进入到Ceres, ceres的具体分析可以根据 参考4 进行阅读。
// cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock(
OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr /* loss function */, ceres_pose_estimate);
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}
references
1. https://blog.csdn/liuxiaofei823/article/details/70207814
2. https://blog.csdn/roadseek_zw/article/details/66968762
3. https://blog.csdn/u012209790/article/details/82735923
4. https://blog.csdn/u012209790/article/details/82735923
5. https://blog.csdn/xiaoma_bk/article/details/81128482 Pose Extrapolate 理解。
本文标签: 第四章代码CartographerSCANMATCH
版权声明:本文标题:cartographer探秘第四章之代码解析(三) --- scan match 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://m.elefans.com/xitong/1729851051a1215415.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论