cartographer代码学习笔记- map_builder_bridge_.AddTrajectory

Running小学生

已于 2023-11-30 19:58:19 修改

阅读量307

点赞数 11

分类专栏： cartographer 文章标签：学习笔记

于 2023-11-30 18:51:28 首次发布

本文链接：https://blog.csdn.net/weixin_28683491/article/details/134710850

版权

cartographer 专栏收录该内容

19 篇文章 1 订阅

订阅专栏

顺序流程图

在这里插入图片描述

代码解析

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      [this](const int trajectory_id, const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.ignore_out_of_order_messages,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

调用map_builder_ 的AddTrajectoryBuilder接口，接口定义

  int AddTrajectoryBuilder(
      const std::set<SensorId> &expected_sensor_ids,//传感器类型和话题
      const proto::TrajectoryBuilderOptions &trajectory_options, 
      LocalSlamResultCallback local_slam_result_callback) override;

实现部分

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
    //根据当前轨迹vector大小赋值轨迹id
    // 在.h文件中定义 std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
    //  trajectory_builders_
  const int trajectory_id = trajectory_builders_.size();

//定义里程计运动滤波器，并判断是否设置里程计滤波，如果设置了，里程计滤波器添加
  absl::optional<MotionFilter> pose_graph_odometry_motion_filter;
  if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
    LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
    pose_graph_odometry_motion_filter.emplace(
        MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));//根据里程计滤波相关参数初始化运动滤波对象
  }

1、运动滤波定义

class MotionFilter {
 public:
  explicit MotionFilter(const proto::MotionFilterOptions& options);//将参数赋值为options_

  // If the accumulated motion (linear, rotational, or time) is above the
  // threshold, returns false. Otherwise the relative motion is accumulated and
  // true is returned.
  bool IsSimilar(common::Time time, const transform::Rigid3d& pose);

 private:
  const proto::MotionFilterOptions options_;
  int num_total_ = 0;
  int num_different_ = 0;
  common::Time last_time_;
  transform::Rigid3d last_pose_;
};

2、判断2d还是3d

//根据3d或2d参数初始化局部轨迹构造器和全局轨迹构造器
  if (options_.use_trajectory_builder_3d()) {
  //定义3d
  } else {
  //定义2d
  }

本章主要解析2d部分
智能指针参考https://blog.csdn.net/qq_53982713/article/details/128550462

   //定义LocalTrajectoryBuilder2D 对象local_trajectory_builder
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
    //LocalTrajectoryBuilder2D 初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

LocalTrajectoryBuilder2D定义，初始化参数，active_submaps_，运动滤波，实时csm，csm以及数据处理

LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,
    const std::vector<std::string>& expected_range_sensor_ids)
    : options_(options),
      active_submaps_(options.submaps_options()),
      motion_filter_(options_.motion_filter_options()),
      real_time_correlative_scan_matcher_(
          options_.real_time_correlative_scan_matcher_options()),
      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
      range_data_collator_(expected_range_sensor_ids) {}

C++强制类型转换: https://blog.csdn.net/bit_zyx/article/details/127816591

    //强制转化pose_graph_为PoseGraph2D*类型
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));

CollatedTrajectoryBuilder初始化

  trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

CreateGlobalTrajectoryBuilder2D定义
返回TrajectoryBuilderInterface类型的智能指针

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}

GlobalTrajectoryBuilder继承TrajectoryBuilderInterface，初始化轨迹id，图优化，局部轨迹构建，回调函数，以及滤波器等。

  GlobalTrajectoryBuilder(
      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
      const int trajectory_id, PoseGraph* const pose_graph,
      const LocalSlamResultCallback& local_slam_result_callback,
      const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter)
      : trajectory_id_(trajectory_id),
        pose_graph_(pose_graph),
        local_trajectory_builder_(std::move(local_trajectory_builder)),
        local_slam_result_callback_(local_slam_result_callback),
        pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {}

3、纯定位模式触发及设定初始位置

//纯定位触发模式
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());
//设置轨迹初始位置，此处可用于设定定位起始点
  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }

MaybeAddPureLocalizationTrimmer详细内容

在定位模式下保持3张子图进行匹配

void MaybeAddPureLocalizationTrimmer(
    const int trajectory_id,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    PoseGraph* pose_graph) {
  if (trajectory_options.pure_localization()) {//根据参数判断存在纯定位
    LOG(WARNING)
        << "'TrajectoryBuilderOptions::pure_localization' field is deprecated. "
           "Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
    pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
        trajectory_id, 3 /* max_submaps_to_keep */));//pose_graph
    return;
  }
  if (trajectory_options.has_pure_localization_trimmer()) {
    pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
        trajectory_id,
        trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
  }
}

pose_graph->AddTrimmer

void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
  // C++11 does not allow us to move a unique_ptr into a lambda.
  //调用release 会切断unique_ptr 和它原来管理的对象的联系。
  //release 返回的指针通常被用来初始化另一个智能指针或给另一个智能指针赋值。
  //如果不用另一个智能指针来保存release返回的指针，程序就要负责资源的释放
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); 
  AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    trimmers_.emplace_back(trimmer_ptr);
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

SensorBridge 的实例与 CollatedTrajectoryBuilder 对象绑定在一起

  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.ignore_out_of_order_messages,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id));

sensor_bridges_定义为map容器，key为轨迹id ，value为SensorBridge指针

std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

SensorBridge定位接口：

SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const bool ignore_out_of_order_messages, const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      ignore_out_of_order_messages_(ignore_out_of_order_messages),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

tf_bridge_主要是根据时间进行坐标转换到tracking_frame
trajectory_builder_是TrajectoryBuilderInterface对象，在数据进来以后通过该接口调用AddSensorData处理数据。

map_builder_->GetTrajectoryBuilder(trajectory_id)定义
返回当前轨迹id下std::unique_ptrmapping::TrajectoryBuilderInterface对象，初始化SensorBridge成员变量trajectory_builder_

  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }