Cartographer源码阅读(2)：Node和MapBuilder对象

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

　　首先是map_builder_.AddTrajectoryBuilder(...)，调用了map_builder_对象的方法。其次是sensor_bridges_键值对的赋值。

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调用。

　　2D/3D区分：同时可以看到，这里对2D和3D情况作了区分，根据options_.use_trajectory_builder_3d()确定使用的类型。

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

　　以IMU数据为例，auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id)，根据trajectory_id获取sensor_bridge_ptr。注意这里因为是订阅的其它ROS主题（Topic），所以sensor_id参数是从其他主题传入的。（即当前程序内部有一套主题名称的字符串，订阅了外部主题也有一套名称字符串表示。这样两者通过同样的名称字符串建立了关系）

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);
}

　　最后调用了sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);的代码。这里又通过trajectory_builder_调用了AddSensorData方法，由于之前做为参数传入的是CollatedTrajectoryBuilder，所以实际调用的是CollatedTrajectoryBuilder的AddSensorData方法。

void SensorBridge::HandleImuMessage(const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) 
{
     std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
     if (imu_data != nullptr) 
    {
            trajectory_builder_->AddSensorData( sensor_id, cartographer::sensor::ImuData{imu_data->time, imu_data->linear_acceleration, imu_data->angular_velocity});
    }
}

　　SensorBridge类实现代码，消息转换函数查看msg_conversion.cc文件：

  1 SensorBridge::SensorBridge(
  2     const int num_subdivisions_per_laser_scan,
  3     const std::string& tracking_frame,
  4     const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
  5     carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
  6     : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
  7       tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
  8       trajectory_builder_(trajectory_builder) {}
  9 
 10 std::unique_ptr<::cartographer::sensor::OdometryData>
 11 SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {
 12   const carto::common::Time time = FromRos(msg->header.stamp);
 13   const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
 14       time, CheckNoLeadingSlash(msg->child_frame_id));
 15   if (sensor_to_tracking == nullptr) {
 16     return nullptr;
 17   }
 18   return ::cartographer::common::make_unique<
 19       ::cartographer::sensor::OdometryData>(
 20       ::cartographer::sensor::OdometryData{
 21           time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
 22 }
 23 
 24 void SensorBridge::HandleOdometryMessage(
 25     const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
 26   std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
 27       ToOdometryData(msg);
 28   if (odometry_data != nullptr) {
 29     trajectory_builder_->AddSensorData(
 30         sensor_id, cartographer::sensor::OdometryData{odometry_data->time,
 31                                                       odometry_data->pose});
 32   }
 33 }
 34 
 35 std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
 36     const sensor_msgs::Imu::ConstPtr& msg) {
 37   CHECK_NE(msg->linear_acceleration_covariance[0], -1)
 38       << "Your IMU data claims to not contain linear acceleration measurements "
 39          "by setting linear_acceleration_covariance[0] to -1. Cartographer "
 40          "requires this data to work. See "
 41          "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
 42   CHECK_NE(msg->angular_velocity_covariance[0], -1)
 43       << "Your IMU data claims to not contain angular velocity measurements "
 44          "by setting angular_velocity_covariance[0] to -1. Cartographer "
 45          "requires this data to work. See "
 46          "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
 47 
 48   const carto::common::Time time = FromRos(msg->header.stamp);
 49   const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
 50       time, CheckNoLeadingSlash(msg->header.frame_id));
 51   if (sensor_to_tracking == nullptr) {
 52     return nullptr;
 53   }
 54   CHECK(sensor_to_tracking->translation().norm() < 1e-5)
 55       << "The IMU frame must be colocated with the tracking frame. "
 56          "Transforming linear acceleration into the tracking frame will "
 57          "otherwise be imprecise.";
 58   return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>(
 59       ::cartographer::sensor::ImuData{
 60           time,
 61           sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
 62           sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
 63 }
 64 
 65 void SensorBridge::HandleImuMessage(const std::string& sensor_id,
 66                                     const sensor_msgs::Imu::ConstPtr& msg) {
 67   std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
 68   if (imu_data != nullptr) {
 69     trajectory_builder_->AddSensorData(
 70         sensor_id, cartographer::sensor::ImuData{imu_data->time,
 71                                                  imu_data->linear_acceleration,
 72                                                  imu_data->angular_velocity});
 73   }
 74 }
 75 
 76 void SensorBridge::HandleLaserScanMessage(
 77     const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
 78   ::cartographer::sensor::PointCloudWithIntensities point_cloud;
 79   ::cartographer::common::Time time;
 80   std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
 81   HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 82 }
 83 
 84 void SensorBridge::HandleMultiEchoLaserScanMessage(
 85     const std::string& sensor_id,
 86     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
 87   ::cartographer::sensor::PointCloudWithIntensities point_cloud;
 88   ::cartographer::common::Time time;
 89   std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
 90   HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 91 }
 92 
 93 void SensorBridge::HandlePointCloud2Message(
 94     const std::string& sensor_id,
 95     const sensor_msgs::PointCloud2::ConstPtr& msg) {
 96   pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
 97   pcl::fromROSMsg(*msg, pcl_point_cloud);
 98   carto::sensor::TimedPointCloud point_cloud;
 99   for (const auto& point : pcl_point_cloud) {
100     point_cloud.emplace_back(point.x, point.y, point.z, 0.f);
101   }
102   HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
103                     point_cloud);
104 }
105 
106 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
107 
108 void SensorBridge::HandleLaserScan(
109     const std::string& sensor_id, const carto::common::Time time,
110     const std::string& frame_id,
111     const carto::sensor::PointCloudWithIntensities& points) {
112   CHECK_LE(points.points.back()[3], 0);
113   // TODO(gaschler): Use per-point time instead of subdivisions.
114   for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
115     const size_t start_index =
116         points.points.size() * i / num_subdivisions_per_laser_scan_;
117     const size_t end_index =
118         points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
119     carto::sensor::TimedPointCloud subdivision(
120         points.points.begin() + start_index, points.points.begin() + end_index);
121     if (start_index == end_index) {
122       continue;
123     }
124     const double time_to_subdivision_end = subdivision.back()[3];
125     // `subdivision_time` is the end of the measurement so sensor::Collator will
126     // send all other sensor data first.
127     const carto::common::Time subdivision_time =
128         time + carto::common::FromSeconds(time_to_subdivision_end);
129     for (auto& point : subdivision) {
130       point[3] -= time_to_subdivision_end;
131     }
132     CHECK_EQ(subdivision.back()[3], 0);
133     HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
134   }
135 }
136 
137 void SensorBridge::HandleRangefinder(
138     const std::string& sensor_id, const carto::common::Time time,
139     const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
140   const auto sensor_to_tracking =
141       tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
142   if (sensor_to_tracking != nullptr) {
143     trajectory_builder_->AddSensorData(
144         sensor_id, cartographer::sensor::TimedPointCloudData{
145                        time, sensor_to_tracking->translation().cast<float>(),
146                        carto::sensor::TransformTimedPointCloud(
147                            ranges, sensor_to_tracking->cast<float>())});
148   }
149 }
SensorBridge
 1 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) {
 2   PointCloudWithIntensities point_cloud;
 3   // We check for intensity field here to avoid run-time warnings if we pass in
 4   // a PointCloud2 without intensity.
 5   if (PointCloud2HasField(message, "intensity")) {
 6     pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
 7     pcl::fromROSMsg(message, pcl_point_cloud);
 8     for (const auto& point : pcl_point_cloud) {
 9       point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
10       point_cloud.intensities.push_back(point.intensity);
11     }
12   } else {
13     pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
14     pcl::fromROSMsg(message, pcl_point_cloud);
15 
16     // If we don't have an intensity field, just copy XYZ and fill in
17     // 1.0.
18     for (const auto& point : pcl_point_cloud) {
19       point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
20       point_cloud.intensities.push_back(1.0);
21     }
22   }
23   return std::make_tuple(point_cloud, FromRos(message.header.stamp));
24 }
msg_conversion.cc

