Remove unused 'imu_gravity_variance' option. (#363)

cartographer/kalman_filter/pose_tracker.cc

@@ -111,17 +111,6 @@ PoseTracker::State ModelFunction(const PoseTracker::State& state,
 
 }  // namespace
 
-PoseAndCovariance operator*(const transform::Rigid3d& transform,
-                            const PoseAndCovariance& pose_and_covariance) {
-  GaussianDistribution<double, 6> distribution(
-      Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);
-  Eigen::Matrix<double, 6, 6> linear_transform;
-  linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
-      Eigen::Matrix3d::Zero(), transform.rotation().matrix();
-  return {transform * pose_and_covariance.pose,
-          (linear_transform * distribution).GetCovariance()};
-}
-
 proto::PoseTrackerOptions CreatePoseTrackerOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::PoseTrackerOptions options;
@@ -133,8 +122,6 @@ proto::PoseTrackerOptions CreatePoseTrackerOptions(
       parameter_dictionary->GetDouble("velocity_model_variance"));
   options.set_imu_gravity_time_constant(
       parameter_dictionary->GetDouble("imu_gravity_time_constant"));
-  options.set_imu_gravity_variance(
-      parameter_dictionary->GetDouble("imu_gravity_variance"));
   options.set_num_odometry_states(
       parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
   CHECK_GT(options.num_odometry_states(), 0);
@@ -165,20 +152,8 @@ PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
   return kalman_filter_.GetBelief();
 }
 
-void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
+transform::Rigid3d PoseTracker::GetPoseEstimateMean(const common::Time time) {
-                                                   transform::Rigid3d* pose,
+  return RigidFromState(GetBelief(time).GetMean());
-                                                   PoseCovariance* covariance) {
-  const Distribution belief = GetBelief(time);
-  *pose = RigidFromState(belief.GetMean());
-  static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
-  static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
-  static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
-  static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
-  static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
-  static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
-  *covariance = belief.GetCovariance().block<6, 6>(0, 0);
-  covariance->block<2, 2>(3, 3) +=
-      options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
 }
 
 const PoseTracker::Distribution PoseTracker::BuildModelNoise(

cartographer/kalman_filter/pose_tracker.h

@@ -36,17 +36,8 @@
 namespace cartographer {
 namespace kalman_filter {
 
-typedef Eigen::Matrix3d Pose2DCovariance;
 typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
 
-struct PoseAndCovariance {
-  transform::Rigid3d pose;
-  PoseCovariance covariance;
-};
-
-PoseAndCovariance operator*(const transform::Rigid3d& transform,
-                            const PoseAndCovariance& pose_and_covariance);
-
 PoseCovariance BuildPoseCovariance(double translational_variance,
                                    double rotational_variance);
 
@@ -80,11 +71,9 @@ class PoseTracker {
   PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
   virtual ~PoseTracker();
 
-  // Sets 'pose' and 'covariance' to the mean and covariance of the belief at
+  // Returns the pose of the mean of the belief at 'time' which must be >= to
-  // 'time' which must be >= to the current time. Must not be nullptr.
+  // the current time.
-  void GetPoseEstimateMeanAndCovariance(common::Time time,
+  transform::Rigid3d GetPoseEstimateMean(common::Time time);
-                                        transform::Rigid3d* pose,
-                                        PoseCovariance* covariance);
 
   // Updates from an IMU reading (in the IMU frame).
   void AddImuLinearAccelerationObservation(

cartographer/kalman_filter/pose_tracker_test.cc

@@ -44,7 +44,6 @@ class PoseTrackerTest : public ::testing::Test {
             position_model_variance = 1e-8,
             velocity_model_variance = 1e-8,
             imu_gravity_time_constant = 100.,
-            imu_gravity_variance = 1e-9,
             num_odometry_states = 1,
         }
         )text");
@@ -59,9 +58,7 @@ class PoseTrackerTest : public ::testing::Test {
 
 TEST_F(PoseTrackerTest, SaveAndRestore) {
   std::vector<Rigid3d> poses(3);
-  std::vector<PoseCovariance> covariances(3);
+  poses[0] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(1500));
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
-                                                  &poses[0], &covariances[0]);
 
   pose_tracker_->AddImuLinearAccelerationObservation(
       common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
@@ -72,18 +69,15 @@ TEST_F(PoseTrackerTest, SaveAndRestore) {
   pose_tracker_->AddImuLinearAccelerationObservation(
       common::FromUniversal(3000), observation);
 
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
+  poses[1] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(3500));
-                                                  &poses[1], &covariances[1]);
 
   copy_of_pose_tracker.AddImuLinearAccelerationObservation(
       common::FromUniversal(3000), observation);
-  copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
+  poses[2] =
-      common::FromUniversal(3500), &poses[2], &covariances[2]);
+      copy_of_pose_tracker.GetPoseEstimateMean(common::FromUniversal(3500));
 
   EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
-  EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
   EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
-  EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
 }
 
 TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
@@ -96,9 +90,7 @@ TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
   }
 
   {
-    Rigid3d pose;
+    const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
-    PoseCovariance covariance;
-    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
     const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
     const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
     EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
@@ -113,9 +105,7 @@ TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
 
   time += std::chrono::milliseconds(5);
 
-  Rigid3d pose;
+  const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
-  PoseCovariance covariance;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
   const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
   const Eigen::Quaterniond expected = Eigen::Quaterniond(
       Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
@@ -133,9 +123,7 @@ TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
   }
 
   {
-    Rigid3d pose;
+    const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
-    PoseCovariance covariance;
-    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
     const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
     const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
     EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
@@ -153,9 +141,7 @@ TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
 
   time += std::chrono::milliseconds(5);
 
-  Rigid3d pose;
+  const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
-  PoseCovariance covariance;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
   const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
   const Eigen::Quaterniond expected = Eigen::Quaterniond(
       Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
@@ -174,9 +160,7 @@ TEST_F(PoseTrackerTest, AddPoseObservation) {
   }
 
   {
-    Rigid3d actual;
+    const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
-    PoseCovariance covariance;
-    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
     EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
   }
 
@@ -193,9 +177,7 @@ TEST_F(PoseTrackerTest, AddPoseObservation) {
 
   time += std::chrono::milliseconds(15);
 
-  Rigid3d actual;
+  const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
-  PoseCovariance covariance;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
   EXPECT_THAT(actual, IsNearly(expected, 1e-3));
 }
 
@@ -221,13 +203,11 @@ TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
   odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
 
   Rigid3d actual;
-  PoseCovariance unused_covariance;
   for (const Rigid3d& pose : odometer_track) {
     time += std::chrono::seconds(1);
     pose_tracker_->AddOdometerPoseObservation(
         time, pose, kOdometerVariance * PoseCovariance::Identity());
-    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
+    actual = pose_tracker_->GetPoseEstimateMean(time);
-                                                    &unused_covariance);
     EXPECT_THAT(actual, IsNearly(pose, 1e-2));
   }
   // Sanity check that the test has signal:

cartographer/kalman_filter/proto/pose_tracker_options.proto

@@ -25,7 +25,6 @@ message PoseTrackerOptions {
   // Time constant for the orientation moving average based on observed gravity
   // via linear acceleration.
   optional double imu_gravity_time_constant = 4;
-  optional double imu_gravity_variance = 5;
 
   // Maximum number of previous odometry states to keep.
   optional int32 num_odometry_states = 6;

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -58,11 +58,6 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
 
   pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
   pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
-
-  transform::Rigid3d pose_estimate;
-  kalman_filter::PoseCovariance unused_covariance_estimate;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate,
-                                                  &unused_covariance_estimate);
 }
 
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
@@ -74,10 +69,8 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
     return nullptr;
   }
 
-  transform::Rigid3d pose_prediction;
+  const transform::Rigid3d pose_prediction =
-  kalman_filter::PoseCovariance unused_covariance_prediction;
+      pose_tracker_->GetPoseEstimateMean(time);
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(
-      time, &pose_prediction, &unused_covariance_prediction);
   if (num_accumulated_ == 0) {
     first_pose_prediction_ = pose_prediction.cast<float>();
     accumulated_range_data_ =
@@ -131,10 +124,8 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
     return nullptr;
   }
 
-  transform::Rigid3d pose_prediction;
+  const transform::Rigid3d pose_prediction =
-  kalman_filter::PoseCovariance unused_covariance_prediction;
+      pose_tracker_->GetPoseEstimateMean(time);
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(
-      time, &pose_prediction, &unused_covariance_prediction);
 
   std::shared_ptr<const Submap> matching_submap =
       active_submaps_.submaps().front();
@@ -174,9 +165,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
               .scan_matcher_variance() *
           kalman_filter::PoseCovariance::Identity());
 
-  kalman_filter::PoseCovariance unused_covariance_estimate;
+  scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time);
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(
-      time, &scan_matcher_pose_estimate_, &unused_covariance_estimate);
 
   last_pose_estimate_ = {
       time, scan_matcher_pose_estimate_,

cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc

@@ -107,7 +107,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
               position_model_variance = 0.000654766,
               velocity_model_variance = 0.053926,
               imu_gravity_time_constant = 1.,
-              imu_gravity_variance = 1e-4,
               num_odometry_states = 1,
             },
 

configuration_files/trajectory_builder_3d.lua

@@ -71,7 +71,6 @@ TRAJECTORY_BUILDER_3D = {
       velocity_model_variance = 0.53926,
       -- This disables gravity alignment in local SLAM.
       imu_gravity_time_constant = 1e9,
-      imu_gravity_variance = 0,
       num_odometry_states = 1,
     },
 

docs/source/configuration.rst

@@ -49,9 +49,6 @@ double imu_gravity_time_constant
   Time constant for the orientation moving average based on observed gravity
   via linear acceleration.
 
-double imu_gravity_variance
-  Not yet documented.
-
 int32 num_odometry_states
   Maximum number of previous odometry states to keep.