diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 769d8ac..42fa9ab 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -44,8 +44,7 @@ class GlobalTrajectoryBuilderInterface { // 3. UKF estimate after integrating any measurements // // Finally, 'pose' is the end-user visualization of orientation and - // 'point_cloud' is the point cloud, in the odometry frame, used to make - // this estimate. + // 'point_cloud' is the point cloud, in the local map frame. struct PoseEstimate { PoseEstimate() = default; PoseEstimate(common::Time time, diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index c429ee9..5740d61 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -106,9 +106,10 @@ class SparsePoseGraph { virtual std::vector GetSubmapTransforms( const Submaps& submaps) = 0; - // Returns the transform converting from odometry based transforms to the - // optimized world. - virtual transform::Rigid3d GetOdometryToMapTransform( + // Returns the transform converting data in the local map frame (i.e. the + // continuous, non-loop-closed frame) into the global map frame (i.e. the + // discontinuous, loop-closed frame). + virtual transform::Rigid3d GetLocalToGlobalTransform( const mapping::Submaps& submaps) = 0; // Returns the current optimized trajectory. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index f599ede..f47847e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -90,7 +90,7 @@ void SparsePoseGraph::AddScan( const mapping::Submaps* submaps, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { - const transform::Rigid3d optimized_pose(GetOdometryToMapTransform(*submaps) * + const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); @@ -393,7 +393,7 @@ std::vector SparsePoseGraph::constraints_3d() { return {}; } -transform::Rigid3d SparsePoseGraph::GetOdometryToMapTransform( +transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps& submaps) { return GetSubmapTransforms(submaps).back() * submaps.Get(submaps.size() - 1)->local_pose().inverse(); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2ef8bda..84a1c16 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { override; std::vector GetSubmapTransforms( const mapping::Submaps& submaps) EXCLUDES(mutex_) override; - transform::Rigid3d GetOdometryToMapTransform(const mapping::Submaps& submaps) + transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps) EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 854e46b..63a9e55 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -88,7 +88,7 @@ int SparsePoseGraph::AddScan( const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, const Submap* const matching_submap, const std::vector& insertion_submaps) { - const transform::Rigid3d optimized_pose(GetOdometryToMapTransform(*submaps) * + const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) * pose); common::MutexLocker locker(&mutex_); @@ -401,7 +401,7 @@ std::vector SparsePoseGraph::constraints_3d() { return constraints_; } -transform::Rigid3d SparsePoseGraph::GetOdometryToMapTransform( +transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps& submaps) { return GetSubmapTransforms(submaps).back() * submaps.Get(submaps.size() - 1)->local_pose().inverse(); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index e4c8da6..3e2b3bb 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -89,7 +89,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { override; std::vector GetSubmapTransforms( const mapping::Submaps& submaps) EXCLUDES(mutex_) override; - transform::Rigid3d GetOdometryToMapTransform(const mapping::Submaps& submaps) + transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps) EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_);