Tiny improvement of the gravity estimation. (#494)

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.

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 {
+  Eigen::Quaterniond EstimateGravityOrientation(common::Time time);
-    return imu_tracker_->orientation();
-  }
 
  private:
   void UpdateVelocitiesFromPoses();

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<float>())};
 
-  return InsertIntoSubmap(
+  return InsertIntoSubmap(time, filtered_range_data, gravity_alignment,
-      time, filtered_range_data, filtered_point_cloud_in_tracking,
+                          filtered_point_cloud_in_tracking,
-      low_resolution_point_cloud_in_tracking, pose_estimate);
+                          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::InsertionResult>
 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<float>()),
-      extrapolator_->gravity_orientation());
+      gravity_alignment);
   return std::unique_ptr<InsertionResult>(new InsertionResult{
 
       std::make_shared<const mapping::TrajectoryNode::Data>(

cartographer/mapping_3d/local_trajectory_builder.h

@@ -65,6 +65,7 @@ class LocalTrajectoryBuilder {
 
   std::unique_ptr<InsertionResult> 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);

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -275,9 +275,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
   }
   const float kSafetyMargin = 1.f - 1e-2f;
   const float angular_step_size =
-      kSafetyMargin * std::acos(1.f -
+      kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
-                                common::Pow2(resolution_) /
+                                          (2.f * common::Pow2(max_scan_range)));
-                                    (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

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<OptimizationProblem::Constraint>&
-             constraint : constraints_) {
+                 constraint : constraints_) {
           if (constraint != nullptr) {
             result.push_back(*constraint);
           }

cartographer/transform/rigid_transform.h

@@ -35,8 +35,7 @@ class Rigid2 {
   using Vector = Eigen::Matrix<FloatType, 2, 1>;
   using Rotation2D = Eigen::Rotation2D<FloatType>;
 
-  Rigid2()
+  Rigid2() : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {}
-      : 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<FloatType> Identity() {
+  static Rigid2<FloatType> Identity() { return Rigid2<FloatType>(); }
-    return Rigid2<FloatType>();
-  }
 
   template <typename OtherType>
   Rigid2<OtherType> cast() const {
@@ -128,8 +125,7 @@ class Rigid3 {
   using Quaternion = Eigen::Quaternion<FloatType>;
   using AngleAxis = Eigen::AngleAxis<FloatType>;
 
-  Rigid3()
+  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
-      : 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<FloatType> Identity() {
+  static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }
-    return Rigid3<FloatType>();
-  }
 
   template <typename OtherType>
   Rigid3<OtherType> cast() const {