[人工智能]Cartographer(2)--轨迹构建

1.在node_main里面

?map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(

? ? ? node_options.map_builder_options);初始化地图构建器

在文件：/Cartographer/Cartographer/mapping/map_builder.cc

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) {
  CHECK(options.use_trajectory_builder_2d() ^
        options.use_trajectory_builder_3d());
  if (options.use_trajectory_builder_2d()) { //二维配置
    pose_graph_ = absl::make_unique<PoseGraph2D>(//图优化
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem2D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  }
  if (options.use_trajectory_builder_3d()) {
    pose_graph_ = absl::make_unique<PoseGraph3D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem3D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  }
  if (options.collate_by_trajectory()) {   //传感器收集
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
}

?

2.在node_main里面：Node node(node_options, std::move(map_builder), &tf_buffer, false);提供的服务

其中还继承了? ? ? map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)?

map_builder_bridge_：作为Cartographer_ros与Cartographer交互接口类。

3.在node_main里面：node.StartTrajectoryWithDefaultTopics(trajectory_options);

进入到node.cc

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {

? absl::MutexLock lock(&mutex_);

? CHECK(ValidateTrajectoryOptions(options));

? AddTrajectory(options, DefaultSensorTopics());

}

//输入options：配置信息
//SensorTopics:传感器话题
int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);//传感器id,有些同一类多个传感器
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);//通过map_builder_bridge_添加轨迹
  AddExtrapolator(trajectory_id, options);//添加外推器
  AddSensorSamplers(trajectory_id, options);//采样器
  LaunchSubscribers(options, topics, trajectory_id);// 订阅话题与注册回调函数
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));//发送警告当不匹配
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

?3.1进入：map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

//输入传感器Id
//轨迹配置
//返回轨迹ID
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.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 添加轨迹

初始化激光扫描?sensor_bridges_

3.2 添加外推器?AddExtrapolator(trajectory_id, options); 配置外推周期

void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  CHECK(extrapolators_.count(trajectory_id) == 0);
  const double gravity_time_constant =   //获取重力时间常数
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()//三维里面的
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();//二维轨迹里面的 
  extrapolators_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

3.3添加传感器采样器AddSensorSamplers(trajectory_id, options);配置采样器采样率

3.4订阅话题与注册回调函数LaunchSubscribers(options, topics, trajectory_id);

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const cartographer_ros_msgs::SensorTopics& topics,
                             const int trajectory_id) {
//便历laser_scans topic
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.laser_scan_topic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});//订阅LaserScanMessage
  }

    //便历multi_echo_laser_scan_topic
  for (const std::string& topic :
       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
                                 options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

//便历point_cloud2_topic
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.point_cloud2_topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

  // 是否使用IMU 3d必须使用 imu数据回调
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    std::string topic = topics.imu_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                        trajectory_id, topic，&node_handle_, this),topic});
  }
//Odom数据回调
  if (options.use_odometry) {
    std::string topic = topics.odometry_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, topic,
                                                  &node_handle_, this),
         topic});
  }

  if (options.use_nav_sat) {
    std::string topic = topics.nav_sat_fix_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  if (options.use_landmarks) {
    std::string topic = topics.landmark_topic;
    ROS_WARN_STREAM("landmark topic:" << topic);
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cotek_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
}