From 2df8bcde615d5952847184c726f919d3b12869f2 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 26 Jun 2017 13:23:58 +0200 Subject: [PATCH] Remove unused 'imu_gravity_variance' option. (#363) --- cartographer/kalman_filter/pose_tracker.cc | 29 +------------ cartographer/kalman_filter/pose_tracker.h | 17 ++------ .../kalman_filter/pose_tracker_test.cc | 42 +++++-------------- .../proto/pose_tracker_options.proto | 1 - .../kalman_local_trajectory_builder.cc | 21 +++------- .../kalman_local_trajectory_builder_test.cc | 1 - configuration_files/trajectory_builder_3d.lua | 1 - docs/source/configuration.rst | 3 -- 8 files changed, 21 insertions(+), 94 deletions(-) diff --git a/cartographer/kalman_filter/pose_tracker.cc b/cartographer/kalman_filter/pose_tracker.cc index be3154e..caba4fe 100644 --- a/cartographer/kalman_filter/pose_tracker.cc +++ b/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 distribution( - Eigen::Matrix::Zero(), pose_and_covariance.covariance); - Eigen::Matrix 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* pose, - 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(); +transform::Rigid3d PoseTracker::GetPoseEstimateMean(const common::Time time) { + return RigidFromState(GetBelief(time).GetMean()); } const PoseTracker::Distribution PoseTracker::BuildModelNoise( diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h index 84db576..cc5d465 100644 --- a/cartographer/kalman_filter/pose_tracker.h +++ b/cartographer/kalman_filter/pose_tracker.h @@ -36,17 +36,8 @@ namespace cartographer { namespace kalman_filter { -typedef Eigen::Matrix3d Pose2DCovariance; typedef Eigen::Matrix 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 - // 'time' which must be >= to the current time. Must not be nullptr. - void GetPoseEstimateMeanAndCovariance(common::Time time, - transform::Rigid3d* pose, - PoseCovariance* covariance); + // Returns the pose of the mean of the belief at 'time' which must be >= to + // the current time. + transform::Rigid3d GetPoseEstimateMean(common::Time time); // Updates from an IMU reading (in the IMU frame). void AddImuLinearAccelerationObservation( diff --git a/cartographer/kalman_filter/pose_tracker_test.cc b/cartographer/kalman_filter/pose_tracker_test.cc index dcf47d3..290a028 100644 --- a/cartographer/kalman_filter/pose_tracker_test.cc +++ b/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 poses(3); - std::vector covariances(3); - pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500), - &poses[0], &covariances[0]); + poses[0] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(1500)); 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], &covariances[1]); + poses[1] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(3500)); copy_of_pose_tracker.AddImuLinearAccelerationObservation( common::FromUniversal(3000), observation); - copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance( - common::FromUniversal(3500), &poses[2], &covariances[2]); + poses[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; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); + const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time); 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; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); + const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time); 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; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); + const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time); 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; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); + const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time); 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; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance); + const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time); EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3)); } @@ -193,9 +177,7 @@ TEST_F(PoseTrackerTest, AddPoseObservation) { time += std::chrono::milliseconds(15); - Rigid3d actual; - PoseCovariance covariance; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance); + const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time); 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, - &unused_covariance); + actual = pose_tracker_->GetPoseEstimateMean(time); EXPECT_THAT(actual, IsNearly(pose, 1e-2)); } // Sanity check that the test has signal: diff --git a/cartographer/kalman_filter/proto/pose_tracker_options.proto b/cartographer/kalman_filter/proto/pose_tracker_options.proto index d34620c..6b6fa87 100644 --- a/cartographer/kalman_filter/proto/pose_tracker_options.proto +++ b/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; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index caf9f49..6d03b7b 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/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 @@ -74,10 +69,8 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData( return nullptr; } - transform::Rigid3d pose_prediction; - kalman_filter::PoseCovariance unused_covariance_prediction; - pose_tracker_->GetPoseEstimateMeanAndCovariance( - time, &pose_prediction, &unused_covariance_prediction); + const transform::Rigid3d pose_prediction = + pose_tracker_->GetPoseEstimateMean(time); if (num_accumulated_ == 0) { first_pose_prediction_ = pose_prediction.cast(); accumulated_range_data_ = @@ -131,10 +124,8 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( return nullptr; } - transform::Rigid3d pose_prediction; - kalman_filter::PoseCovariance unused_covariance_prediction; - pose_tracker_->GetPoseEstimateMeanAndCovariance( - time, &pose_prediction, &unused_covariance_prediction); + const transform::Rigid3d pose_prediction = + pose_tracker_->GetPoseEstimateMean(time); std::shared_ptr 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; - pose_tracker_->GetPoseEstimateMeanAndCovariance( - time, &scan_matcher_pose_estimate_, &unused_covariance_estimate); + scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time); last_pose_estimate_ = { time, scan_matcher_pose_estimate_, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 08dd978..2b321b4 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/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, }, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index bf9915c..2a59c8b 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/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, }, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 1d2c1f3..60bdc9a 100644 --- a/docs/source/configuration.rst +++ b/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.