Removes odometer covariance from public API. (#137)

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

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);

cartographer/mapping/collated_trajectory_builder.cc

@@ -94,8 +94,8 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
       return;
 
     case sensor::Data::Type::kOdometer:
-      wrapped_trajectory_builder_->AddOdometerData(
+      wrapped_trajectory_builder_->AddOdometerData(data->time,
-          data->time, data->odometer.pose, data->odometer.covariance);
+                                                   data->odometer_pose);
       return;
   }
   LOG(FATAL);

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(
+  virtual void AddOdometerData(common::Time time,
-      common::Time time, const transform::Rigid3d& pose,
+                               const transform::Rigid3d& pose) = 0;
-      const kalman_filter::PoseCovariance& covariance) = 0;
 };
 
 }  // namespace mapping

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 transform::Rigid3d& odometer_pose) {
-                       const kalman_filter::PoseCovariance& covariance) {
     AddSensorData(sensor_id,
-                  common::make_unique<sensor::Data>(
+                  common::make_unique<sensor::Data>(time, odometer_pose));
-                      time, sensor::Data::Odometer{pose, covariance}));
   }
 };
 

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -56,10 +56,9 @@ void GlobalTrajectoryBuilder::AddImuData(
                                        angular_velocity);
 }
 
-void GlobalTrajectoryBuilder::AddOdometerData(
+void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
-    const common::Time time, const transform::Rigid3d& pose,
+                                              const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
+  local_trajectory_builder_.AddOdometerData(time, pose);
-  local_trajectory_builder_.AddOdometerData(time, pose, covariance);
 }
 
 const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

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(
+  void AddOdometerData(common::Time time,
-      common::Time time, const transform::Rigid3d& pose,
+                       const transform::Rigid3d& pose) override;
-      const kalman_filter::PoseCovariance& covariance) override;
 
  private:
   const proto::LocalTrajectoryBuilderOptions options_;

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(
+void LocalTrajectoryBuilder::AddOdometerData(const common::Time time,
-    const common::Time time, const transform::Rigid3d& pose,
+                                             const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
   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()));
   }
 }
 

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,
+  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
-                       const kalman_filter::PoseCovariance& covariance);
 
   const Submaps* submaps() const;
 

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;
 }

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -59,10 +59,9 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index);
 }
 
-void GlobalTrajectoryBuilder::AddOdometerData(
+void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
-    const common::Time time, const transform::Rigid3d& pose,
+                                              const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
+  local_trajectory_builder_->AddOdometerData(time, pose);
-  local_trajectory_builder_->AddOdometerData(time, pose, covariance);
 }
 
 const GlobalTrajectoryBuilder::PoseEstimate&

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(
+  void AddOdometerData(common::Time time,
-      common::Time time, const transform::Rigid3d& pose,
+                       const transform::Rigid3d& pose) override;
-      const kalman_filter::PoseCovariance& covariance) override;
   const PoseEstimate& pose_estimate() const override;
 
  private:

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 common::Time time, const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
   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&

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -51,10 +51,8 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
       const sensor::PointCloud& ranges) override;
-  void AddOdometerData(
+  void AddOdometerData(common::Time time,
-      common::Time time, const transform::Rigid3d& pose,
+                       const transform::Rigid3d& pose) override;
-      const kalman_filter::PoseCovariance& covariance) override;
-
   void AddTrajectoryNodeIndex(int trajectory_node_index) override;
   const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;

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;
 }
 

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.,

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -56,9 +56,8 @@ class LocalTrajectoryBuilderInterface {
   virtual std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
       const sensor::PointCloud& ranges) = 0;
-  virtual void AddOdometerData(
+  virtual void AddOdometerData(common::Time time,
-      common::Time time, const transform::Rigid3d& pose,
+                               const transform::Rigid3d& pose) = 0;
-      const kalman_filter::PoseCovariance& covariance) = 0;
 
   // Register a 'trajectory_node_index' from the SparsePoseGraph corresponding
   // to the latest inserted laser scan. This is used to remember which

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 common::Time time, const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
   odometer_data_.push_back(OdometerData{time, pose});
   RemoveObsoleteSensorData();
 }

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -55,10 +55,8 @@ class OptimizingLocalTrajectoryBuilder
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
       const sensor::PointCloud& ranges) override;
-  void AddOdometerData(
+  void AddOdometerData(const common::Time time,
-      const common::Time time, const transform::Rigid3d& pose,
+                       const transform::Rigid3d& pose) override;
-      const kalman_filter::PoseCovariance& covariance) override;
-
   void AddTrajectoryNodeIndex(int trajectory_node_index) override;
   const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;

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;
 }

cartographer/sensor/collator_test.cc

@@ -31,13 +31,13 @@ namespace {
 TEST(Collator, Ordering) {
   const std::array<string, 4> kSensorId = {
       {"horizontal_laser", "vertical_laser", "imu", "odometry"}};
-  Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{});
+  Data zero(common::FromUniversal(0), Data::Rangefinder{});
-  Data first(common::FromUniversal(100), sensor::Data::Rangefinder{});
+  Data first(common::FromUniversal(100), Data::Rangefinder{});
-  Data second(common::FromUniversal(200), sensor::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 fourth(common::FromUniversal(400), Data::Rangefinder{});
-  Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{});
+  Data fifth(common::FromUniversal(500), Data::Rangefinder{});
-  Data sixth(common::FromUniversal(600), Data::Odometer{});
+  Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity());
 
   std::vector<std::pair<string, Data>> received;
   Collator collator;

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)
+  Data(const common::Time time, const transform::Rigid3d& odometer_pose)
-      : type(Type::kOdometer), time(time), odometer(odometer) {}
+      : 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

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 = {

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 = {