From 5673334f0e9e9c5a9d306eb6e7fc1112e1de6283 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 1 Sep 2017 10:22:13 +0200 Subject: [PATCH] Tiny improvement of the gravity estimation. (#494) --- cartographer/mapping/pose_extrapolator.cc | 7 +++++++ cartographer/mapping/pose_extrapolator.h | 4 +--- .../mapping_3d/local_trajectory_builder.cc | 12 ++++++++---- cartographer/mapping_3d/local_trajectory_builder.h | 1 + .../scan_matching/fast_correlative_scan_matcher.cc | 5 ++--- .../sparse_pose_graph/constraint_builder.cc | 2 +- cartographer/transform/rigid_transform.h | 14 ++++---------- 7 files changed, 24 insertions(+), 21 deletions(-) diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index b0e034f..05925de 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -130,6 +130,13 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) { transform::Rigid3d::Rotation(ExtrapolateRotation(time)); } +Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation( + const common::Time time) { + ImuTracker imu_tracker = *imu_tracker_; + AdvanceImuTracker(time, &imu_tracker); + return imu_tracker.orientation(); +} + void PoseExtrapolator::UpdateVelocitiesFromPoses() { if (timed_pose_queue_.size() < 2) { // We need two poses to estimate velocities. diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 19a282a..66976e1 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -54,9 +54,7 @@ class PoseExtrapolator { transform::Rigid3d ExtrapolatePose(common::Time time); // Gravity alignment estimate. - Eigen::Quaterniond gravity_orientation() const { - return imu_tracker_->orientation(); - } + Eigen::Quaterniond EstimateGravityOrientation(common::Time time); private: void UpdateVelocitiesFromPoses(); diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index e43aeee..bb9540b 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -156,15 +156,18 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const transform::Rigid3d pose_estimate = matching_submap->local_pose() * pose_observation_in_submap; extrapolator_->AddPose(time, pose_estimate); + const Eigen::Quaterniond gravity_alignment = + extrapolator_->EstimateGravityOrientation(time); last_pose_estimate_ = { time, pose_estimate, sensor::TransformPointCloud(filtered_range_data.returns, pose_estimate.cast())}; - return InsertIntoSubmap( - time, filtered_range_data, filtered_point_cloud_in_tracking, - low_resolution_point_cloud_in_tracking, pose_estimate); + return InsertIntoSubmap(time, filtered_range_data, gravity_alignment, + filtered_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, + pose_estimate); } void LocalTrajectoryBuilder::AddOdometerData( @@ -184,6 +187,7 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { std::unique_ptr LocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, + const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& high_resolution_point_cloud, const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose_observation) { @@ -199,7 +203,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap( active_submaps_.InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), - extrapolator_->gravity_orientation()); + gravity_alignment); return std::unique_ptr(new InsertionResult{ std::make_shared( diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 66f3060..f78115a 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -65,6 +65,7 @@ class LocalTrajectoryBuilder { std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& range_data_in_tracking, + const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& high_resolution_point_cloud, const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose_observation); diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 2e10fa9..8c01996 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -275,9 +275,8 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( } const float kSafetyMargin = 1.f - 1e-2f; const float angular_step_size = - kSafetyMargin * std::acos(1.f - - common::Pow2(resolution_) / - (2.f * common::Pow2(max_scan_range))); + kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) / + (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt( search_parameters.angular_search_window / angular_step_size); // TODO(whess): Should there be a small search window for rotations around diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 2043724..9502ad1 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -266,7 +266,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { for (const std::unique_ptr& - constraint : constraints_) { + constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); } diff --git a/cartographer/transform/rigid_transform.h b/cartographer/transform/rigid_transform.h index f14d4ea..71e1ad5 100644 --- a/cartographer/transform/rigid_transform.h +++ b/cartographer/transform/rigid_transform.h @@ -35,8 +35,7 @@ class Rigid2 { using Vector = Eigen::Matrix; using Rotation2D = Eigen::Rotation2D; - Rigid2() - : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {} + Rigid2() : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {} Rigid2(const Vector& translation, const Rotation2D& rotation) : translation_(translation), rotation_(rotation) {} Rigid2(const Vector& translation, const double rotation) @@ -54,9 +53,7 @@ class Rigid2 { return Rigid2(vector, Rotation2D::Identity()); } - static Rigid2 Identity() { - return Rigid2(); - } + static Rigid2 Identity() { return Rigid2(); } template Rigid2 cast() const { @@ -128,8 +125,7 @@ class Rigid3 { using Quaternion = Eigen::Quaternion; using AngleAxis = Eigen::AngleAxis; - Rigid3() - : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {} + Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {} Rigid3(const Vector& translation, const Quaternion& rotation) : translation_(translation), rotation_(rotation) {} Rigid3(const Vector& translation, const AngleAxis& rotation) @@ -147,9 +143,7 @@ class Rigid3 { return Rigid3(vector, Quaternion::Identity()); } - static Rigid3 Identity() { - return Rigid3(); - } + static Rigid3 Identity() { return Rigid3(); } template Rigid3 cast() const {