From ab05459f1c7fd326ee18a39e54506a0bc52bc134 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 21 Feb 2018 08:53:09 +0100 Subject: [PATCH] Move GlobalTrajectoryBuilder to cc file (#923) * Move GlobalTrajectoryBuilder to cc file. This allows to instrument file-level static metrics. Also, it is a cleaner interface. * two create functions * drop superfluous typename --- .../mapping/global_trajectory_builder.cc | 155 ++++++++++++++++++ .../mapping/global_trajectory_builder.h | 118 ++----------- cartographer/mapping/map_builder.cc | 16 +- 3 files changed, 179 insertions(+), 110 deletions(-) create mode 100644 cartographer/internal/mapping/global_trajectory_builder.cc diff --git a/cartographer/internal/mapping/global_trajectory_builder.cc b/cartographer/internal/mapping/global_trajectory_builder.cc new file mode 100644 index 0000000..10a5c5b --- /dev/null +++ b/cartographer/internal/mapping/global_trajectory_builder.cc @@ -0,0 +1,155 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/internal/mapping/global_trajectory_builder.h" + +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping/local_slam_result_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { +namespace { + +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( + 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_(std::move(local_trajectory_builder)), + local_slam_result_callback_(local_slam_result_callback) {} + ~GlobalTrajectoryBuilder() override {} + + GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; + GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; + + 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( + 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 insertion_result; + if (matching_result->insertion_result != nullptr) { + auto node_id = 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_); + insertion_result = common::make_unique(InsertionResult{ + node_id, matching_result->insertion_result->constant_data, + std::vector>( + matching_result->insertion_result->insertion_submaps.begin(), + matching_result->insertion_result->insertion_submaps.end())}); + } + 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(insertion_result)); + } + } + + void AddSensorData(const std::string& sensor_id, + const sensor::ImuData& imu_data) override { + 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; + if (local_trajectory_builder_) { + local_trajectory_builder_->AddOdometryData(odometry_data); + } + pose_graph_->AddOdometryData(trajectory_id_, odometry_data); + } + + void AddSensorData( + const std::string& sensor_id, + const sensor::FixedFramePoseData& fixed_frame_pose) override { + if (fixed_frame_pose.pose.has_value()) { + CHECK(fixed_frame_pose.pose.value().IsValid()) + << fixed_frame_pose.pose.value(); + } + pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); + } + + void AddSensorData(const std::string& sensor_id, + const sensor::LandmarkData& landmark_data) override { + pose_graph_->AddLandmarkData(trajectory_id_, landmark_data); + } + + void AddLocalSlamResultData(std::unique_ptr + local_slam_result_data) override { + CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with " + "local_trajectory_builder_ present."; + local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_); + } + + private: + const int trajectory_id_; + PoseGraph* const pose_graph_; + std::unique_ptr local_trajectory_builder_; + LocalSlamResultCallback local_slam_result_callback_; +}; + +} // namespace + +std::unique_ptr CreateGlobalTrajectoryBuilder2D( + std::unique_ptr + local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph2D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback) { + return common::make_unique>( + std::move(local_trajectory_builder), trajectory_id, pose_graph, + local_slam_result_callback); +} + +std::unique_ptr CreateGlobalTrajectoryBuilder3D( + std::unique_ptr + local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph3D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback) { + return common::make_unique>( + std::move(local_trajectory_builder), trajectory_id, pose_graph, + local_slam_result_callback); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index 0e04905..3aeae5d 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -17,113 +17,31 @@ #ifndef CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_ -#include "cartographer/mapping/trajectory_builder_interface.h" +#include +#include "cartographer/internal/mapping_2d/local_trajectory_builder.h" +#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping/local_slam_result_data.h" -#include "glog/logging.h" +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer/mapping_2d/pose_graph_2d.h" +#include "cartographer/mapping_3d/pose_graph_3d.h" namespace cartographer { namespace mapping { -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( - 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_(std::move(local_trajectory_builder)), - local_slam_result_callback_(local_slam_result_callback) {} - ~GlobalTrajectoryBuilder() override {} +std::unique_ptr CreateGlobalTrajectoryBuilder2D( + std::unique_ptr + local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph2D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback); - GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; - GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; - - 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( - 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 insertion_result; - if (matching_result->insertion_result != nullptr) { - auto node_id = 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_); - insertion_result = common::make_unique(InsertionResult{ - node_id, matching_result->insertion_result->constant_data, - std::vector>( - matching_result->insertion_result->insertion_submaps.begin(), - matching_result->insertion_result->insertion_submaps.end())}); - } - 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(insertion_result)); - } - } - - void AddSensorData(const std::string& sensor_id, - const sensor::ImuData& imu_data) override { - 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; - if (local_trajectory_builder_) { - local_trajectory_builder_->AddOdometryData(odometry_data); - } - pose_graph_->AddOdometryData(trajectory_id_, odometry_data); - } - - void AddSensorData( - const std::string& sensor_id, - const sensor::FixedFramePoseData& fixed_frame_pose) override { - if (fixed_frame_pose.pose.has_value()) { - CHECK(fixed_frame_pose.pose.value().IsValid()) - << fixed_frame_pose.pose.value(); - } - pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); - } - - void AddSensorData(const std::string& sensor_id, - const sensor::LandmarkData& landmark_data) override { - pose_graph_->AddLandmarkData(trajectory_id_, landmark_data); - } - - void AddLocalSlamResultData(std::unique_ptr - local_slam_result_data) override { - CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with " - "local_trajectory_builder_ present."; - local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_); - } - - private: - const int trajectory_id_; - PoseGraph* const pose_graph_; - std::unique_ptr local_trajectory_builder_; - LocalSlamResultCallback local_slam_result_callback_; -}; +std::unique_ptr CreateGlobalTrajectoryBuilder3D( + std::unique_ptr + local_trajectory_builder, + const int trajectory_id, mapping::PoseGraph3D* const pose_graph, + const TrajectoryBuilderInterface::LocalSlamResultCallback& + local_slam_result_callback); } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 12f8480..6fee0d1 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -90,11 +90,9 @@ int MapBuilder::AddTrajectoryBuilder( trajectory_builders_.push_back( common::make_unique( sensor_collator_.get(), trajectory_id, expected_sensor_ids, - common::make_unique>( - std::move(local_trajectory_builder), trajectory_id, - pose_graph_3d_.get(), local_slam_result_callback))); + CreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder), + trajectory_id, pose_graph_3d_.get(), + local_slam_result_callback))); } else { std::unique_ptr local_trajectory_builder; @@ -106,11 +104,9 @@ int MapBuilder::AddTrajectoryBuilder( trajectory_builders_.push_back( common::make_unique( sensor_collator_.get(), trajectory_id, expected_sensor_ids, - common::make_unique>( - std::move(local_trajectory_builder), trajectory_id, - pose_graph_2d_.get(), local_slam_result_callback))); + CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), + trajectory_id, pose_graph_2d_.get(), + local_slam_result_callback))); } if (trajectory_options.pure_localization()) { constexpr int kSubmapsToKeep = 3;