From a39bbff70cf1aa5f1ecd568f0d54112bb1c4b535 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Mon, 21 Nov 2016 12:34:52 +0100 Subject: [PATCH] Removes odometer covariance from public API. (#137) --- cartographer/kalman_filter/pose_tracker.cc | 15 +++++++++++++++ cartographer/kalman_filter/pose_tracker.h | 3 +++ .../mapping/collated_trajectory_builder.cc | 4 ++-- .../global_trajectory_builder_interface.h | 5 ++--- cartographer/mapping/trajectory_builder.h | 6 ++---- .../mapping_2d/global_trajectory_builder.cc | 7 +++---- .../mapping_2d/global_trajectory_builder.h | 5 ++--- .../mapping_2d/local_trajectory_builder.cc | 16 +++++++++++----- .../mapping_2d/local_trajectory_builder.h | 3 +-- .../proto/local_trajectory_builder_options.proto | 3 +++ .../mapping_3d/global_trajectory_builder.cc | 7 +++---- .../mapping_3d/global_trajectory_builder.h | 5 ++--- .../kalman_local_trajectory_builder.cc | 10 +++++++--- .../mapping_3d/kalman_local_trajectory_builder.h | 6 ++---- .../kalman_local_trajectory_builder_options.cc | 4 ++++ .../kalman_local_trajectory_builder_test.cc | 3 +++ .../local_trajectory_builder_interface.h | 5 ++--- .../optimizing_local_trajectory_builder.cc | 3 +-- .../optimizing_local_trajectory_builder.h | 6 ++---- ...kalman_local_trajectory_builder_options.proto | 3 +++ cartographer/sensor/collator_test.cc | 12 ++++++------ cartographer/sensor/data.h | 13 +++---------- configuration_files/trajectory_builder_2d.lua | 2 ++ configuration_files/trajectory_builder_3d.lua | 3 +++ 24 files changed, 87 insertions(+), 62 deletions(-) diff --git a/cartographer/kalman_filter/pose_tracker.cc b/cartographer/kalman_filter/pose_tracker.cc index 40381db..427af57 100644 --- a/cartographer/kalman_filter/pose_tracker.cc +++ b/cartographer/kalman_filter/pose_tracker.cc @@ -356,5 +356,20 @@ PoseCovariance PoseCovarianceFromProtoMatrix( return covariance; } +PoseCovariance BuildPoseCovariance(const double translational_variance, + const double rotational_variance) { + const Eigen::Matrix3d translational = + Eigen::Matrix3d::Identity() * translational_variance; + const Eigen::Matrix3d rotational = + Eigen::Matrix3d::Identity() * rotational_variance; + // clang-format off + PoseCovariance covariance; + covariance << + translational, Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), rotational; + // clang-format on + return covariance; +} + } // namespace kalman_filter } // namespace cartographer diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h index 9fc958e..96cef6a 100644 --- a/cartographer/kalman_filter/pose_tracker.h +++ b/cartographer/kalman_filter/pose_tracker.h @@ -54,6 +54,9 @@ Pose2DCovariance Project2D(const PoseCovariance& embedded_covariance); PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance, double position_variance, double orientation_variance); +PoseCovariance BuildPoseCovariance(double translational_variance, + double rotational_variance); + // Deserializes the 'proto_matrix' into a PoseCovariance. PoseCovariance PoseCovarianceFromProtoMatrix( const sensor::proto::Matrix& proto_matrix); diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index c621075..0dfbcb4 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -94,8 +94,8 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( return; case sensor::Data::Type::kOdometer: - wrapped_trajectory_builder_->AddOdometerData( - data->time, data->odometer.pose, data->odometer.covariance); + wrapped_trajectory_builder_->AddOdometerData(data->time, + data->odometer_pose); return; } LOG(FATAL); diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 086bba2..467bfb9 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -56,9 +56,8 @@ class GlobalTrajectoryBuilderInterface { virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; - virtual void AddOdometerData( - common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) = 0; + virtual void AddOdometerData(common::Time time, + const transform::Rigid3d& pose) = 0; }; } // namespace mapping diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 92b2036..311047a 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -100,11 +100,9 @@ class TrajectoryBuilder { } void AddOdometerData(const string& sensor_id, common::Time time, - const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { + const transform::Rigid3d& odometer_pose) { AddSensorData(sensor_id, - common::make_unique( - time, sensor::Data::Odometer{pose, covariance})); + common::make_unique(time, odometer_pose)); } }; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index f7d3b8d..22c4882 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -56,10 +56,9 @@ void GlobalTrajectoryBuilder::AddImuData( angular_velocity); } -void GlobalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { - local_trajectory_builder_.AddOdometerData(time, pose, covariance); +void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, + const transform::Rigid3d& pose) { + local_trajectory_builder_.AddOdometerData(time, pose); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index cfdb3f5..dcd83ad 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -44,9 +44,8 @@ class GlobalTrajectoryBuilder const sensor::PointCloud& ranges) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - void AddOdometerData( - common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) override; + void AddOdometerData(common::Time time, + const transform::Rigid3d& pose) override; private: const proto::LocalTrajectoryBuilderOptions options_; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index c697cc9..dc54329 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -59,6 +59,10 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( *options.mutable_submaps_options() = CreateSubmapsOptions( parameter_dictionary->GetDictionary("submaps").get()); options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); + options.set_odometer_translational_variance( + parameter_dictionary->GetDouble("odometer_translational_variance")); + options.set_odometer_rotational_variance( + parameter_dictionary->GetDouble("odometer_rotational_variance")); return options; } @@ -264,15 +268,17 @@ void LocalTrajectoryBuilder::AddImuData( << common::RadToDeg(kMaxInclination); } -void LocalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { +void LocalTrajectoryBuilder::AddOdometerData(const common::Time time, + const transform::Rigid3d& pose) { if (pose_tracker_ == nullptr) { // Until we've initialized the UKF with our first IMU message, we cannot - // process odometry poses. + // process odometer poses. LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized."; } else { - pose_tracker_->AddOdometerPoseObservation(time, pose, covariance); + pose_tracker_->AddOdometerPoseObservation( + time, pose, kalman_filter::BuildPoseCovariance( + options_.odometer_translational_variance(), + options_.odometer_rotational_variance())); } } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index ffebdb7..a594cf9 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -67,8 +67,7 @@ class LocalTrajectoryBuilder { common::Time, const sensor::LaserFan& laser_fan); void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); - void AddOdometerData(common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance); + void AddOdometerData(common::Time time, const transform::Rigid3d& pose); const Submaps* submaps() const; diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index 8df426d..eca575d 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto @@ -56,4 +56,7 @@ message LocalTrajectoryBuilderOptions { // True if IMU data should be expected and used. optional bool use_imu_data = 12; + + optional double odometer_translational_variance = 17; + optional double odometer_rotational_variance = 18; } diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 8d0154f..5cb8ecc 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -59,10 +59,9 @@ void GlobalTrajectoryBuilder::AddRangefinderData( local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index); } -void GlobalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { - local_trajectory_builder_->AddOdometerData(time, pose, covariance); +void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, + const transform::Rigid3d& pose) { + local_trajectory_builder_->AddOdometerData(time, pose); } const GlobalTrajectoryBuilder::PoseEstimate& diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 0b6b9b9..28efb95 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -42,9 +42,8 @@ class GlobalTrajectoryBuilder const Eigen::Vector3d& angular_velocity) override; void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; - void AddOdometerData( - common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) override; + void AddOdometerData(common::Time time, + const transform::Rigid3d& pose) override; const PoseEstimate& pose_estimate() const override; private: diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 004918d..1489f8d 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -189,15 +189,19 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( } void KalmanLocalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { + const common::Time time, const transform::Rigid3d& pose) { if (!pose_tracker_) { pose_tracker_.reset(new kalman_filter::PoseTracker( options_.kalman_local_trajectory_builder_options() .pose_tracker_options(), kalman_filter::PoseTracker::ModelFunction::k3D, time)); } - pose_tracker_->AddOdometerPoseObservation(time, pose, covariance); + pose_tracker_->AddOdometerPoseObservation( + time, pose, kalman_filter::BuildPoseCovariance( + options_.kalman_local_trajectory_builder_options() + .odometer_translational_variance(), + options_.kalman_local_trajectory_builder_options() + .odometer_rotational_variance())); } const KalmanLocalTrajectoryBuilder::PoseEstimate& diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 3d1a922..b57003d 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -51,10 +51,8 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; - void AddOdometerData( - common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) override; - + void AddOdometerData(common::Time time, + const transform::Rigid3d& pose) override; void AddTrajectoryNodeIndex(int trajectory_node_index) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc index 72ad5f0..0a1c97a 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc @@ -36,6 +36,10 @@ CreateKalmanLocalTrajectoryBuilderOptions( *options.mutable_pose_tracker_options() = kalman_filter::CreatePoseTrackerOptions( parameter_dictionary->GetDictionary("pose_tracker").get()); + options.set_odometer_translational_variance( + parameter_dictionary->GetDouble("odometer_translational_variance")); + options.set_odometer_rotational_variance( + parameter_dictionary->GetDouble("odometer_rotational_variance")); return options; } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 1667442..a1fd5e0 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -111,6 +111,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { imu_gravity_variance = 1e-4, num_odometry_states = 1, }, + + odometer_translational_variance = 1e-7, + odometer_rotational_variance = 1e-7, }, optimizing_local_trajectory_builder = { high_resolution_grid_weight = 5., diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 0bf14b6..a04fcd7 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -56,9 +56,8 @@ class LocalTrajectoryBuilderInterface { virtual std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) = 0; - virtual void AddOdometerData( - common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) = 0; + virtual void AddOdometerData(common::Time time, + const transform::Rigid3d& pose) = 0; // Register a 'trajectory_node_index' from the SparsePoseGraph corresponding // to the latest inserted laser scan. This is used to remember which diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index e2ab29d..1d20407 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -126,8 +126,7 @@ void OptimizingLocalTrajectoryBuilder::AddImuData( } void OptimizingLocalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) { + const common::Time time, const transform::Rigid3d& pose) { odometer_data_.push_back(OdometerData{time, pose}); RemoveObsoleteSensorData(); } diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 422b53f..12775d7 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -55,10 +55,8 @@ class OptimizingLocalTrajectoryBuilder std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; - void AddOdometerData( - const common::Time time, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance) override; - + void AddOdometerData(const common::Time time, + const transform::Rigid3d& pose) override; void AddTrajectoryNodeIndex(int trajectory_node_index) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; diff --git a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto index a5eb823..2a28f04 100644 --- a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto @@ -27,4 +27,7 @@ message KalmanLocalTrajectoryBuilderOptions { optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 2; optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; + + optional double odometer_translational_variance = 4; + optional double odometer_rotational_variance = 5; } diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index cea4223..5d07aa9 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -31,13 +31,13 @@ namespace { TEST(Collator, Ordering) { const std::array kSensorId = { {"horizontal_laser", "vertical_laser", "imu", "odometry"}}; - Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{}); - Data first(common::FromUniversal(100), sensor::Data::Rangefinder{}); - Data second(common::FromUniversal(200), sensor::Data::Rangefinder{}); + Data zero(common::FromUniversal(0), Data::Rangefinder{}); + Data first(common::FromUniversal(100), Data::Rangefinder{}); + Data second(common::FromUniversal(200), Data::Rangefinder{}); Data third(common::FromUniversal(300), Data::Imu{}); - Data fourth(common::FromUniversal(400), sensor::Data::Rangefinder{}); - Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{}); - Data sixth(common::FromUniversal(600), Data::Odometer{}); + Data fourth(common::FromUniversal(400), Data::Rangefinder{}); + Data fifth(common::FromUniversal(500), Data::Rangefinder{}); + Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity()); std::vector> received; Collator collator; diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 5cb9705..81a0b64 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -42,27 +42,20 @@ struct Data { PointCloud ranges; }; - struct Odometer { - transform::Rigid3d pose; - // TODO(damonkohler): Remove this in favor of using the configurable - // constant variance directly. - kalman_filter::PoseCovariance covariance; - }; - Data(const common::Time time, const Imu& imu) : type(Type::kImu), time(time), imu(imu) {} Data(const common::Time time, const Rangefinder& rangefinder) : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} - Data(const common::Time time, const Odometer& odometer) - : type(Type::kOdometer), time(time), odometer(odometer) {} + Data(const common::Time time, const transform::Rigid3d& odometer_pose) + : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} Type type; common::Time time; Imu imu; Rangefinder rangefinder; - Odometer odometer; + transform::Rigid3d odometer_pose; }; } // namespace sensor diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 62a14f4..066b89a 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -20,6 +20,8 @@ TRAJECTORY_BUILDER_2D = { laser_max_z = 2., laser_missing_echo_ray_length = 5., laser_voxel_filter_size = 0.025, + odometer_translational_variance = 1e-7, + odometer_rotational_variance = 1e-7, use_online_correlative_scan_matching = false, adaptive_voxel_filter = { diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index d5df44d..cc61e99 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -83,6 +83,9 @@ TRAJECTORY_BUILDER_3D = { translation_delta_cost_weight = 1e-1, rotation_delta_cost_weight = 1e-1, }, + + odometer_translational_variance = 1e-7, + odometer_rotational_variance = 1e-7, }, optimizing_local_trajectory_builder = {