From 8c7c4e3d2ad74c892cc77a3b54757bab411b90cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Tue, 9 Jan 2018 16:54:30 +0100 Subject: [PATCH] Make LocalTrajectoryBuilder optional. (#799) This is to prepare for the cloud-based mapping case where the robot is running cartographer in pure-localization mode and the cloud instance only solves the global problem. In that case a `LocalTrajectoryBuilder` need to be instantiated in for the cloud instance. --- .../mapping/global_trajectory_builder.h | 22 +++++++++----- cartographer/mapping/map_builder.cc | 30 ++++++++++++------- 2 files changed, 35 insertions(+), 17 deletions(-) diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index 0d2d12f..ff47580 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -28,13 +28,15 @@ template class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { public: + // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no + // 'TimedPointCloudData' may be added in that case. GlobalTrajectoryBuilder( - const LocalTrajectoryBuilderOptions& options, const int trajectory_id, - PoseGraph* const pose_graph, + std::unique_ptr local_trajectory_builder, + 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_trajectory_builder_(std::move(local_trajectory_builder)), local_slam_result_callback_(local_slam_result_callback) {} ~GlobalTrajectoryBuilder() override {} @@ -44,8 +46,10 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override { + CHECK(local_trajectory_builder_) + << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder."; std::unique_ptr - matching_result = local_trajectory_builder_.AddRangeData( + matching_result = local_trajectory_builder_->AddRangeData( timed_point_cloud_data.time, sensor::TimedRangeData{timed_point_cloud_data.origin, timed_point_cloud_data.ranges, @@ -71,14 +75,18 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override { - local_trajectory_builder_.AddImuData(imu_data); + if (local_trajectory_builder_) { + local_trajectory_builder_->AddImuData(imu_data); + } pose_graph_->AddImuData(trajectory_id_, imu_data); } void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) override { CHECK(odometry_data.pose.IsValid()) << odometry_data.pose; - local_trajectory_builder_.AddOdometryData(odometry_data); + if (local_trajectory_builder_) { + local_trajectory_builder_->AddOdometryData(odometry_data); + } pose_graph_->AddOdometryData(trajectory_id_, odometry_data); } @@ -95,7 +103,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { private: const int trajectory_id_; PoseGraph* const pose_graph_; - LocalTrajectoryBuilder local_trajectory_builder_; + std::unique_ptr local_trajectory_builder_; LocalSlamResultCallback local_slam_result_callback_; }; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 0fb02b4..d36de3b 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -75,29 +75,39 @@ int MapBuilder::AddTrajectoryBuilder( LocalSlamResultCallback local_slam_result_callback) { const int trajectory_id = trajectory_builders_.size(); if (options_.use_trajectory_builder_3d()) { - CHECK(trajectory_options.has_trajectory_builder_3d_options()); + std::unique_ptr + local_trajectory_builder; + if (trajectory_options.has_trajectory_builder_3d_options()) { + local_trajectory_builder = + common::make_unique( + trajectory_options.trajectory_builder_3d_options()); + } trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique>( - trajectory_options.trajectory_builder_3d_options(), - trajectory_id, pose_graph_3d_.get(), - local_slam_result_callback))); + mapping_3d::PoseGraph>>(std::move(local_trajectory_builder), + trajectory_id, pose_graph_3d_.get(), + local_slam_result_callback))); } else { - CHECK(trajectory_options.has_trajectory_builder_2d_options()); + std::unique_ptr + local_trajectory_builder; + if (trajectory_options.has_trajectory_builder_2d_options()) { + local_trajectory_builder = + common::make_unique( + trajectory_options.trajectory_builder_2d_options()); + } trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique>( - trajectory_options.trajectory_builder_2d_options(), - trajectory_id, pose_graph_2d_.get(), - local_slam_result_callback))); + mapping_2d::PoseGraph>>(std::move(local_trajectory_builder), + trajectory_id, pose_graph_2d_.get(), + local_slam_result_callback))); } if (trajectory_options.pure_localization()) { constexpr int kSubmapsToKeep = 3;