Cartographer源碼閱讀(4)：Node和MapBuilder對象2

　　MapBuilder的成員變量sensor::Collator sensor_collator_;

　　再次閱讀MapBuilder::AddTrajectoryBuilder方法。首先構造了mapping::GlobalTrajectoryBuilder實例，接着作為參數構造了CollatedTrajectoryBuilder實例。

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

　　這里sensor_collator_作為參數傳入，參與CollatedTrajectoryBuilder構造。查看構造函數：

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

　　這里是回調函數，std::unique_ptr是表示參數為智能指針。

 [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
 {
        HandleCollatedSensorData(sensor_id, std::move(data));
  }

　　（1）查看sensor::Collator的AddTrajectory方法：

void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set<std::string>& 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> data)
                    {
                      callback(sensor_id, std::move(data));
                    });
       queue_keys_[trajectory_id].push_back(queue_key);
  }
}

　　for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。

　　（2）查看HandleCollatedSensorData方法。調用了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());這里wrapped_trajectory_builder_是在CollatedTrajectoryBuilder構造函數中賦值的。為GlobalTrajectoryBuilder對象。因而查看sensor::Data的AddToTrajectoryBuilder() 方法。

　　virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data類的一個虛方法。內部執行了trajectory_builder->AddSensorData(sensor_id_, data_);

最后調用的是GlobalTrajectoryBuilder對象的AddSensorData(xx)方法。

void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> 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());
  }

} 

 

template <typename DataType>
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_);
  }

 private:
  const DataType data_;
};

　　再以IMU數據為例，GlobalTrajectoryBuilder類的AddSensorData(xx)：

void AddSensorData(const std::string& sensor_id,  const sensor::ImuData& imu_data) override 
{
      local_trajectory_builder_.AddImuData(imu_data);
      pose_graph_->AddImuData(trajectory_id_, imu_data);
}

　　再看一下激光點雲的數據

void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override
 {
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>  matching_result = 
       local_trajectory_builder_.AddRangeData( timed_point_cloud_data.time, 
                                               sensor::TimedRangeData {timed_point_cloud_data.origin, 
                                               timed_point_cloud_data.ranges, {}}
       );
    if (matching_result == nullptr) 
    {
        // The range data has not been fully accumulated yet.
        return;
    }
    std::unique_ptr<mapping::NodeId> node_id;
    if (matching_result->insertion_result != nullptr) 
    {
          node_id = ::cartographer::common::make_unique<mapping::NodeId>(
          pose_graph_->AddNode(matching_result->insertion_result->constant_data, 
trajectory_id_, matching_result->insertion_result->insertion_submaps));
          CHECK_EQ(node_id->trajectory_id, trajectory_id_);
    }
    if (local_slam_result_callback_) 
    {
      local_slam_result_callback_( trajectory_id_, matching_result->time, 
             matching_result->local_pose,
          std::move(matching_result->range_data_in_local), std::move(node_id));
     }
  }

　　這里有兩個重要的步驟一個是local_trajectory_builder_.AddRangeData（xxx），一個是 pose_graph_->AddNode(xxx)方法。同時std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result作為AddNode方法的參數。

mapping::NodeId PoseGraph::AddNode(
    std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  common::MutexLocker locker(&mutex_);
  AddTrajectoryIfNeeded(trajectory_id);
  const mapping::NodeId node_id = trajectory_nodes_.Append(
      trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
  ++num_trajectory_nodes_;

  // Test if the 'insertion_submap.back()' is one we never saw before.
  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
          insertion_submaps.back()) {
    // We grow 'submap_data_' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.
    const mapping::SubmapId submap_id =
        submap_data_.Append(trajectory_id, SubmapData());
    submap_data_.at(submap_id).submap = insertion_submaps.back();
  }

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  const bool newly_finished_submap = insertion_submaps.front()->finished();
  AddWorkItem([=]() REQUIRES(mutex_) {
    ComputeConstraintsForNode(node_id, insertion_submaps,
                              newly_finished_submap);
  });
  return node_id;
}

　　PoseGraph::AddNode方法很重要，分析節點和子圖的關系。

　　此處強調一下GlobalTrajectoryBuilder的兩個關鍵對象local_trajectory_builder_和pose_graph_。

  PoseGraph* const pose_graph_;
  LocalTrajectoryBuilder local_trajectory_builder_;

　　接下來按照准備安裝ROS消息發布和處理的流程進行分析，即數據流。

---

 

參考資料：

http://blog.csdn.net/datase/article/details/78665862

http://blog.csdn.net/learnmoreonce/article/category/6989560