　　查看CollatedTrajectoryBuilder的AddSensorData方法，在CollatedTrajectoryBuilder的头文件中，包括4个覆写的AddSensorData(x,x)方法，方法中通过sensor::MakeDispatchable转换为Dispatchable<DataType>类型。

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

 void AddSensorData(const std::string& sensor_id,  const sensor::ImuData& imu_data) override
 {
       AddSensorData(sensor::MakeDispatchable(sensor_id, imu_data));
 }

  void AddSensorData(const std::string& sensor_id,  const sensor::OdometryData& odometry_data) override 
{
      AddSensorData(sensor::MakeDispatchable(sensor_id, odometry_data));
}

  void AddSensorData(const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override 
{
     AddSensorData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
}

　　最终定位到了sensor_collator_对象的方法。

void CollatedTrajectoryBuilder::AddSensorData( std::unique_ptr<sensor::Data> data) 
{
      sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

　　

　　查看几个类CollatedTrajectoryBuilder，mapping::GlobalTrajectoryBuilder

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id,  
           const std::unordered_set<std::string>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> 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())
{
      sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids, [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) 
     {HandleCollatedSensorData(sensor_id, std::move(data));});
}

　　mapping::GlobalTrajectoryBuilder构造函数

GlobalTrajectoryBuilder(const LocalTrajectoryBuilderOptions& options, 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_(options), local_slam_result_callback_(local_slam_result_callback) 
{}

　　注意这里的继承关系：

　　　　class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface

　　　　class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface

　　在mapping_2d和mapping_3d两个命名空间下分别存在2个local_trajectory_builder_类，实现了局部的扫描匹配和子图构建。代码在cartographer\cartographer\internal文件夹下。

　　另外一个重要的Node类变量是extrapolators_，该对象在Node类的处理Odometry和IMU数据时都有用到，作用是位姿推算。在文一种Node::AddTrajectory方法中调用了AddExtrapolator(trajectory_id, options);

1 std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;

 

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));
}

 map的emplace方法，高效插入。http://en.cppreference.com/w/cpp/container/map/emplace

posted @ 2017-12-28 21:33  太一吾鱼水  阅读(2261)  评论(0)  编辑  收藏  举报