diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/pose_graph/constraint_builder.cc similarity index 98% rename from cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc rename to cartographer/mapping_2d/pose_graph/constraint_builder.cc index 30c0412..89d62af 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" +#include "cartographer/mapping_2d/pose_graph/constraint_builder.h" #include #include @@ -36,7 +36,7 @@ namespace cartographer { namespace mapping_2d { -namespace sparse_pose_graph { +namespace pose_graph { transform::Rigid2d ComputeSubmapPose(const Submap& submap) { return transform::Project2D(submap.local_pose()); @@ -287,6 +287,6 @@ void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) { submap_scan_matchers_.erase(submap_id); } -} // namespace sparse_pose_graph +} // namespace pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder.h similarity index 96% rename from cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h rename to cartographer/mapping_2d/pose_graph/constraint_builder.h index 44f61d8..3060abc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #include #include @@ -42,7 +42,7 @@ namespace cartographer { namespace mapping_2d { -namespace sparse_pose_graph { +namespace pose_graph { // Returns (map <- submap) where 'submap' is a coordinate system at the origin // of the Submap. @@ -178,8 +178,8 @@ class ConstraintBuilder { common::Histogram score_histogram_ GUARDED_BY(mutex_); }; -} // namespace sparse_pose_graph +} // namespace pose_graph } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_ diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/pose_graph/optimization_problem.cc similarity index 97% rename from cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc rename to cartographer/mapping_2d/pose_graph/optimization_problem.cc index 4fa1b2e..c9ec296 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem.h" #include #include @@ -27,7 +27,7 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" -#include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h" +#include "cartographer/mapping_2d/pose_graph/spa_cost_function.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -35,7 +35,7 @@ namespace cartographer { namespace mapping_2d { -namespace sparse_pose_graph { +namespace pose_graph { namespace { @@ -274,6 +274,6 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose( second_node_data.initial_pose); } -} // namespace sparse_pose_graph +} // namespace pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem.h similarity index 93% rename from cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h rename to cartographer/mapping_2d/pose_graph/optimization_problem.h index 8b25631..fd81636 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ -#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #include #include @@ -37,7 +37,7 @@ namespace cartographer { namespace mapping_2d { -namespace sparse_pose_graph { +namespace pose_graph { struct NodeData { common::Time time; @@ -106,8 +106,8 @@ class OptimizationProblem { sensor::MapByTime odometry_data_; }; -} // namespace sparse_pose_graph +} // namespace pose_graph } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ diff --git a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_2d/pose_graph/spa_cost_function.h similarity index 91% rename from cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h rename to cartographer/mapping_2d/pose_graph/spa_cost_function.h index ef113f7..f770ace 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/spa_cost_function.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ #include @@ -30,7 +30,7 @@ namespace cartographer { namespace mapping_2d { -namespace sparse_pose_graph { +namespace pose_graph { class SpaCostFunction { public: @@ -81,8 +81,8 @@ class SpaCostFunction { const Constraint::Pose pose_; }; -} // namespace sparse_pose_graph +} // namespace pose_graph } // namespace mapping_2d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_ diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index d0c6f80..370c4c9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -87,9 +87,8 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * - sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]) - .inverse() * - sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1])); + pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() * + pose_graph::ComputeSubmapPose(*insertion_submaps[1])); return {last_submap_id, mapping::SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; } @@ -232,8 +231,7 @@ void SparsePoseGraph::ComputeConstraintsForNode( transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data().at(matching_id).global_pose * - sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) - .inverse() * + pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * pose; optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, constant_data->time, pose, optimized_pose, @@ -245,8 +243,7 @@ void SparsePoseGraph::ComputeConstraintsForNode( CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = - sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * - pose; + pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose; constraints_.push_back(Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), @@ -310,7 +307,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( void SparsePoseGraph::HandleWorkQueue() { constraint_builder_.WhenDone( - [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { + [this](const pose_graph::ConstraintBuilder::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); @@ -364,8 +361,8 @@ void SparsePoseGraph::WaitForAllComputations() { } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( - [this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { + [this, + ¬ification](const pose_graph::ConstraintBuilder::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; @@ -407,7 +404,7 @@ void SparsePoseGraph::AddSubmapFromProto( submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. optimized_submap_transforms_.Insert( - submap_id, sparse_pose_graph::SubmapData{global_submap_pose_2d}); + submap_id, 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; @@ -638,7 +635,7 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const mapping::MapById& + const mapping::MapById& submap_transforms, const int trajectory_id) const { auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 98f84c7..998847a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -35,8 +35,8 @@ #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity_state.h" -#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" -#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/pose_graph/constraint_builder.h" +#include "cartographer/mapping_2d/pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/odometry_data.h" @@ -175,7 +175,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Computes the local to global map frame transform based on the given // optimized 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( - const mapping::MapById& + const mapping::MapById& submap_transforms, int trajectory_id) const REQUIRES(mutex_); @@ -212,8 +212,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { bool run_loop_closure_ GUARDED_BY(mutex_) = false; // Current optimization problem. - sparse_pose_graph::OptimizationProblem optimization_problem_; - sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); + pose_graph::OptimizationProblem optimization_problem_; + 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 @@ -227,7 +227,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - mapping::MapById + mapping::MapById optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish.