diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 7313bd5..36648d3 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -253,12 +253,13 @@ void Submap3D::ToResponseProto( response->add_textures()); } -void Submap3D::InsertRangeData(const sensor::RangeData& range_data, +void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local, const RangeDataInserter3D& range_data_inserter, const float high_resolution_max_range) { CHECK(!insertion_finished()); + // Transform range data into submap frame. const sensor::RangeData transformed_range_data = sensor::TransformRangeData( - range_data, local_pose().inverse().cast()); + range_data_in_local, local_pose().inverse().cast()); range_data_inserter.Insert( FilterRangeDataByMaxRange(transformed_range_data, high_resolution_max_range), @@ -284,11 +285,11 @@ std::vector> ActiveSubmaps3D::submaps() const { std::vector> ActiveSubmaps3D::InsertRangeData( const sensor::RangeData& range_data, - const Eigen::Quaterniond& gravity_alignment) { + const Eigen::Quaterniond& local_from_gravity_aligned) { if (submaps_.empty() || submaps_.back()->num_range_data() == options_.num_range_data()) { AddSubmap(transform::Rigid3d(range_data.origin.cast(), - gravity_alignment)); + local_from_gravity_aligned)); } for (auto& submap : submaps_) { submap->InsertRangeData(range_data, range_data_inserter_, diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index 39f0bf9..c8a4a7c 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -90,12 +90,12 @@ class ActiveSubmaps3D { ActiveSubmaps3D(const ActiveSubmaps3D&) = delete; ActiveSubmaps3D& operator=(const ActiveSubmaps3D&) = delete; - // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is - // used for the orientation of new submaps so that the z axis approximately - // aligns with gravity. + // Inserts 'range_data_in_local' into the Submap collection. + // 'local_from_gravity_aligned' is used for the orientation of new submaps so + // that the z axis approximately aligns with gravity. std::vector> InsertRangeData( - const sensor::RangeData& range_data, - const Eigen::Quaterniond& gravity_alignment); + const sensor::RangeData& range_data_in_local, + const Eigen::Quaterniond& local_from_gravity_aligned); std::vector> submaps() const; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 240cd4a..ced8659 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -352,9 +352,13 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap( if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; } + const auto local_from_gravity_aligned = + pose_estimate.rotation() * gravity_alignment.inverse(); + std::vector> insertion_submaps = active_submaps_.InsertRangeData(filtered_range_data_in_local, - gravity_alignment); + local_from_gravity_aligned); + const Eigen::VectorXf rotational_scan_matcher_histogram = scan_matching::RotationalScanMatcher::ComputeHistogram( sensor::TransformPointCloud( diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index b1eb652..0b14c12 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -54,7 +54,8 @@ class PoseExtrapolator { void AddOdometryData(const sensor::OdometryData& odometry_data); transform::Rigid3d ExtrapolatePose(common::Time time); - // Gravity alignment estimate. + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. Eigen::Quaterniond EstimateGravityOrientation(common::Time time); private: