diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index f8b075c..8333d20 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -83,7 +83,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( 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. - const auto& first_submap_pose = submap_data.at(last_submap_id).pose; + const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * @@ -192,7 +192,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, // submap's trajectory, it suffices to do a match constrained to a local // search window. const transform::Rigid2d initial_relative_pose = - optimization_problem_.submap_data().at(submap_id).pose.inverse() * + optimization_problem_.submap_data() + .at(submap_id) + .global_pose.inverse() * optimization_problem_.node_data().at(node_id).pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, @@ -229,7 +231,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = - optimization_problem_.submap_data().at(matching_id).pose * + optimization_problem_.submap_data().at(matching_id).global_pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) .inverse() * pose; @@ -386,8 +388,9 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Submap& submap) { +void SparsePoseGraph::AddSubmapFromProto( + const transform::Rigid3d& global_submap_pose, + const mapping::proto::Submap& submap) { if (!submap.has_submap_2d()) { return; } @@ -396,19 +399,20 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, submap.submap_id().submap_index()}; std::shared_ptr submap_ptr = std::make_shared(submap.submap_2d()); - const transform::Rigid2d global_pose_2d = transform::Project2D(global_pose); + const transform::Rigid2d global_submap_pose_2d = + transform::Project2D(global_submap_pose); common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(submap_id.trajectory_id); submap_data_.Insert(submap_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; - // Immediately show the submap at the optimized pose. + // Immediately show the submap at the 'global_submap_pose'. optimized_submap_transforms_.Insert( - submap_id, sparse_pose_graph::SubmapData{global_pose_2d}); - AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) { + submap_id, sparse_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; - optimization_problem_.InsertSubmap(submap_id, global_pose_2d); + optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d); }); } @@ -643,7 +647,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( const mapping::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( - submap_transforms.at(last_optimized_submap_id).pose) * + submap_transforms.at(last_optimized_submap_id).global_pose) * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -658,8 +662,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( auto submap = it->data.submap; if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. - return {submap, transform::Embed3D( - optimized_submap_transforms_.at(submap_id).pose)}; + return {submap, + transform::Embed3D( + optimized_submap_transforms_.at(submap_id).global_pose)}; } // We have to extrapolate. return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2286482..4bc12ea 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -65,11 +65,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new node with 'constant_data' and a 'pose' that will later be - // optimized. The 'pose' was determined by scan matching against - // 'insertion_submaps.front()' and the scan was inserted into the - // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is - // 'true', this submap was inserted into for the last time. + // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was + // determined by scan matching against 'insertion_submaps.front()' and the + // scan 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 AddScan( std::shared_ptr constant_data, int trajectory_id, @@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void FinishTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override; - void AddSubmapFromProto(const transform::Rigid3d& global_pose, + void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const mapping::proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 1dfda4c..e5e5418 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -96,14 +96,15 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { node_data_.Trim(node_id); } -void OptimizationProblem::AddSubmap(const int trajectory_id, - const transform::Rigid2d& submap_pose) { - submap_data_.Append(trajectory_id, SubmapData{submap_pose}); +void OptimizationProblem::AddSubmap( + const int trajectory_id, const transform::Rigid2d& global_submap_pose) { + submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, - const transform::Rigid2d& submap_pose) { - submap_data_.Insert(submap_id, SubmapData{submap_pose}); +void OptimizationProblem::InsertSubmap( + const mapping::SubmapId& submap_id, + const transform::Rigid2d& global_submap_pose) { + submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); } void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { @@ -133,7 +134,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (const auto& submap_id_data : submap_data_) { const bool frozen = frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; - C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.pose)); + C_submaps.Insert(submap_id_data.id, + FromPose(submap_id_data.data.global_pose)); problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3); if (first_submap || frozen) { first_submap = false; @@ -221,7 +223,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, // Store the result. for (const auto& C_submap_id_data : C_submaps) { - submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data); + submap_data_.at(C_submap_id_data.id).global_pose = + ToPose(C_submap_id_data.data); } for (const auto& C_node_id_data : C_nodes) { node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data); diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 834ced0..b91798f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -47,7 +47,7 @@ struct NodeData { }; struct SubmapData { - transform::Rigid2d pose; + transform::Rigid2d global_pose; }; // Implements the SPA loop closure method. @@ -75,14 +75,15 @@ class OptimizationProblem { const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment); void TrimTrajectoryNode(const mapping::NodeId& node_id); - void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); + void AddSubmap(int trajectory_id, + const transform::Rigid2d& global_submap_pose); void InsertSubmap(const mapping::SubmapId& submap_id, - const transform::Rigid2d& submap_pose); + const transform::Rigid2d& global_submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); - // Computes the optimized poses. + // Optimizes the global poses. void Solve(const std::vector& constraints, const std::set& frozen_trajectories); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index b864d21..30dc3e1 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -82,7 +82,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( 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. - const auto& first_submap_pose = submap_data.at(last_submap_id).pose; + const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * insertion_submaps[0]->local_pose().inverse() * @@ -183,7 +183,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, optimization_problem_.node_data().at(node_id).global_pose; const transform::Rigid3d global_submap_pose = - optimization_problem_.submap_data().at(submap_id).pose; + optimization_problem_.submap_data().at(submap_id).global_pose; const transform::Rigid3d global_submap_pose_inverse = global_submap_pose.inverse(); @@ -247,12 +247,12 @@ void SparsePoseGraph::ComputeConstraintsForScan( 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 transform::Rigid3d& pose = constant_data->local_pose; - const transform::Rigid3d optimized_pose = - optimization_problem_.submap_data().at(matching_id).pose * - insertion_submaps.front()->local_pose().inverse() * pose; + const transform::Rigid3d& local_pose = constant_data->local_pose; + const transform::Rigid3d global_pose = + optimization_problem_.submap_data().at(matching_id).global_pose * + insertion_submaps.front()->local_pose().inverse() * local_pose; optimization_problem_.AddTrajectoryNode( - matching_id.trajectory_id, constant_data->time, pose, optimized_pose); + matching_id.trajectory_id, constant_data->time, local_pose, global_pose); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; // Even if this was the last scan added to 'submap_id', the submap will only @@ -260,7 +260,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid3d constraint_transform = - insertion_submaps[i]->local_pose().inverse() * pose; + insertion_submaps[i]->local_pose().inverse() * local_pose; constraints_.push_back( Constraint{submap_id, node_id, @@ -402,8 +402,9 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, - const mapping::proto::Submap& submap) { +void SparsePoseGraph::AddSubmapFromProto( + const transform::Rigid3d& global_submap_pose, + const mapping::proto::Submap& submap) { if (!submap.has_submap_3d()) { return; } @@ -417,13 +418,13 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, AddTrajectoryIfNeeded(submap_id.trajectory_id); submap_data_.Insert(submap_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; - // Immediately show the submap at the optimized pose. + // Immediately show the submap at the 'global_submap_pose'. optimized_submap_transforms_.Insert( - submap_id, sparse_pose_graph::SubmapData{global_pose}); - AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) { + submap_id, sparse_pose_graph::SubmapData{global_submap_pose}); + AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; - optimization_problem_.InsertSubmap(submap_id, global_pose); + optimization_problem_.InsertSubmap(submap_id, global_submap_pose); }); } @@ -506,7 +507,7 @@ void SparsePoseGraph::LogResidualHistograms() { const cartographer::transform::Rigid3d node_to_submap_constraint = constraint.pose.zbar_ij; const cartographer::transform::Rigid3d optimized_submap_to_map = - optimized_submap_transforms_.at(constraint.submap_id).pose; + optimized_submap_transforms_.at(constraint.submap_id).global_pose; const cartographer::transform::Rigid3d optimized_node_to_submap = optimized_submap_to_map.inverse() * optimized_node_to_map; const cartographer::transform::Rigid3d residual = @@ -659,7 +660,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( } const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. - return submap_transforms.at(last_optimized_submap_id).pose * + return submap_transforms.at(last_optimized_submap_id).global_pose * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -674,7 +675,7 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( auto submap = it->data.submap; if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. - return {submap, optimized_submap_transforms_.at(submap_id).pose}; + return {submap, optimized_submap_transforms_.at(submap_id).global_pose}; } // We have to extrapolate. return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 88b1671..e89fbfa 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -65,11 +65,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; - // Adds a new node with 'constant_data' and a 'pose' that will later be - // optimized. The 'pose' was determined by scan matching against - // 'insertion_submaps.front()' and the scan was inserted into the - // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is - // 'true', this submap was inserted into for the last time. + // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was + // determined by scan matching against 'insertion_submaps.front()' and the + // scan 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 AddScan( std::shared_ptr constant_data, int trajectory_id, @@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void FinishTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override; - void AddSubmapFromProto(const transform::Rigid3d& global_pose, + void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const mapping::proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 5292029..d4d6604 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -102,21 +102,22 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { node_data_.Trim(node_id); } -void OptimizationProblem::AddSubmap(const int trajectory_id, - const transform::Rigid3d& submap_pose) { +void OptimizationProblem::AddSubmap( + const int trajectory_id, const transform::Rigid3d& global_submap_pose) { CHECK_GE(trajectory_id, 0); trajectory_data_.resize(std::max(trajectory_data_.size(), static_cast(trajectory_id) + 1)); - submap_data_.Append(trajectory_id, SubmapData{submap_pose}); + submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); } -void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, - const transform::Rigid3d& submap_pose) { +void OptimizationProblem::InsertSubmap( + const mapping::SubmapId& submap_id, + const transform::Rigid3d& global_submap_pose) { CHECK_GE(submap_id.trajectory_id, 0); trajectory_data_.resize( std::max(trajectory_data_.size(), static_cast(submap_id.trajectory_id) + 1)); - submap_data_.Insert(submap_id, SubmapData{submap_pose}); + submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); } void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { @@ -161,7 +162,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, // gravity alignment. C_submaps.Insert( submap_id_data.id, - CeresPose(submap_id_data.data.pose, translation_parameterization(), + CeresPose(submap_id_data.data.global_pose, + translation_parameterization(), common::make_unique>(), &problem)); @@ -170,7 +172,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, } else { C_submaps.Insert( submap_id_data.id, - CeresPose(submap_id_data.data.pose, translation_parameterization(), + CeresPose(submap_id_data.data.global_pose, + translation_parameterization(), common::make_unique(), &problem)); } @@ -420,7 +423,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, // Store the result. for (const auto& C_submap_id_data : C_submaps) { - submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); + submap_data_.at(C_submap_id_data.id).global_pose = + C_submap_id_data.data.ToRigid(); } for (const auto& C_node_id_data : C_nodes) { node_data_.at(C_node_id_data.id).global_pose = diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 92c0ceb..ddbd087 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -47,7 +47,7 @@ struct NodeData { }; struct SubmapData { - transform::Rigid3d pose; + transform::Rigid3d global_pose; }; // Implements the SPA loop closure method. @@ -79,14 +79,15 @@ class OptimizationProblem { const transform::Rigid3d& local_pose, const transform::Rigid3d& global_pose); void TrimTrajectoryNode(const mapping::NodeId& node_id); - void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); + void AddSubmap(int trajectory_id, + const transform::Rigid3d& global_submap_pose); void InsertSubmap(const mapping::SubmapId& submap_id, - const transform::Rigid3d& submap_pose); + const transform::Rigid3d& global_submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); - // Computes the optimized poses. + // Optimizes the global poses. void Solve(const std::vector& constraints, const std::set& frozen_trajectories);