Cartographer源码阅读

Posted 2020-10-19 岁寒然后知松柏之后凋也！

上文提到特别注意map_builder_bridge_.AddTrajectory(x,x)，查看其中的代码。

int MapBuilderBridge::AddTrajectory(const std::unordered_set<std::string>& expected_sensor_ids,  const TrajectoryOptions& trajectory_options) 
{
     const int trajectory_id = map_builder_.AddTrajectoryBuilder(expected_sensor_ids, trajectory_options.trajectory_builder_options, ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
     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] = cartographer::common::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(...)是Cartographer项目中的代码了。

int MapBuilder::AddTrajectoryBuilder( const std::unordered_set<std::string>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) 
{
    const int trajectory_id = trajectory_builders_.size();//生成trajectory_id
    if (options_.use_trajectory_builder_3d()) 
    {
        CHECK(trajectory_options.has_trajectory_builder_3d_options());
        trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>(
            &sensor_collator_, trajectory_id, expected_sensor_ids,
            common::make_unique<mapping::GlobalTrajectoryBuilder<
                mapping_3d::LocalTrajectoryBuilder,
                mapping_3d::proto::LocalTrajectoryBuilderOptions,
                mapping_3d::PoseGraph>>(
                trajectory_options.trajectory_builder_3d_options(),
                trajectory_id, pose_graph_3d_.get(),
                local_slam_result_callback)));//注意此处的push_back()方法
    }
    else
    {
         CHECK(trajectory_options.has_trajectory_builder_2d_options());
         trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>(
            &sensor_collator_, trajectory_id, expected_sensor_ids,
            common::make_unique<mapping::GlobalTrajectoryBuilder<
                mapping_2d::LocalTrajectoryBuilder,
                mapping_2d::proto::LocalTrajectoryBuilderOptions,
                mapping_2d::PoseGraph>>(
                trajectory_options.trajectory_builder_2d_options(),
                trajectory_id, pose_graph_2d_.get(),
                local_slam_result_callback)));//注意此处的push_back()方法
    }
    if (trajectory_options.pure_localization()) 
    {
         constexpr int kSubmapsToKeep = 3;
         pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(trajectory_id, kSubmapsToKeep));
    }
    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()));
    }
    return trajectory_id;
}

　　注意，trajectory_builders_是根据trajectory_id添加的。以后调用的时候根据trajectory_id调用。

　　在ROS的主循环运行过程中，会不断处理传感器传入的数据。

　　以IMU数据为例，auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id)，根据trajectory_id获取sensor_bridge_ptr。

void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg)
{
  carto::common::MutexLocker lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) 
  {
        return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  if (imu_data_ptr != nullptr) 
  {
        extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}