diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index c6b9c4c..275cef2 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -53,18 +53,19 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { return value; } -// An individual submap, which has a 'local_pose' in the local SLAM frame, keeps +// An individual submap, which has a 'local_pose' in the local map frame, keeps // track of how many range data were inserted into it, and sets the // 'finished_probability_grid' to be used for loop closing once the map no // longer changes. class Submap { public: - Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {} + Submap(const transform::Rigid3d& local_submap_pose) + : local_pose_(local_submap_pose) {} virtual ~Submap() {} virtual void ToProto(proto::Submap* proto) const = 0; - // Local SLAM pose of this submap. + // Pose of this submap in the local map frame. transform::Rigid3d local_pose() const { return local_pose_; } // Number of RangeData inserted. diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 85965f1..afa9e22 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -89,8 +89,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, } if (num_accumulated_ == 0) { first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast(); - accumulated_range_data_ = - sensor::RangeData{range_data.origin, {}, {}}; + accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; } // TODO(gaschler): Take time delta of individual points into account. @@ -147,7 +146,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const transform::Rigid2d pose_prediction = transform::Project2D( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); - transform::Rigid2d pose_estimate_2d; // local frame <- gravity-aligned frame + // local map frame <- gravity-aligned frame + transform::Rigid2d pose_estimate_2d; ScanMatch(time, pose_prediction, gravity_aligned_range_data, &pose_estimate_2d); const transform::Rigid3d pose_estimate = diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2368318..8b2f8ec 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -158,8 +158,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // optimization being run at a time. void RunOptimization() EXCLUDES(mutex_); - // Computes the local to global frame transform based on the given optimized - // 'submap_transforms'. + // Computes the local to global map frame transform based on the given + // optimized 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( const mapping::MapById& submap_transforms, diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 3582318..edc5594 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -69,8 +69,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, } if (num_accumulated_ == 0) { first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast(); - accumulated_range_data_ = - sensor::RangeData{range_data.origin, {}, {}}; + accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; } // TODO(gaschler): Take time delta of individual points into account. diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 039b4fe..19457cf 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -158,8 +158,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // optimization being run at a time. void RunOptimization() EXCLUDES(mutex_); - // Computes the local to global frame transform based on the given optimized - // 'submap_transforms'. + // Computes the local to global map frame transform based on the given + // optimized 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( const mapping::MapById& submap_transforms, diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 90df263..8437d57 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -198,8 +198,8 @@ proto::SubmapsOptions CreateSubmapsOptions( } Submap::Submap(const float high_resolution, const float low_resolution, - const transform::Rigid3d& local_pose) - : mapping::Submap(local_pose), + const transform::Rigid3d& local_submap_pose) + : mapping::Submap(local_submap_pose), high_resolution_hybrid_grid_(high_resolution), low_resolution_hybrid_grid_(low_resolution) {} @@ -283,14 +283,15 @@ void ActiveSubmaps::InsertRangeData( } } -void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) { +void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_submap_pose) { if (submaps_.size() > 1) { submaps_.front()->Finish(); ++matching_submap_index_; submaps_.erase(submaps_.begin()); } submaps_.emplace_back(new Submap(options_.high_resolution(), - options_.low_resolution(), local_pose)); + options_.low_resolution(), + local_submap_pose)); LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index a18b1ec..b9ed50d 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -44,7 +44,7 @@ proto::SubmapsOptions CreateSubmapsOptions( class Submap : public mapping::Submap { public: Submap(float high_resolution, float low_resolution, - const transform::Rigid3d& local_pose); + const transform::Rigid3d& local_submap_pose); explicit Submap(const mapping::proto::Submap3D& proto); void ToProto(mapping::proto::Submap* proto) const override; @@ -103,7 +103,7 @@ class ActiveSubmaps { std::vector> submaps() const; private: - void AddSubmap(const transform::Rigid3d& local_pose); + void AddSubmap(const transform::Rigid3d& local_submap_pose); const proto::SubmapsOptions options_; int matching_submap_index_ = 0; diff --git a/docs/source/terminology.rst b/docs/source/terminology.rst index f2e3ba6..575266f 100644 --- a/docs/source/terminology.rst +++ b/docs/source/terminology.rst @@ -21,7 +21,7 @@ This documents a few common patterns that exist in the Cartographer codebase. Frames ====== -global (map) frame +global map frame This is the frame in which global SLAM results are expressed. It is the fixed map frame including all loop closure and optimization results. The transform between this frame and any other frame can jump when new optimization results @@ -29,14 +29,27 @@ global (map) frame vector points in the -z direction, i.e. the gravitational component measured by an accelerometer is in the +z direction. -local (map) frame +local map frame This is the frame in which local SLAM results are expressed. It is the fixed map frame excluding loop closures and the pose graph optimization. For a given point in time, the transform between this and the global map frame may change, but the transform between this and all other frames does not change. +submap frame + Each submap has a separate fixed frame. + tracking frame - The frame in which sensor data is expressed. + The frame in which sensor data is expressed. It is not fixed, i.e. it changes + over time. It is also different for different trajectories. + +gravity-aligned frame + Only used in 2D. A frame colocated with the tracking frame but with a + different orientation that is approximately aligned with gravity, i.e. the + gravitational acceleration vector points approximately in the -z direction. No + assumption about yaw (rotation around the z axis between this and the tracking + frame) should be made. A different gravity-aligned frame is used for different + trajectory nodes, e.g. yaw can change arbitrarily between gravity-aligned + frames of consecutive nodes. @@ -44,7 +57,15 @@ Transforms ========== local_pose - Transforms data from the tracking frame to the local frame. + Transforms data from the tracking frame (or a submap frame, depending on + context) to the local map frame. global_pose - Transforms data from the tracking frame to the global frame. + Transforms data from the tracking frame (or a submap frame, depending on + context) to the global map frame. + +local_submap_pose + Transforms data from a submap frame to the local map frame. + +global_submap_pose + Transforms data from a submap frame to the global map frame.