From 43544f0fbc542f776cf5f7adf3df2e298bf4003e Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Mon, 19 Feb 2018 20:01:29 +0100 Subject: [PATCH] Rename mapping_2d::PoseGraph to mapping::PoseGraph2D. (#917) [rCode structure RFC](https://github.com/pifon2a/rfcs/blob/e11bca586f8cc733063ea9c94bdade71086ed9ea/text/0000-code-structure.md) --- .../mapping/local_slam_result_data.cc | 33 ++- cartographer/mapping/local_slam_result_data.h | 26 +- cartographer/mapping/map_builder.cc | 11 +- cartographer/mapping/map_builder.h | 7 +- .../{pose_graph.cc => pose_graph_2d.cc} | 269 +++++++++--------- .../{pose_graph.h => pose_graph_2d.h} | 121 ++++---- ...se_graph_test.cc => pose_graph_2d_test.cc} | 73 +++-- 7 files changed, 263 insertions(+), 277 deletions(-) rename cartographer/mapping_2d/{pose_graph.cc => pose_graph_2d.cc} (77%) rename cartographer/mapping_2d/{pose_graph.h => pose_graph_2d.h} (73%) rename cartographer/mapping_2d/{pose_graph_test.cc => pose_graph_2d_test.cc} (84%) diff --git a/cartographer/mapping/local_slam_result_data.cc b/cartographer/mapping/local_slam_result_data.cc index 8d7da54..6c15caa 100644 --- a/cartographer/mapping/local_slam_result_data.cc +++ b/cartographer/mapping/local_slam_result_data.cc @@ -20,53 +20,52 @@ namespace cartographer { namespace mapping { -LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id, +LocalSlamResultData::LocalSlamResultData(const std::string& sensor_id, common::Time time) : Data(sensor_id), time_(time) {} LocalSlamResult2D::LocalSlamResult2D( - const std::string &sensor_id, common::Time time, + const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector> - &insertion_submaps) + const std::vector>& + insertion_submaps) : LocalSlamResultData(sensor_id, time), node_data_(node_data), insertion_submaps_(insertion_submaps) {} void LocalSlamResult2D::AddToTrajectoryBuilder( - mapping::TrajectoryBuilderInterface *const trajectory_builder) { + mapping::TrajectoryBuilderInterface* const trajectory_builder) { trajectory_builder->AddLocalSlamResultData( common::make_unique(*this)); } void LocalSlamResult2D::AddToPoseGraph(int trajectory_id, - mapping::PoseGraph *pose_graph) const { - DCHECK(dynamic_cast(pose_graph)); - mapping_2d::PoseGraph *pose_graph_2d = - static_cast(pose_graph); + mapping::PoseGraph* pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + PoseGraph2D* pose_graph_2d = static_cast(pose_graph); pose_graph_2d->AddNode(node_data_, trajectory_id, insertion_submaps_); } LocalSlamResult3D::LocalSlamResult3D( - const std::string &sensor_id, common::Time time, + const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector> - &insertion_submaps) + const std::vector>& + insertion_submaps) : LocalSlamResultData(sensor_id, time), node_data_(node_data), insertion_submaps_(insertion_submaps) {} void LocalSlamResult3D::AddToTrajectoryBuilder( - mapping::TrajectoryBuilderInterface *const trajectory_builder) { + mapping::TrajectoryBuilderInterface* const trajectory_builder) { trajectory_builder->AddLocalSlamResultData( common::make_unique(*this)); } void LocalSlamResult3D::AddToPoseGraph(int trajectory_id, - mapping::PoseGraph *pose_graph) const { - DCHECK(dynamic_cast(pose_graph)); - mapping_3d::PoseGraph *pose_graph_3d = - static_cast(pose_graph); + mapping::PoseGraph* pose_graph) const { + DCHECK(dynamic_cast(pose_graph)); + mapping_3d::PoseGraph* pose_graph_3d = + static_cast(pose_graph); pose_graph_3d->AddNode(node_data_, trajectory_id, insertion_submaps_); } diff --git a/cartographer/mapping/local_slam_result_data.h b/cartographer/mapping/local_slam_result_data.h index 571c2f4..817d0af 100644 --- a/cartographer/mapping/local_slam_result_data.h +++ b/cartographer/mapping/local_slam_result_data.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H #define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H -#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_2d/pose_graph_2d.h" #include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/sensor/data.h" @@ -27,11 +27,11 @@ namespace mapping { class TrajectoryBuilderInterface; class LocalSlamResultData : public cartographer::sensor::Data { public: - LocalSlamResultData(const std::string &sensor_id, common::Time time); + LocalSlamResultData(const std::string& sensor_id, common::Time time); common::Time GetTime() const override { return time_; } virtual void AddToPoseGraph(int trajectory_id, - mapping::PoseGraph *pose_graph) const = 0; + mapping::PoseGraph* pose_graph) const = 0; private: common::Time time_; @@ -40,15 +40,15 @@ class LocalSlamResultData : public cartographer::sensor::Data { class LocalSlamResult2D : public LocalSlamResultData { public: LocalSlamResult2D( - const std::string &sensor_id, common::Time time, + const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector> - &insertion_submaps); + const std::vector>& + insertion_submaps); void AddToTrajectoryBuilder( - mapping::TrajectoryBuilderInterface *const trajectory_builder) override; + mapping::TrajectoryBuilderInterface* const trajectory_builder) override; void AddToPoseGraph(int trajectory_id, - mapping::PoseGraph *pose_graph) const override; + mapping::PoseGraph* pose_graph) const override; private: std::shared_ptr node_data_; @@ -58,15 +58,15 @@ class LocalSlamResult2D : public LocalSlamResultData { class LocalSlamResult3D : public LocalSlamResultData { public: LocalSlamResult3D( - const std::string &sensor_id, common::Time time, + const std::string& sensor_id, common::Time time, std::shared_ptr node_data, - const std::vector> - &insertion_submaps); + const std::vector>& + insertion_submaps); void AddToTrajectoryBuilder( - mapping::TrajectoryBuilderInterface *const trajectory_builder) override; + mapping::TrajectoryBuilderInterface* const trajectory_builder) override; void AddToPoseGraph(int trajectory_id, - mapping::PoseGraph *pose_graph) const override; + mapping::PoseGraph* pose_graph) const override; private: std::shared_ptr node_data_; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index cd00345..8640dfd 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -58,7 +58,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions( MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { if (options.use_trajectory_builder_2d()) { - pose_graph_2d_ = common::make_unique( + pose_graph_2d_ = common::make_unique( options_.pose_graph_options(), &thread_pool_); pose_graph_ = pose_graph_2d_.get(); } @@ -74,8 +74,6 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) } } -MapBuilder::~MapBuilder() {} - int MapBuilder::AddTrajectoryBuilder( const std::set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, @@ -111,10 +109,9 @@ int MapBuilder::AddTrajectoryBuilder( 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))); + mapping_2d::proto::LocalTrajectoryBuilderOptions, PoseGraph2D>>( + std::move(local_trajectory_builder), trajectory_id, + pose_graph_2d_.get(), local_slam_result_callback))); } if (trajectory_options.pure_localization()) { constexpr int kSubmapsToKeep = 3; diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 16fda2a..db2ce33 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -23,9 +23,8 @@ #include #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" -#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_2d/pose_graph_2d.h" #include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/sensor/collator_interface.h" @@ -40,7 +39,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions( class MapBuilder : public MapBuilderInterface { public: explicit MapBuilder(const proto::MapBuilderOptions& options); - ~MapBuilder() override; + ~MapBuilder() override {} MapBuilder(const MapBuilder&) = delete; MapBuilder& operator=(const MapBuilder&) = delete; @@ -77,7 +76,7 @@ class MapBuilder : public MapBuilderInterface { const proto::MapBuilderOptions options_; common::ThreadPool thread_pool_; - std::unique_ptr pose_graph_2d_; + std::unique_ptr pose_graph_2d_; std::unique_ptr pose_graph_3d_; mapping::PoseGraph* pose_graph_; diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph_2d.cc similarity index 77% rename from cartographer/mapping_2d/pose_graph.cc rename to cartographer/mapping_2d/pose_graph_2d.cc index 8134d79..e02226f 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_2d/pose_graph_2d.h" #include #include @@ -37,23 +37,24 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { -PoseGraph::PoseGraph(const mapping::proto::PoseGraphOptions& options, - common::ThreadPool* thread_pool) +PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions& options, + common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options()), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} -PoseGraph::~PoseGraph() { +PoseGraph2D::~PoseGraph2D() { WaitForAllComputations(); common::MutexLocker locker(&mutex_); CHECK(work_queue_ == nullptr); } -std::vector PoseGraph::InitializeGlobalSubmapPoses( +std::vector PoseGraph2D::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, - const std::vector>& insertion_submaps) { + const std::vector>& + insertion_submaps) { CHECK(!insertion_submaps.empty()); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { @@ -71,14 +72,14 @@ std::vector PoseGraph::InitializeGlobalSubmapPoses( insertion_submaps[0]->local_pose())); } CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); - const mapping::SubmapId submap_id{trajectory_id, 0}; + const SubmapId submap_id{trajectory_id, 0}; CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); return {submap_id}; } CHECK_EQ(2, insertion_submaps.size()); const auto end_it = submap_data.EndOfTrajectory(trajectory_id); CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); - const mapping::SubmapId last_submap_id = std::prev(end_it)->id; + const SubmapId last_submap_id = std::prev(end_it)->id; if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { // In this case, 'last_submap_id' is the ID of // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new. @@ -86,29 +87,31 @@ std::vector PoseGraph::InitializeGlobalSubmapPoses( optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * - pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() * - pose_graph::ComputeSubmapPose(*insertion_submaps[1])); + mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[0]) + .inverse() * + mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[1])); return {last_submap_id, - mapping::SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; + SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; } CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back()); - const mapping::SubmapId front_submap_id{trajectory_id, - last_submap_id.submap_index - 1}; + const SubmapId front_submap_id{trajectory_id, + last_submap_id.submap_index - 1}; CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front()); return {front_submap_id, last_submap_id}; } -mapping::NodeId PoseGraph::AddNode( - std::shared_ptr constant_data, +NodeId PoseGraph2D::AddNode( + std::shared_ptr constant_data, const int trajectory_id, - const std::vector>& insertion_submaps) { + const std::vector>& + 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}); + const NodeId node_id = trajectory_nodes_.Append( + trajectory_id, TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; // Test if the 'insertion_submap.back()' is one we never saw before. @@ -117,8 +120,7 @@ mapping::NodeId PoseGraph::AddNode( 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()); + const SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = insertion_submaps.back(); } @@ -132,7 +134,7 @@ mapping::NodeId PoseGraph::AddNode( return node_id; } -void PoseGraph::AddWorkItem(const std::function& work_item) { +void PoseGraph2D::AddWorkItem(const std::function& work_item) { if (work_queue_ == nullptr) { work_item(); } else { @@ -140,7 +142,7 @@ void PoseGraph::AddWorkItem(const std::function& work_item) { } } -void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { +void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { trajectory_connectivity_state_.Add(trajectory_id); // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { @@ -150,30 +152,30 @@ void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { } } -void PoseGraph::AddImuData(const int trajectory_id, - const sensor::ImuData& imu_data) { +void PoseGraph2D::AddImuData(const int trajectory_id, + const sensor::ImuData& imu_data) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_.AddImuData(trajectory_id, imu_data); }); } -void PoseGraph::AddOdometryData(const int trajectory_id, - const sensor::OdometryData& odometry_data) { +void PoseGraph2D::AddOdometryData(const int trajectory_id, + const sensor::OdometryData& odometry_data) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_.AddOdometryData(trajectory_id, odometry_data); }); } -void PoseGraph::AddFixedFramePoseData( +void PoseGraph2D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { LOG(FATAL) << "Not yet implemented for 2D."; } -void PoseGraph::AddLandmarkData(int trajectory_id, - const sensor::LandmarkData& landmark_data) +void PoseGraph2D::AddLandmarkData(int trajectory_id, + const sensor::LandmarkData& landmark_data) EXCLUDES(mutex_) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { @@ -187,8 +189,8 @@ void PoseGraph::AddLandmarkData(int trajectory_id, }); } -void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph2D::ComputeConstraint(const NodeId& node_id, + const SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); const common::Time node_time = GetLatestNodeTime(node_id, submap_id); @@ -220,44 +222,46 @@ void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void PoseGraph::ComputeConstraintsForOldNodes( - const mapping::SubmapId& submap_id) { +void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { - const mapping::NodeId& node_id = node_id_data.id; + const NodeId& node_id = node_id_data.id; if (submap_data.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } } } -void PoseGraph::ComputeConstraintsForNode( - const mapping::NodeId& node_id, - std::vector> insertion_submaps, +void PoseGraph2D::ComputeConstraintsForNode( + const NodeId& node_id, + std::vector> insertion_submaps, const bool newly_finished_submap) { const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; - const std::vector submap_ids = InitializeGlobalSubmapPoses( + const std::vector submap_ids = InitializeGlobalSubmapPoses( node_id.trajectory_id, constant_data->time, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); - const mapping::SubmapId matching_id = submap_ids.front(); + const SubmapId matching_id = submap_ids.front(); const transform::Rigid2d pose = transform::Project2D( constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data().at(matching_id).global_pose * - pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * + mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps.front()) + .inverse() * pose; optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, constant_data->time, pose, optimized_pose, constant_data->gravity_alignment); for (size_t i = 0; i < insertion_submaps.size(); ++i) { - const mapping::SubmapId submap_id = submap_ids[i]; + const SubmapId submap_id = submap_ids[i]; // Even if this was the last node added to 'submap_id', the submap will // only be marked as finished in 'submap_data_' further below. CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = - pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose; + mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[i]) + .inverse() * + pose; constraints_.push_back(Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), @@ -274,7 +278,7 @@ void PoseGraph::ComputeConstraintsForNode( } if (newly_finished_submap) { - const mapping::SubmapId finished_submap_id = submap_ids.front(); + const SubmapId finished_submap_id = submap_ids.front(); SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); CHECK(finished_submap_data.state == SubmapState::kActive); finished_submap_data.state = SubmapState::kFinished; @@ -291,7 +295,7 @@ void PoseGraph::ComputeConstraintsForNode( } } -void PoseGraph::DispatchOptimization() { +void PoseGraph2D::DispatchOptimization() { run_loop_closure_ = true; // If there is a 'work_queue_' already, some other thread will take care. if (work_queue_ == nullptr) { @@ -299,12 +303,12 @@ void PoseGraph::DispatchOptimization() { HandleWorkQueue(); } } -common::Time PoseGraph::GetLatestNodeTime( - const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { +common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; const SubmapData& submap_data = submap_data_.at(submap_id); if (!submap_data.node_ids.empty()) { - const mapping::NodeId last_submap_node_id = + const NodeId last_submap_node_id = *submap_data_.at(submap_id).node_ids.rbegin(); time = std::max( time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); @@ -312,8 +316,8 @@ common::Time PoseGraph::GetLatestNodeTime( return time; } -void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) { - CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); +void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) { + CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, @@ -321,9 +325,9 @@ void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) { time); } -void PoseGraph::HandleWorkQueue() { +void PoseGraph2D::HandleWorkQueue() { constraint_builder_.WhenDone( - [this](const pose_graph::ConstraintBuilder::Result& result) { + [this](const mapping_2d::pose_graph::ConstraintBuilder::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); @@ -339,11 +343,10 @@ void PoseGraph::HandleWorkQueue() { trimmer->Trim(&trimming_handle); } trimmers_.erase( - std::remove_if( - trimmers_.begin(), trimmers_.end(), - [](std::unique_ptr& trimmer) { - return trimmer->IsFinished(); - }), + std::remove_if(trimmers_.begin(), trimmers_.end(), + [](std::unique_ptr& trimmer) { + return trimmer->IsFinished(); + }), trimmers_.end()); num_nodes_since_last_loop_closure_ = 0; @@ -362,7 +365,7 @@ void PoseGraph::HandleWorkQueue() { }); } -void PoseGraph::WaitForAllComputations() { +void PoseGraph2D::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); const int num_finished_nodes_at_start = @@ -385,8 +388,8 @@ void PoseGraph::WaitForAllComputations() { } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( - [this, - ¬ification](const pose_graph::ConstraintBuilder::Result& result) { + [this, ¬ification]( + const mapping_2d::pose_graph::ConstraintBuilder::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; @@ -394,7 +397,7 @@ void PoseGraph::WaitForAllComputations() { locker.Await([¬ification]() { return notification; }); } -void PoseGraph::FinishTrajectory(const int trajectory_id) { +void PoseGraph2D::FinishTrajectory(const int trajectory_id) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); finished_trajectories_.insert(trajectory_id); @@ -407,11 +410,11 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) { }); } -bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) { +bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) { return finished_trajectories_.count(trajectory_id) > 0; } -void PoseGraph::FreezeTrajectory(const int trajectory_id) { +void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { common::MutexLocker locker(&mutex_); trajectory_connectivity_state_.Add(trajectory_id); AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { @@ -420,16 +423,16 @@ void PoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, - const mapping::proto::Submap& submap) { +void PoseGraph2D::AddSubmapFromProto( + const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { if (!submap.has_submap_2d()) { return; } - const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(), - submap.submap_id().submap_index()}; - std::shared_ptr submap_ptr = - std::make_shared(submap.submap_2d()); + const SubmapId submap_id = {submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; + std::shared_ptr submap_ptr = + std::make_shared(submap.submap_2d()); const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose); @@ -438,8 +441,8 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, submap_data_.Insert(submap_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. - global_submap_poses_.Insert(submap_id, - pose_graph::SubmapData{global_submap_pose_2d}); + global_submap_poses_.Insert( + submap_id, mapping_2d::pose_graph::SubmapData{global_submap_pose_2d}); AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -447,18 +450,16 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, }); } -void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Node& node) { - const mapping::NodeId node_id = {node.node_id().trajectory_id(), - node.node_id().node_index()}; - std::shared_ptr constant_data = - std::make_shared( - mapping::FromProto(node.node_data())); +void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, + const proto::Node& node) { + const NodeId node_id = {node.node_id().trajectory_id(), + node.node_id().node_index()}; + std::shared_ptr constant_data = + std::make_shared(FromProto(node.node_data())); common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(node_id.trajectory_id); - trajectory_nodes_.Insert(node_id, - mapping::TrajectoryNode{constant_data, global_pose}); + trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose}); AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1); @@ -474,20 +475,20 @@ void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } -void PoseGraph::SetTrajectoryDataFromProto( - const mapping::proto::TrajectoryData& data) { +void PoseGraph2D::SetTrajectoryDataFromProto( + const proto::TrajectoryData& data) { // Not implemented yet in 2D. } -void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { submap_data_.at(submap_id).node_ids.insert(node_id); }); } -void PoseGraph::AddSerializedConstraints( +void PoseGraph2D::AddSerializedConstraints( const std::vector& constraints) { common::MutexLocker locker(&mutex_); AddWorkItem([this, constraints]() REQUIRES(mutex_) { @@ -519,15 +520,15 @@ void PoseGraph::AddSerializedConstraints( }); } -void PoseGraph::AddTrimmer(std::unique_ptr trimmer) { +void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { common::MutexLocker locker(&mutex_); // C++11 does not allow us to move a unique_ptr into a lambda. - mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); + PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); AddWorkItem([this, trimmer_ptr]() REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); } -void PoseGraph::RunFinalOptimization() { +void PoseGraph2D::RunFinalOptimization() { { common::MutexLocker locker(&mutex_); AddWorkItem([this]() REQUIRES(mutex_) { @@ -545,7 +546,7 @@ void PoseGraph::RunFinalOptimization() { WaitForAllComputations(); } -void PoseGraph::RunOptimization() { +void PoseGraph2D::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } @@ -578,7 +579,7 @@ void PoseGraph::RunOptimization() { const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); - const mapping::NodeId last_optimized_node_id = + const NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id; auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); @@ -594,26 +595,24 @@ void PoseGraph::RunOptimization() { global_submap_poses_ = submap_data; } -mapping::MapById -PoseGraph::GetTrajectoryNodes() { +MapById PoseGraph2D::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); return trajectory_nodes_; } -mapping::MapById -PoseGraph::GetTrajectoryNodePoses() { - mapping::MapById node_poses; +MapById PoseGraph2D::GetTrajectoryNodePoses() { + MapById node_poses; common::MutexLocker locker(&mutex_); for (const auto& node_id_data : trajectory_nodes_) { node_poses.Insert( node_id_data.id, - mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr, - node_id_data.data.global_pose}); + TrajectoryNodePose{node_id_data.data.constant_data != nullptr, + node_id_data.data.global_pose}); } return node_poses; } -std::map PoseGraph::GetLandmarkPoses() { +std::map PoseGraph2D::GetLandmarkPoses() { std::map landmark_poses; for (const auto& landmark : landmark_nodes_) { // Landmark without value has not been optimized yet. @@ -624,27 +623,27 @@ std::map PoseGraph::GetLandmarkPoses() { return landmark_poses; } -sensor::MapByTime PoseGraph::GetImuData() { +sensor::MapByTime PoseGraph2D::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); } -sensor::MapByTime PoseGraph::GetOdometryData() { +sensor::MapByTime PoseGraph2D::GetOdometryData() { common::MutexLocker locker(&mutex_); return optimization_problem_.odometry_data(); } -std::map -PoseGraph::GetTrajectoryData() { +std::map +PoseGraph2D::GetTrajectoryData() { return {}; // Not implemented yet in 2D. } sensor::MapByTime -PoseGraph::GetFixedFramePoseData() { +PoseGraph2D::GetFixedFramePoseData() { return {}; // Not implemented yet in 2D. } -std::vector PoseGraph::constraints() { +std::vector PoseGraph2D::constraints() { std::vector result; common::MutexLocker locker(&mutex_); for (const Constraint& constraint : constraints_) { @@ -661,16 +660,16 @@ std::vector PoseGraph::constraints() { return result; } -void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, - const int to_trajectory_id, - const transform::Rigid3d& pose, - const common::Time time) { +void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id, + const int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) { common::MutexLocker locker(&mutex_); initial_trajectory_poses_[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time}; } -transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose( +transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( const int trajectory_id, const common::Time time) const { CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); @@ -690,27 +689,25 @@ transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose( .transform; } -transform::Rigid3d PoseGraph::GetLocalToGlobalTransform( +transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); } -std::vector> PoseGraph::GetConnectedTrajectories() { +std::vector> PoseGraph2D::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData( - const mapping::SubmapId& submap_id) { +PoseGraph::SubmapData PoseGraph2D::GetSubmapData(const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } -mapping::MapById -PoseGraph::GetAllSubmapData() { +MapById +PoseGraph2D::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - mapping::MapById - submaps; + MapById submaps; for (const auto& submap_id_data : submap_data_) { submaps.Insert(submap_id_data.id, GetSubmapDataUnderLock(submap_id_data.id)); @@ -718,22 +715,22 @@ PoseGraph::GetAllSubmapData() { return submaps; } -mapping::MapById -PoseGraph::GetAllSubmapPoses() { +MapById +PoseGraph2D::GetAllSubmapPoses() { common::MutexLocker locker(&mutex_); - mapping::MapById submap_poses; + MapById submap_poses; for (const auto& submap_id_data : submap_data_) { auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); submap_poses.Insert( submap_id_data.id, - mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(), - submap_data.pose}); + PoseGraph::SubmapPose{submap_data.submap->num_range_data(), + submap_data.pose}); } return submap_poses; } -transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( - const mapping::MapById& +transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( + const MapById& global_submap_poses, const int trajectory_id) const { auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); @@ -748,7 +745,7 @@ transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( return transform::Rigid3d::Identity(); } } - const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; + const SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. return transform::Embed3D( global_submap_poses.at(last_optimized_submap_id).global_pose) * @@ -757,8 +754,8 @@ transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( .inverse(); } -mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock( - const mapping::SubmapId& submap_id) { +PoseGraph::SubmapData PoseGraph2D::GetSubmapDataUnderLock( + const SubmapId& submap_id) { const auto it = submap_data_.find(submap_id); if (it == submap_data_.end()) { return {}; @@ -775,27 +772,27 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock( submap->local_pose()}; } -PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent) +PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent) : parent_(parent) {} -int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const { +int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const { const auto& submap_data = parent_->optimization_problem_.submap_data(); return submap_data.SizeOfTrajectoryOrZero(trajectory_id); } -bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const { +bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { return parent_->IsTrajectoryFinished(trajectory_id); } -void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( - const mapping::SubmapId& submap_id) { +void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed( + const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); // Compile all nodes that are still INTRA_SUBMAP constrained once the submap // with 'submap_id' is gone. - std::set nodes_to_retain; + std::set nodes_to_retain; for (const Constraint& constraint : parent_->constraints_) { if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && constraint.submap_id != submap_id) { @@ -803,7 +800,7 @@ void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( } } // Remove all 'constraints_' related to 'submap_id'. - std::set nodes_to_remove; + std::set nodes_to_remove; { std::vector constraints; for (const Constraint& constraint : parent_->constraints_) { @@ -839,11 +836,11 @@ void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( // Remove the 'nodes_to_remove' from the pose graph and the optimization // problem. - for (const mapping::NodeId& node_id : nodes_to_remove) { + for (const NodeId& node_id : nodes_to_remove) { parent_->trajectory_nodes_.Trim(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id); } } -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph_2d.h similarity index 73% rename from cartographer/mapping_2d/pose_graph.h rename to cartographer/mapping_2d/pose_graph_2d.h index 9ef4600..9ae47ef 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ -#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_ #include #include @@ -46,7 +46,7 @@ #include "cartographer/transform/transform.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { // Implements the loop closure method called Sparse Pose Adjustment (SPA) from // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." @@ -57,25 +57,24 @@ namespace mapping_2d { // Each node has been matched against one or more submaps (adding a constraint // for each match), both poses of nodes and of submaps are to be optimized. // All constraints are between a submap i and a node j. -class PoseGraph : public mapping::PoseGraph { +class PoseGraph2D : public PoseGraph { public: - PoseGraph(const mapping::proto::PoseGraphOptions& options, - common::ThreadPool* thread_pool); - ~PoseGraph() override; + PoseGraph2D(const proto::PoseGraphOptions& options, + common::ThreadPool* thread_pool); + ~PoseGraph2D() override; - PoseGraph(const PoseGraph&) = delete; - PoseGraph& operator=(const PoseGraph&) = delete; + PoseGraph2D(const PoseGraph2D&) = delete; + PoseGraph2D& operator=(const PoseGraph2D&) = delete; // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // determined by scan matching against 'insertion_submaps.front()' and the // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - mapping::NodeId AddNode( - std::shared_ptr constant_data, - int trajectory_id, - const std::vector>& insertion_submaps) - EXCLUDES(mutex_); + NodeId AddNode(std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& + insertion_submaps) EXCLUDES(mutex_); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override EXCLUDES(mutex_); @@ -94,30 +93,28 @@ class PoseGraph : public mapping::PoseGraph { bool IsTrajectoryFinished(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override; void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, - const mapping::proto::Submap& submap) override; + const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Node& node) override; - void SetTrajectoryDataFromProto( - const mapping::proto::TrajectoryData& data) override; - void AddNodeToSubmap(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) override; + const proto::Node& node) override; + void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override; + void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) override; void AddSerializedConstraints( const std::vector& constraints) override; - void AddTrimmer(std::unique_ptr trimmer) override; + void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - mapping::PoseGraph::SubmapData GetSubmapData( - const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; - mapping::MapById - GetAllSubmapData() EXCLUDES(mutex_) override; - mapping::MapById GetAllSubmapPoses() + PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) EXCLUDES(mutex_) override; + MapById GetAllSubmapData() + EXCLUDES(mutex_) override; + MapById GetAllSubmapPoses() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; - mapping::MapById - GetTrajectoryNodes() override EXCLUDES(mutex_); - mapping::MapById - GetTrajectoryNodePoses() override EXCLUDES(mutex_); + MapById GetTrajectoryNodes() override + EXCLUDES(mutex_); + MapById GetTrajectoryNodePoses() override + EXCLUDES(mutex_); std::map GetLandmarkPoses() override EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); @@ -140,12 +137,12 @@ class PoseGraph : public mapping::PoseGraph { // Likewise, all new nodes are matched against submaps which are finished. enum class SubmapState { kActive, kFinished }; struct SubmapData { - std::shared_ptr submap; + std::shared_ptr submap; // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. - std::set node_ids; + std::set node_ids; SubmapState state = SubmapState::kActive; }; @@ -158,23 +155,23 @@ class PoseGraph : public mapping::PoseGraph { // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. - std::vector InitializeGlobalSubmapPoses( + std::vector InitializeGlobalSubmapPoses( int trajectory_id, const common::Time time, - const std::vector>& insertion_submaps) - REQUIRES(mutex_); + const std::vector>& + insertion_submaps) REQUIRES(mutex_); // Adds constraints for a node, and starts scan matching in the background. void ComputeConstraintsForNode( - const mapping::NodeId& node_id, - std::vector> insertion_submaps, + const NodeId& node_id, + std::vector> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_); // Computes constraints for a node and submap pair. - void ComputeConstraint(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) REQUIRES(mutex_); + void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) + REQUIRES(mutex_); // Adds constraints for older nodes whenever a new submap is finished. - void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id) + void ComputeConstraintsForOldNodes(const SubmapId& submap_id) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have @@ -192,22 +189,22 @@ class PoseGraph : public mapping::PoseGraph { // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( - const mapping::MapById& + const MapById& global_submap_poses, int trajectory_id) const REQUIRES(mutex_); - mapping::PoseGraph::SubmapData GetSubmapDataUnderLock( - const mapping::SubmapId& submap_id) REQUIRES(mutex_); + PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) + REQUIRES(mutex_); - common::Time GetLatestNodeTime(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) const + common::Time GetLatestNodeTime(const NodeId& node_id, + const SubmapId& submap_id) const REQUIRES(mutex_); // Updates the trajectory connectivity structure with a new constraint. void UpdateTrajectoryConnectivity(const Constraint& constraint) REQUIRES(mutex_); - const mapping::proto::PoseGraphOptions options_; + const proto::PoseGraphOptions options_; common::Mutex mutex_; // If it exists, further work items must be added to this queue, and will be @@ -216,7 +213,7 @@ class PoseGraph : public mapping::PoseGraph { GUARDED_BY(mutex_); // How our various trajectories are related. - mapping::TrajectoryConnectivityState trajectory_connectivity_state_; + TrajectoryConnectivityState trajectory_connectivity_state_; // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> @@ -232,31 +229,29 @@ class PoseGraph : public mapping::PoseGraph { void DispatchOptimization() REQUIRES(mutex_); // Current optimization problem. - pose_graph::OptimizationProblem optimization_problem_; - pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); + mapping_2d::pose_graph::OptimizationProblem optimization_problem_; + mapping_2d::pose_graph::ConstraintBuilder constraint_builder_ + GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. - mapping::MapById submap_data_ - GUARDED_BY(mutex_); + MapById submap_data_ GUARDED_BY(mutex_); // Data that are currently being shown. - mapping::MapById trajectory_nodes_ - GUARDED_BY(mutex_); + MapById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - mapping::MapById - global_submap_poses_ GUARDED_BY(mutex_); + MapById global_submap_poses_ + GUARDED_BY(mutex_); // Global landmark poses with all observations. std::map landmark_nodes_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. - std::vector> trimmers_ - GUARDED_BY(mutex_); + std::vector> trimmers_ GUARDED_BY(mutex_); // Set of all frozen trajectories not being optimized. std::set frozen_trajectories_ GUARDED_BY(mutex_); @@ -270,22 +265,22 @@ class PoseGraph : public mapping::PoseGraph { // Allows querying and manipulating the pose graph by the 'trimmers_'. The // 'mutex_' of the pose graph is held while this class is used. - class TrimmingHandle : public mapping::Trimmable { + class TrimmingHandle : public Trimmable { public: - TrimmingHandle(PoseGraph* parent); + TrimmingHandle(PoseGraph2D* parent); ~TrimmingHandle() override {} int num_submaps(int trajectory_id) const override; - void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) + void MarkSubmapAsTrimmed(const SubmapId& submap_id) REQUIRES(parent_->mutex_) override; bool IsFinished(int trajectory_id) const override; private: - PoseGraph* const parent_; + PoseGraph2D* const parent_; }; }; -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_ diff --git a/cartographer/mapping_2d/pose_graph_test.cc b/cartographer/mapping_2d/pose_graph_2d_test.cc similarity index 84% rename from cartographer/mapping_2d/pose_graph_test.cc rename to cartographer/mapping_2d/pose_graph_2d_test.cc index 4ecff80..76be41f 100644 --- a/cartographer/mapping_2d/pose_graph_test.cc +++ b/cartographer/mapping_2d/pose_graph_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_2d/pose_graph_2d.h" #include #include @@ -32,12 +32,12 @@ #include "gmock/gmock.h" namespace cartographer { -namespace mapping_2d { +namespace mapping { namespace { -class PoseGraphTest : public ::testing::Test { +class PoseGraph2DTest : public ::testing::Test { protected: - PoseGraphTest() : thread_pool_(1) { + PoseGraph2DTest() : thread_pool_(1) { // Builds a wavy, irregularly circular point cloud that is unique // rotationally. This gives us good rotational texture and avoids any // possibility of the CeresScanMatcher preferring free space (> @@ -58,8 +58,8 @@ class PoseGraphTest : public ::testing::Test { miss_probability = 0.495, }, })text"); - active_submaps_ = common::make_unique( - CreateSubmapsOptions(parameter_dictionary.get())); + active_submaps_ = common::make_unique( + mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())); } { @@ -132,9 +132,8 @@ class PoseGraphTest : public ::testing::Test { log_residual_histograms = true, global_constraint_search_after_n_seconds = 10.0, })text"); - pose_graph_ = common::make_unique( - mapping::CreatePoseGraphOptions(parameter_dictionary.get()), - &thread_pool_); + pose_graph_ = common::make_unique( + CreatePoseGraphOptions(parameter_dictionary.get()), &thread_pool_); } current_pose_ = transform::Rigid2d::Identity(); @@ -146,7 +145,7 @@ class PoseGraphTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - std::vector> insertion_submaps; + std::vector> insertion_submaps; for (const auto& submap : active_submaps_->submaps()) { insertion_submaps.push_back(submap); } @@ -158,14 +157,14 @@ class PoseGraphTest : public ::testing::Test { range_data, transform::Embed3D(pose_estimate.cast()))); pose_graph_->AddNode( - std::make_shared( - mapping::TrajectoryNode::Data{common::FromUniversal(0), - Eigen::Quaterniond::Identity(), - range_data.returns, - {}, - {}, - {}, - transform::Embed3D(pose_estimate)}), + std::make_shared( + TrajectoryNode::Data{common::FromUniversal(0), + Eigen::Quaterniond::Identity(), + range_data.returns, + {}, + {}, + {}, + transform::Embed3D(pose_estimate)}), kTrajectoryId, insertion_submaps); } @@ -179,19 +178,19 @@ class PoseGraphTest : public ::testing::Test { } sensor::PointCloud point_cloud_; - std::unique_ptr active_submaps_; + std::unique_ptr active_submaps_; common::ThreadPool thread_pool_; - std::unique_ptr pose_graph_; + std::unique_ptr pose_graph_; transform::Rigid2d current_pose_; }; -TEST_F(PoseGraphTest, EmptyMap) { +TEST_F(PoseGraph2DTest, EmptyMap) { pose_graph_->RunFinalOptimization(); const auto nodes = pose_graph_->GetTrajectoryNodes(); EXPECT_TRUE(nodes.empty()); } -TEST_F(PoseGraphTest, NoMovement) { +TEST_F(PoseGraph2DTest, NoMovement) { MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); @@ -200,15 +199,15 @@ TEST_F(PoseGraphTest, NoMovement) { ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u)); - EXPECT_THAT(nodes.at(mapping::NodeId{0, 0}).global_pose, + EXPECT_THAT(nodes.at(NodeId{0, 0}).global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes.at(mapping::NodeId{0, 1}).global_pose, + EXPECT_THAT(nodes.at(NodeId{0, 1}).global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes.at(mapping::NodeId{0, 2}).global_pose, + EXPECT_THAT(nodes.at(NodeId{0, 2}).global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } -TEST_F(PoseGraphTest, NoOverlappingNodes) { +TEST_F(PoseGraph2DTest, NoOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -221,15 +220,15 @@ TEST_F(PoseGraphTest, NoOverlappingNodes) { ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); for (int i = 0; i != 4; ++i) { - EXPECT_THAT(poses[i], - IsNearly(transform::Project2D( - nodes.at(mapping::NodeId{0, i}).global_pose), - 1e-2)) + EXPECT_THAT( + poses[i], + IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose), + 1e-2)) << i; } } -TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) { +TEST_F(PoseGraph2DTest, ConsecutivelyOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -242,15 +241,15 @@ TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) { ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); for (int i = 0; i != 5; ++i) { - EXPECT_THAT(poses[i], - IsNearly(transform::Project2D( - nodes.at(mapping::NodeId{0, i}).global_pose), - 1e-2)) + EXPECT_THAT( + poses[i], + IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose), + 1e-2)) << i; } } -TEST_F(PoseGraphTest, OverlappingNodes) { +TEST_F(PoseGraph2DTest, OverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector ground_truth; @@ -285,5 +284,5 @@ TEST_F(PoseGraphTest, OverlappingNodes) { } } // namespace -} // namespace mapping_2d +} // namespace mapping } // namespace cartographer