cartographer之trajector_builder的关系问题

map_builder

作为一个主要builder串联一个完整的SLAM建图，　TrajectoryBuilder(local slam) 和　PoseGraph( loop closure)继承于MapBuilderInterface
整个global_trajectory_builder，　collated_trajectory_builder, collator(TrajectoryCollator)　local_trajectory_builder等的关系可以参照下面两幅图像，彼此关系也关联这数据的传递cartographer_ros_传感器数据篇

变量

common::ThreadPool thread_pool_;
std::unique_ptr pose_graph_;
std::unique_ptr sensor_collator_;
std::vector> trajectory_builders_;

构造函数中完成了对变量　pose_graph_和　sensor_collator_的定义
这其中sensor_collator_有两种分别为

1, Collator

void Collator::AddTrajectory(
    const int trajectory_id,
    const std::unordered_set& expected_sensor_ids,
    const Callback& callback)
{

  for (const auto& sensor_id : expected_sensor_ids)
  {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr data)
                    {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

2, TrajectoryCollator

他们都继承与CollatorInterface

核心方法AddTrajectoryBuilder

我们以２D为例：

1, 定义LocalTrajectoryBuilder2D;

2, 为trajectory_builders_添加一条CollatedTrajectoryBuilder;

3, CollatedTrajectoryBuilder需要sensor_collator_, GlobalTrajectoryBuilder2D;

4, data->AddToTrajectoryBuilder

5, GlobalTrajectoryBuilder2D->AddSensorData

CollatedTrajectoryBuilder和GlobalTrajectoryBuilder2D都继承于TrajectoryBuilderInterface

int MapBuilder::AddTrajectoryBuilder(const std::set& expected_sensor_ids,
                                     const proto::TrajectoryBuilderOptions& trajectory_options,
                                     LocalSlamResultCallback local_slam_result_callback)
{
  　const int trajectory_id = trajectory_builders_.size();
    std::unique_ptr local_trajectory_builder;
    //  1 
    if (trajectory_options.has_trajectory_builder_2d_options())
    {
      local_trajectory_builder = common::make_unique(trajectory_options.trajectory_builder_2d_options(),
                                                                               SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast(pose_graph_.get()));
	//2,3
    trajectory_builders_.push_back(
        common::make_unique(sensor_collator_.get(),
                                                       trajectory_id,
                                                       expected_sensor_ids,
                                                       CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder),
                                                                                        trajectory_id,
                                                                                         static_cast(pose_graph_.get()),
                                                                                         local_slam_result_callback)));

    if (trajectory_options.has_overlapping_submaps_trimmer_2d())
    {
      const auto& trimmer_options = trajectory_options.overlapping_submaps_trimmer_2d();
      pose_graph_->AddTrimmer(common::make_unique( trimmer_options.fresh_submaps_count(),
           trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options()
                               .submaps_options()
                               .grid_options_2d()
                               .resolution()),
          trimmer_options.min_added_submaps_count()));
    }
  }
  if (trajectory_options.pure_localization())
  {
    constexpr int kSubmapsToKeep = 3;
    pose_graph_->AddTrimmer(common::make_unique(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()));
  }
  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids)
  {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = trajectory_options;
  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
  return trajectory_id;
}

collated_trajectory_builder

CollatorInterface::AddTrajectory

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::CollatorInterface* const sensor_collator, 
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　const int trajectory_id,
    　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　const std::set& expected_sensor_ids,
    　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　std::unique_ptr wrapped_trajectory_builder)
												    : sensor_collator_(sensor_collator),
												      trajectory_id_(trajectory_id),
												      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
												      last_logging_time_(std::chrono::steady_clock::now())
 {
	  std::unordered_set expected_sensor_id_strings;
	  for (const auto& sensor_id : expected_sensor_ids)
	  {
	    　expected_sensor_id_strings.insert(sensor_id.id);
	  }
	  sensor_collator_->AddTrajectory(
									  　trajectory_id, 
									  　expected_sensor_id_strings,
									     [this](const std::string& sensor_id, std::unique_ptr data) 
									      {
									        HandleCollatedSensorData(sensor_id, std::move(data));
									      }
	     　　　　　　　　　　　　　　　　　 );
}

void AddSensorData( const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) override 
{
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

Dispatchable::Data

template 
class Dispatchable : public Data {
 public:
  Dispatchable(const std::string &sensor_id, const DataType &data)
      : Data(sensor_id), data_(data) {}

  common::Time GetTime() const override { return data_.time; }
  void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *const trajectory_builder) override
   {
      trajectory_builder->AddSensorData(sensor_id_, data_);
  }
  const DataType &data() const { return data_; }

 private:
  const DataType data_;
};

template 
std::unique_ptr> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
  return common::make_unique>(sensor_id, data);
}

data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());

void CollatedTrajectoryBuilder::HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr data)
{
  LOG(INFO)<<"4, Handle Data";
  auto it = rate_timers_.find(sensor_id);
  if (it == rate_timers_.end())
  {
    it = rate_timers_
             .emplace(
                 std::piecewise_construct, std::forward_as_tuple(sensor_id),
                 std::forward_as_tuple(
                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
             .first;
  }
  it->second.Pulse(data->GetTime());

  if (std::chrono::steady_clock::now() - last_logging_time_ >
      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))
  {
    for (const auto& pair : rate_timers_)
    {
      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
    }
    last_logging_time_ = std::chrono::steady_clock::now();
  }

  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

wrapped_trajectory_builder_.get() 则为

global_trajectory_builder

trajectory_builder_interface

map_builder_interface

一个基础类建图，　TrajectoryBuilder　＋　PoseGraph
Creates a new trajectory builder and returns its index.

  virtual int AddTrajectoryBuilder(const std::set& expected_sensor_ids,
                                   const proto::TrajectoryBuilderOptions& trajectory_options,
                                   LocalSlamResultCallback local_slam_result_callback) = 0;

Returns the ‘TrajectoryBuilderInterface’ corresponding to the specified ‘trajectory_id’ or ‘nullptr’ if the trajectory has no corresponding builder.

  virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( int trajectory_id) const = 0;

Marks the TrajectoryBuilder corresponding to ‘trajectory_id’ as finished, i.e. no further sensor data is expected.

virtual void FinishTrajectory(int trajectory_id) = 0;

local_trajectory_builder