From c25379cd200ef0c77f758c6f242ba9c3dde3dda1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 15 Nov 2017 13:50:18 +0100 Subject: [PATCH] Rename mapping_{2,3}d::SparsePoseGraph. (#678) [RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md) --- cartographer/mapping/map_builder.cc | 16 +-- cartographer/mapping/map_builder.h | 8 +- .../{sparse_pose_graph.cc => pose_graph.cc} | 107 ++++++++--------- .../{sparse_pose_graph.h => pose_graph.h} | 22 ++-- ..._pose_graph_test.cc => pose_graph_test.cc} | 42 +++---- .../{sparse_pose_graph.cc => pose_graph.cc} | 109 +++++++++--------- .../{sparse_pose_graph.h => pose_graph.h} | 22 ++-- 7 files changed, 158 insertions(+), 168 deletions(-) rename cartographer/mapping_2d/{sparse_pose_graph.cc => pose_graph.cc} (89%) rename cartographer/mapping_2d/{sparse_pose_graph.h => pose_graph.h} (94%) rename cartographer/mapping_2d/{sparse_pose_graph_test.cc => pose_graph_test.cc} (90%) rename cartographer/mapping_3d/{sparse_pose_graph.cc => pose_graph.cc} (89%) rename cartographer/mapping_3d/{sparse_pose_graph.h => pose_graph.h} (94%) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index f19bf1a..a57f6cb 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -60,14 +60,14 @@ MapBuilder::MapBuilder( thread_pool_(options.num_background_threads()), local_slam_result_callback_(local_slam_result_callback) { if (options.use_trajectory_builder_2d()) { - sparse_pose_graph_2d_ = common::make_unique( + pose_graph_2d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); - pose_graph_ = sparse_pose_graph_2d_.get(); + pose_graph_ = pose_graph_2d_.get(); } if (options.use_trajectory_builder_3d()) { - sparse_pose_graph_3d_ = common::make_unique( + pose_graph_3d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); - pose_graph_ = sparse_pose_graph_3d_.get(); + pose_graph_ = pose_graph_3d_.get(); } } @@ -85,9 +85,9 @@ int MapBuilder::AddTrajectoryBuilder( common::make_unique>( + mapping_3d::PoseGraph>>( trajectory_options.trajectory_builder_3d_options(), - trajectory_id, sparse_pose_graph_3d_.get(), + trajectory_id, pose_graph_3d_.get(), local_slam_result_callback_))); } else { CHECK(trajectory_options.has_trajectory_builder_2d_options()); @@ -97,9 +97,9 @@ int MapBuilder::AddTrajectoryBuilder( common::make_unique>( + mapping_2d::PoseGraph>>( trajectory_options.trajectory_builder_2d_options(), - trajectory_id, sparse_pose_graph_2d_.get(), + trajectory_id, pose_graph_2d_.get(), local_slam_result_callback_))); } if (trajectory_options.pure_localization()) { diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index e5b5d35..b620252 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -35,8 +35,8 @@ #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" -#include "cartographer/mapping_2d/sparse_pose_graph.h" -#include "cartographer/mapping_3d/sparse_pose_graph.h" +#include "cartographer/mapping_2d/pose_graph.h" +#include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/sensor/collator.h" namespace cartographer { @@ -101,8 +101,8 @@ class MapBuilder { const proto::MapBuilderOptions options_; common::ThreadPool thread_pool_; - std::unique_ptr sparse_pose_graph_2d_; - std::unique_ptr sparse_pose_graph_3d_; + std::unique_ptr pose_graph_2d_; + std::unique_ptr pose_graph_3d_; mapping::PoseGraph* pose_graph_; LocalSlamResultCallback local_slam_result_callback_; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc similarity index 89% rename from cartographer/mapping_2d/sparse_pose_graph.cc rename to cartographer/mapping_2d/pose_graph.cc index e924c37..1c3a00a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/sparse_pose_graph.h" +#include "cartographer/mapping_2d/pose_graph.h" #include #include @@ -39,20 +39,19 @@ namespace cartographer { namespace mapping_2d { -SparsePoseGraph::SparsePoseGraph( - const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool) +PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options()), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} -SparsePoseGraph::~SparsePoseGraph() { +PoseGraph::~PoseGraph() { WaitForAllComputations(); common::MutexLocker locker(&mutex_); CHECK(work_queue_ == nullptr); } -std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( +std::vector PoseGraph::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); @@ -99,7 +98,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -mapping::NodeId SparsePoseGraph::AddNode( +mapping::NodeId PoseGraph::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { @@ -133,7 +132,7 @@ mapping::NodeId SparsePoseGraph::AddNode( return node_id; } -void SparsePoseGraph::AddWorkItem(const std::function& work_item) { +void PoseGraph::AddWorkItem(const std::function& work_item) { if (work_queue_ == nullptr) { work_item(); } else { @@ -141,7 +140,7 @@ void SparsePoseGraph::AddWorkItem(const std::function& work_item) { } } -void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { +void PoseGraph::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]) { @@ -151,30 +150,30 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { } } -void SparsePoseGraph::AddImuData(const int trajectory_id, - const sensor::ImuData& imu_data) { +void PoseGraph::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 SparsePoseGraph::AddOdometryData( - const int trajectory_id, const sensor::OdometryData& odometry_data) { +void PoseGraph::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 SparsePoseGraph::AddFixedFramePoseData( +void PoseGraph::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { LOG(FATAL) << "Not yet implemented for 2D."; } -void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); const common::Time node_time = GetLatestNodeTime(node_id, submap_id); @@ -206,7 +205,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void SparsePoseGraph::ComputeConstraintsForOldNodes( +void PoseGraph::ComputeConstraintsForOldNodes( const mapping::SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { @@ -217,7 +216,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes( } } -void SparsePoseGraph::ComputeConstraintsForNode( +void PoseGraph::ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { @@ -282,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForNode( } } -common::Time SparsePoseGraph::GetLatestNodeTime( +common::Time PoseGraph::GetLatestNodeTime( const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; const SubmapData& submap_data = submap_data_.at(submap_id); @@ -295,8 +294,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime( return time; } -void SparsePoseGraph::UpdateTrajectoryConnectivity( - const Constraint& constraint) { +void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) { CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); @@ -305,7 +303,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( time); } -void SparsePoseGraph::HandleWorkQueue() { +void PoseGraph::HandleWorkQueue() { constraint_builder_.WhenDone( [this](const pose_graph::ConstraintBuilder::Result& result) { { @@ -339,7 +337,7 @@ void SparsePoseGraph::HandleWorkQueue() { }); } -void SparsePoseGraph::WaitForAllComputations() { +void PoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); const int num_finished_nodes_at_start = @@ -370,12 +368,12 @@ void SparsePoseGraph::WaitForAllComputations() { locker.Await([¬ification]() { return notification; }); } -void SparsePoseGraph::FinishTrajectory(const int trajectory_id) { +void PoseGraph::FinishTrajectory(const int trajectory_id) { // TODO(jihoonl): Add a logic to notify trimmers to finish the given // trajectory. } -void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { +void PoseGraph::FreezeTrajectory(const int trajectory_id) { common::MutexLocker locker(&mutex_); trajectory_connectivity_state_.Add(trajectory_id); AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { @@ -384,9 +382,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto( - const transform::Rigid3d& global_submap_pose, - const mapping::proto::Submap& submap) { +void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, + const mapping::proto::Submap& submap) { if (!submap.has_submap_2d()) { return; } @@ -412,8 +409,8 @@ void SparsePoseGraph::AddSubmapFromProto( }); } -void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Node& node) { +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 = @@ -439,15 +436,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } -void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::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 SparsePoseGraph::AddSerializedConstraints( +void PoseGraph::AddSerializedConstraints( const std::vector& constraints) { common::MutexLocker locker(&mutex_); AddWorkItem([this, constraints]() REQUIRES(mutex_) { @@ -479,8 +476,7 @@ void SparsePoseGraph::AddSerializedConstraints( }); } -void SparsePoseGraph::AddTrimmer( - std::unique_ptr trimmer) { +void PoseGraph::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(); @@ -488,7 +484,7 @@ void SparsePoseGraph::AddTrimmer( REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); } -void SparsePoseGraph::RunFinalOptimization() { +void PoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( options_.max_num_final_iterations()); @@ -499,7 +495,7 @@ void SparsePoseGraph::RunFinalOptimization() { .max_num_iterations()); } -void SparsePoseGraph::RunOptimization() { +void PoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } @@ -544,22 +540,22 @@ void SparsePoseGraph::RunOptimization() { } mapping::MapById -SparsePoseGraph::GetTrajectoryNodes() { +PoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); return trajectory_nodes_; } -sensor::MapByTime SparsePoseGraph::GetImuData() { +sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); } -sensor::MapByTime SparsePoseGraph::GetOdometryData() { +sensor::MapByTime PoseGraph::GetOdometryData() { common::MutexLocker locker(&mutex_); return optimization_problem_.odometry_data(); } -std::vector SparsePoseGraph::constraints() { +std::vector PoseGraph::constraints() { std::vector result; common::MutexLocker locker(&mutex_); for (const Constraint& constraint : constraints_) { @@ -576,16 +572,16 @@ std::vector SparsePoseGraph::constraints() { return result; } -void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, - const int to_trajectory_id, - const transform::Rigid3d& pose, - const common::Time time) { +void PoseGraph::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 SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( +transform::Rigid3d PoseGraph::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); @@ -605,25 +601,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( .transform; } -transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( +transform::Rigid3d PoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ComputeLocalToGlobalTransform(optimized_submap_transforms_, trajectory_id); } -std::vector> SparsePoseGraph::GetConnectedTrajectories() { +std::vector> PoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( +mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } mapping::MapById -SparsePoseGraph::GetAllSubmapData() { +PoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); mapping::MapById submaps; for (const auto& submap_id_data : submap_data_) { @@ -633,7 +629,7 @@ SparsePoseGraph::GetAllSubmapData() { return submaps; } -transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( +transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( const mapping::MapById& submap_transforms, const int trajectory_id) const { @@ -658,7 +654,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( .inverse(); } -mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( +mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { const auto it = submap_data_.find(submap_id); if (it == submap_data_.end()) { @@ -677,16 +673,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( submap->local_pose()}; } -SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) +PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent) : parent_(parent) {} -int SparsePoseGraph::TrimmingHandle::num_submaps( - const int trajectory_id) const { +int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const { const auto& submap_data = parent_->optimization_problem_.submap_data(); return submap_data.SizeOfTrajectoryOrZero(trajectory_id); } -void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( +void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( const mapping::SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/pose_graph.h similarity index 94% rename from cartographer/mapping_2d/sparse_pose_graph.h rename to cartographer/mapping_2d/pose_graph.h index 0769842..681547b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ -#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ #include #include @@ -56,14 +56,14 @@ 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 SparsePoseGraph : public mapping::PoseGraph { +class PoseGraph : public mapping::PoseGraph { public: - SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool); - ~SparsePoseGraph() override; + PoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool); + ~PoseGraph() override; - SparsePoseGraph(const SparsePoseGraph&) = delete; - SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + PoseGraph(const PoseGraph&) = delete; + PoseGraph& operator=(const PoseGraph&) = delete; // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // determined by scan matching against 'insertion_submaps.front()' and the @@ -245,7 +245,7 @@ class SparsePoseGraph : public mapping::PoseGraph { // 'mutex_' of the pose graph is held while this class is used. class TrimmingHandle : public mapping::Trimmable { public: - TrimmingHandle(SparsePoseGraph* parent); + TrimmingHandle(PoseGraph* parent); ~TrimmingHandle() override {} int num_submaps(int trajectory_id) const override; @@ -253,11 +253,11 @@ class SparsePoseGraph : public mapping::PoseGraph { REQUIRES(parent_->mutex_) override; private: - SparsePoseGraph* const parent_; + PoseGraph* const parent_; }; }; } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/pose_graph_test.cc similarity index 90% rename from cartographer/mapping_2d/sparse_pose_graph_test.cc rename to cartographer/mapping_2d/pose_graph_test.cc index e117b5c..58fd02f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/pose_graph_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/sparse_pose_graph.h" +#include "cartographer/mapping_2d/pose_graph.h" #include #include @@ -35,9 +35,9 @@ namespace cartographer { namespace mapping_2d { namespace { -class SparsePoseGraphTest : public ::testing::Test { +class PoseGraphTest : public ::testing::Test { protected: - SparsePoseGraphTest() : thread_pool_(1) { + PoseGraphTest() : 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 (> @@ -132,7 +132,7 @@ class SparsePoseGraphTest : public ::testing::Test { log_residual_histograms = true, global_constraint_search_after_n_seconds = 10.0, })text"); - sparse_pose_graph_ = common::make_unique( + pose_graph_ = common::make_unique( mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), &thread_pool_); } @@ -157,7 +157,7 @@ class SparsePoseGraphTest : public ::testing::Test { active_submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - sparse_pose_graph_->AddNode( + pose_graph_->AddNode( std::make_shared( mapping::TrajectoryNode::Data{common::FromUniversal(0), Eigen::Quaterniond::Identity(), @@ -181,22 +181,22 @@ class SparsePoseGraphTest : public ::testing::Test { sensor::PointCloud point_cloud_; std::unique_ptr active_submaps_; common::ThreadPool thread_pool_; - std::unique_ptr sparse_pose_graph_; + std::unique_ptr pose_graph_; transform::Rigid2d current_pose_; }; -TEST_F(SparsePoseGraphTest, EmptyMap) { - sparse_pose_graph_->RunFinalOptimization(); - const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); +TEST_F(PoseGraphTest, EmptyMap) { + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); EXPECT_TRUE(nodes.empty()); } -TEST_F(SparsePoseGraphTest, NoMovement) { +TEST_F(PoseGraphTest, NoMovement) { MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); - sparse_pose_graph_->RunFinalOptimization(); - const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u)); @@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) { transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } -TEST_F(SparsePoseGraphTest, NoOverlappingNodes) { +TEST_F(PoseGraphTest, NoOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -216,8 +216,8 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) { MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.)); poses.emplace_back(current_pose_); } - sparse_pose_graph_->RunFinalOptimization(); - const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); for (int i = 0; i != 4; ++i) { @@ -229,7 +229,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) { } } -TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) { +TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -237,8 +237,8 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) { MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.)); poses.emplace_back(current_pose_); } - sparse_pose_graph_->RunFinalOptimization(); - const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); for (int i = 0; i != 5; ++i) { @@ -250,7 +250,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) { } } -TEST_F(SparsePoseGraphTest, OverlappingNodes) { +TEST_F(PoseGraphTest, OverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector ground_truth; @@ -265,8 +265,8 @@ TEST_F(SparsePoseGraphTest, OverlappingNodes) { ground_truth.emplace_back(current_pose_); poses.emplace_back(noise * current_pose_); } - sparse_pose_graph_->RunFinalOptimization(); - const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + pose_graph_->RunFinalOptimization(); + const auto nodes = pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ::testing::ContainerEq(std::vector{0})); transform::Rigid2d true_movement = diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc similarity index 89% rename from cartographer/mapping_3d/sparse_pose_graph.cc rename to cartographer/mapping_3d/pose_graph.cc index 0f12fa2..d5ab95c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/sparse_pose_graph.h" +#include "cartographer/mapping_3d/pose_graph.h" #include #include @@ -38,21 +38,20 @@ namespace cartographer { namespace mapping_3d { -SparsePoseGraph::SparsePoseGraph( - const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool) +PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options(), pose_graph::OptimizationProblem::FixZ::kNo), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} -SparsePoseGraph::~SparsePoseGraph() { +PoseGraph::~PoseGraph() { WaitForAllComputations(); common::MutexLocker locker(&mutex_); CHECK(work_queue_ == nullptr); } -std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( +std::vector PoseGraph::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); @@ -97,7 +96,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -mapping::NodeId SparsePoseGraph::AddNode( +mapping::NodeId PoseGraph::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { @@ -131,7 +130,7 @@ mapping::NodeId SparsePoseGraph::AddNode( return node_id; } -void SparsePoseGraph::AddWorkItem(const std::function& work_item) { +void PoseGraph::AddWorkItem(const std::function& work_item) { if (work_queue_ == nullptr) { work_item(); } else { @@ -139,7 +138,7 @@ void SparsePoseGraph::AddWorkItem(const std::function& work_item) { } } -void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { +void PoseGraph::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]) { @@ -149,23 +148,23 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { } } -void SparsePoseGraph::AddImuData(const int trajectory_id, - const sensor::ImuData& imu_data) { +void PoseGraph::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 SparsePoseGraph::AddOdometryData( - const int trajectory_id, const sensor::OdometryData& odometry_data) { +void PoseGraph::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 SparsePoseGraph::AddFixedFramePoseData( +void PoseGraph::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { common::MutexLocker locker(&mutex_); @@ -175,8 +174,8 @@ void SparsePoseGraph::AddFixedFramePoseData( }); } -void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); const transform::Rigid3d global_node_pose = @@ -227,7 +226,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void SparsePoseGraph::ComputeConstraintsForOldNodes( +void PoseGraph::ComputeConstraintsForOldNodes( const mapping::SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { @@ -238,7 +237,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes( } } -void SparsePoseGraph::ComputeConstraintsForNode( +void PoseGraph::ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { @@ -299,7 +298,7 @@ void SparsePoseGraph::ComputeConstraintsForNode( } } -common::Time SparsePoseGraph::GetLatestNodeTime( +common::Time PoseGraph::GetLatestNodeTime( const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; const SubmapData& submap_data = submap_data_.at(submap_id); @@ -312,8 +311,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime( return time; } -void SparsePoseGraph::UpdateTrajectoryConnectivity( - const Constraint& constraint) { +void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) { CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); @@ -322,7 +320,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( time); } -void SparsePoseGraph::HandleWorkQueue() { +void PoseGraph::HandleWorkQueue() { constraint_builder_.WhenDone( [this](const pose_graph::ConstraintBuilder::Result& result) { { @@ -356,7 +354,7 @@ void SparsePoseGraph::HandleWorkQueue() { }); } -void SparsePoseGraph::WaitForAllComputations() { +void PoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); const int num_finished_nodes_at_start = @@ -387,12 +385,12 @@ void SparsePoseGraph::WaitForAllComputations() { locker.Await([¬ification]() { return notification; }); } -void SparsePoseGraph::FinishTrajectory(const int trajectory_id) { +void PoseGraph::FinishTrajectory(const int trajectory_id) { // TODO(jihoonl): Add a logic to notify trimmers to finish the given // trajectory. } -void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { +void PoseGraph::FreezeTrajectory(const int trajectory_id) { common::MutexLocker locker(&mutex_); trajectory_connectivity_state_.Add(trajectory_id); AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { @@ -401,9 +399,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto( - const transform::Rigid3d& global_submap_pose, - const mapping::proto::Submap& submap) { +void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, + const mapping::proto::Submap& submap) { if (!submap.has_submap_3d()) { return; } @@ -427,8 +424,8 @@ void SparsePoseGraph::AddSubmapFromProto( }); } -void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Node& node) { +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 = @@ -448,15 +445,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } -void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, - const mapping::SubmapId& submap_id) { +void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::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 SparsePoseGraph::AddSerializedConstraints( +void PoseGraph::AddSerializedConstraints( const std::vector& constraints) { common::MutexLocker locker(&mutex_); AddWorkItem([this, constraints]() REQUIRES(mutex_) { @@ -481,8 +478,7 @@ void SparsePoseGraph::AddSerializedConstraints( }); } -void SparsePoseGraph::AddTrimmer( - std::unique_ptr trimmer) { +void PoseGraph::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(); @@ -490,7 +486,7 @@ void SparsePoseGraph::AddTrimmer( REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); } -void SparsePoseGraph::RunFinalOptimization() { +void PoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( options_.max_num_final_iterations()); @@ -501,7 +497,7 @@ void SparsePoseGraph::RunFinalOptimization() { .max_num_iterations()); } -void SparsePoseGraph::LogResidualHistograms() { +void PoseGraph::LogResidualHistograms() { common::Histogram rotational_residual; common::Histogram translational_residual; for (const Constraint& constraint : constraints_) { @@ -527,7 +523,7 @@ void SparsePoseGraph::LogResidualHistograms() { << rotational_residual.ToString(10); } -void SparsePoseGraph::RunOptimization() { +void PoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } @@ -573,36 +569,36 @@ void SparsePoseGraph::RunOptimization() { } mapping::MapById -SparsePoseGraph::GetTrajectoryNodes() { +PoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); return trajectory_nodes_; } -sensor::MapByTime SparsePoseGraph::GetImuData() { +sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); } -sensor::MapByTime SparsePoseGraph::GetOdometryData() { +sensor::MapByTime PoseGraph::GetOdometryData() { common::MutexLocker locker(&mutex_); return optimization_problem_.odometry_data(); } -std::vector SparsePoseGraph::constraints() { +std::vector PoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; } -void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, - const int to_trajectory_id, - const transform::Rigid3d& pose, - const common::Time time) { +void PoseGraph::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 SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( +transform::Rigid3d PoseGraph::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); @@ -622,25 +618,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( .transform; } -transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( +transform::Rigid3d PoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ComputeLocalToGlobalTransform(optimized_submap_transforms_, trajectory_id); } -std::vector> SparsePoseGraph::GetConnectedTrajectories() { +std::vector> PoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( +mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } mapping::MapById -SparsePoseGraph::GetAllSubmapData() { +PoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); mapping::MapById submaps; for (const auto& submap_id_data : submap_data_) { @@ -650,7 +646,7 @@ SparsePoseGraph::GetAllSubmapData() { return submaps; } -transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( +transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( const mapping::MapById& submap_transforms, const int trajectory_id) const { @@ -674,7 +670,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( .inverse(); } -mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( +mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { const auto it = submap_data_.find(submap_id); if (it == submap_data_.end()) { @@ -691,16 +687,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( submap->local_pose()}; } -SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) +PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent) : parent_(parent) {} -int SparsePoseGraph::TrimmingHandle::num_submaps( - const int trajectory_id) const { +int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const { const auto& submap_data = parent_->optimization_problem_.submap_data(); return submap_data.SizeOfTrajectoryOrZero(trajectory_id); } -void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( +void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( const mapping::SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/pose_graph.h similarity index 94% rename from cartographer/mapping_3d/sparse_pose_graph.h rename to cartographer/mapping_3d/pose_graph.h index 482b1f4..b0ee96b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ -#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_ #include #include @@ -56,14 +56,14 @@ namespace mapping_3d { // 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 SparsePoseGraph : public mapping::PoseGraph { +class PoseGraph : public mapping::PoseGraph { public: - SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool); - ~SparsePoseGraph() override; + PoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool); + ~PoseGraph() override; - SparsePoseGraph(const SparsePoseGraph&) = delete; - SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + PoseGraph(const PoseGraph&) = delete; + PoseGraph& operator=(const PoseGraph&) = delete; // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // determined by scan matching against 'insertion_submaps.front()' and the @@ -249,7 +249,7 @@ class SparsePoseGraph : public mapping::PoseGraph { // 'mutex_' of the pose graph is held while this class is used. class TrimmingHandle : public mapping::Trimmable { public: - TrimmingHandle(SparsePoseGraph* parent); + TrimmingHandle(PoseGraph* parent); ~TrimmingHandle() override {} int num_submaps(int trajectory_id) const override; @@ -257,11 +257,11 @@ class SparsePoseGraph : public mapping::PoseGraph { REQUIRES(parent_->mutex_) override; private: - SparsePoseGraph* const parent_; + PoseGraph* const parent_; }; }; } // namespace mapping_3d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_