Move the ImuTracker and OdometryStateTracker to mapping. (#50)

Both are useful without a UKF.

cartographer/kalman_filter/CMakeLists.txt

@@ -20,16 +20,6 @@ google_library(kalman_filter_gaussian_distribution
     gaussian_distribution.h
 )
 
-google_library(kalman_filter_odometry_state_tracker
-  SRCS
-    odometry_state_tracker.cc
-  HDRS
-    odometry_state_tracker.h
-  DEPENDS
-    common_time
-    transform_rigid_transform
-)
-
 google_library(kalman_filter_pose_tracker
   USES_EIGEN
   USES_GLOG
@@ -43,9 +33,10 @@ google_library(kalman_filter_pose_tracker
     common_port
     common_time
     kalman_filter_gaussian_distribution
-    kalman_filter_odometry_state_tracker
     kalman_filter_proto_pose_tracker_options
     kalman_filter_unscented_kalman_filter
+    mapping_imu_tracker
+    mapping_odometry_state_tracker
     sensor_proto_sensor
     transform_transform
 )

cartographer/kalman_filter/pose_tracker.cc

@@ -123,56 +123,6 @@ PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
 
 }  // namespace
 
-ImuTracker::ImuTracker(const proto::PoseTrackerOptions& options,
-                       const common::Time time)
-    : options_(options),
-      time_(time),
-      last_linear_acceleration_time_(common::Time::min()),
-      orientation_(Eigen::Quaterniond::Identity()),
-      gravity_direction_(Eigen::Vector3d::UnitZ()),
-      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
-
-void ImuTracker::Predict(const common::Time time) {
-  CHECK_LE(time_, time);
-  const double delta_t = common::ToSeconds(time - time_);
-  const Eigen::Quaterniond rotation =
-      transform::AngleAxisVectorToRotationQuaternion(
-          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
-  orientation_ = (orientation_ * rotation).normalized();
-  gravity_direction_ = rotation.inverse() * gravity_direction_;
-  time_ = time;
-}
-
-void ImuTracker::AddImuLinearAccelerationObservation(
-    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
-  Predict(time);
-  // Update the 'gravity_direction_' with an exponential moving average using
-  // the 'imu_gravity_time_constant'.
-  const double delta_t =
-      last_linear_acceleration_time_ > common::Time::min()
-          ? common::ToSeconds(time - last_linear_acceleration_time_)
-          : std::numeric_limits<double>::infinity();
-  last_linear_acceleration_time_ = time;
-  const double alpha =
-      1. - std::exp(-delta_t / options_.imu_gravity_time_constant());
-  gravity_direction_ =
-      (1. - alpha) * gravity_direction_ + alpha * imu_linear_acceleration;
-  // Change the 'orientation_' so that it agrees with the current
-  // 'gravity_direction_'.
-  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
-      gravity_direction_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
-  orientation_ = (orientation_ * rotation).normalized();
-  CHECK_GT((orientation_.inverse() * Eigen::Vector3d::UnitZ())
-               .dot(gravity_direction_),
-           0.);
-}
-
-void ImuTracker::AddImuAngularVelocityObservation(
-    const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
-  Predict(time);
-  imu_angular_velocity_ = imu_angular_velocity;
-}
-
 PoseAndCovariance operator*(const transform::Rigid3d& transform,
                             const PoseAndCovariance& pose_and_covariance) {
   GaussianDistribution<double, 6> distribution(
@@ -219,7 +169,7 @@ PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
       model_function_(model_function),
       time_(time),
       kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
-      imu_tracker_(options, time),
+      imu_tracker_(options.imu_gravity_time_constant(), time),
       odometry_state_tracker_(options.num_odometry_states()) {}
 
 PoseTracker::~PoseTracker() {}
@@ -270,7 +220,7 @@ const PoseTracker::Distribution PoseTracker::BuildModelNoise(
 }
 
 void PoseTracker::Predict(const common::Time time) {
-  imu_tracker_.Predict(time);
+  imu_tracker_.Advance(time);
   CHECK_LE(time_, time);
   const double delta_t = common::ToSeconds(time - time_);
   if (delta_t == 0.) {
@@ -293,14 +243,15 @@ void PoseTracker::Predict(const common::Time time) {
 
 void PoseTracker::AddImuLinearAccelerationObservation(
     const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
-  imu_tracker_.AddImuLinearAccelerationObservation(time,
+  imu_tracker_.Advance(time);
-                                                   imu_linear_acceleration);
+  imu_tracker_.AddImuLinearAccelerationObservation(imu_linear_acceleration);
   Predict(time);
 }
 
 void PoseTracker::AddImuAngularVelocityObservation(
     const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
-  imu_tracker_.AddImuAngularVelocityObservation(time, imu_angular_velocity);
+  imu_tracker_.Advance(time);
+  imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity);
   Predict(time);
 }
 
@@ -349,8 +300,8 @@ void PoseTracker::AddOdometerPoseObservation(
       {time, odometer_pose, RigidFromState(belief.GetMean())});
 }
 
-const OdometryStateTracker::OdometryStates& PoseTracker::odometry_states()
+const mapping::OdometryStateTracker::OdometryStates&
-    const {
+PoseTracker::odometry_states() const {
   return odometry_state_tracker_.odometry_states();
 }
 

cartographer/kalman_filter/pose_tracker.h

@@ -26,9 +26,10 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/gaussian_distribution.h"
-#include "cartographer/kalman_filter/odometry_state_tracker.h"
 #include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
 #include "cartographer/kalman_filter/unscented_kalman_filter.h"
+#include "cartographer/mapping/imu_tracker.h"
+#include "cartographer/mapping/odometry_state_tracker.h"
 #include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/transform.h"
 
@@ -60,35 +61,6 @@ PoseCovariance PoseCovarianceFromProtoMatrix(
 proto::PoseTrackerOptions CreatePoseTrackerOptions(
     common::LuaParameterDictionary* parameter_dictionary);
 
-// Keeps track of the orientation using angular velocities and linear
-// accelerations from an IMU. Because averaged linear acceleration (assuming
-// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
-// though yaw does.
-class ImuTracker {
- public:
-  ImuTracker(const proto::PoseTrackerOptions& options, common::Time time);
-
-  // Updates the orientation to reflect the given 'time'.
-  void Predict(common::Time time);
-
-  // Updates from an IMU reading (in the IMU frame).
-  void AddImuLinearAccelerationObservation(
-      common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
-  void AddImuAngularVelocityObservation(
-      common::Time time, const Eigen::Vector3d& imu_angular_velocity);
-
-  // Query the current orientation estimate.
-  Eigen::Quaterniond orientation() { return orientation_; }
-
- private:
-  const proto::PoseTrackerOptions options_;
-  common::Time time_;
-  common::Time last_linear_acceleration_time_;
-  Eigen::Quaterniond orientation_;
-  Eigen::Vector3d gravity_direction_;
-  Eigen::Vector3d imu_angular_velocity_;
-};
-
 // A Kalman filter for a 3D state of position and orientation.
 // Includes functions to update from IMU and laser scan matches.
 class PoseTracker {
@@ -145,7 +117,7 @@ class PoseTracker {
   // Returns the belief at the 'time' which must be >= to the current time.
   Distribution GetBelief(common::Time time);
 
-  const OdometryStateTracker::OdometryStates& odometry_states() const;
+  const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
 
  private:
   // Returns the distribution required to initialize the KalmanFilter.
@@ -166,8 +138,8 @@ class PoseTracker {
   const ModelFunction model_function_;
   common::Time time_;
   KalmanFilter kalman_filter_;
-  ImuTracker imu_tracker_;
+  mapping::ImuTracker imu_tracker_;
-  OdometryStateTracker odometry_state_tracker_;
+  mapping::OdometryStateTracker odometry_state_tracker_;
 };
 
 }  // namespace kalman_filter

cartographer/mapping/CMakeLists.txt

@@ -28,6 +28,19 @@ google_library(mapping_global_trajectory_builder_interface
     sensor_point_cloud
 )
 
+google_library(mapping_imu_tracker
+  USES_EIGEN
+  USES_GLOG
+  SRCS
+    imu_tracker.cc
+  HDRS
+    imu_tracker.h
+  DEPENDS
+    common_math
+    common_time
+    transform_transform
+)
+
 google_library(mapping_map_builder
   USES_EIGEN
   USES_GLOG
@@ -57,6 +70,16 @@ google_library(mapping_map_builder
     transform_transform
 )
 
+google_library(mapping_odometry_state_tracker
+  SRCS
+    odometry_state_tracker.cc
+  HDRS
+    odometry_state_tracker.h
+  DEPENDS
+    common_time
+    transform_rigid_transform
+)
+
 google_library(mapping_probability_values
   USES_GLOG
   SRCS

cartographer/mapping/imu_tracker.cc

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/imu_tracker.h"
+
+#include <cmath>
+#include <limits>
+
+#include "cartographer/common/math.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+ImuTracker::ImuTracker(const double imu_gravity_time_constant,
+                       const common::Time time)
+    : imu_gravity_time_constant_(imu_gravity_time_constant),
+      time_(time),
+      last_linear_acceleration_time_(common::Time::min()),
+      orientation_(Eigen::Quaterniond::Identity()),
+      gravity_vector_(Eigen::Vector3d::UnitZ()),
+      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
+
+void ImuTracker::Advance(const common::Time time) {
+  CHECK_LE(time_, time);
+  const double delta_t = common::ToSeconds(time - time_);
+  const Eigen::Quaterniond rotation =
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
+  orientation_ = (orientation_ * rotation).normalized();
+  gravity_vector_ = rotation.inverse() * gravity_vector_;
+  time_ = time;
+}
+
+void ImuTracker::AddImuLinearAccelerationObservation(
+    const Eigen::Vector3d& imu_linear_acceleration) {
+  // Update the 'gravity_vector_' with an exponential moving average using the
+  // 'imu_gravity_time_constant'.
+  const double delta_t =
+      last_linear_acceleration_time_ > common::Time::min()
+          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
+          : std::numeric_limits<double>::infinity();
+  last_linear_acceleration_time_ = time_;
+  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
+  gravity_vector_ =
+      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
+  // Change the 'orientation_' so that it agrees with the current
+  // 'gravity_vector_'.
+  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
+      gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
+  orientation_ = (orientation_ * rotation).normalized();
+  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
+  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
+}
+
+void ImuTracker::AddImuAngularVelocityObservation(
+    const Eigen::Vector3d& imu_angular_velocity) {
+  imu_angular_velocity_ = imu_angular_velocity;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/imu_tracker.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_
+#define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_
+
+#include "Eigen/Geometry"
+#include "cartographer/common/time.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Keeps track of the orientation using angular velocities and linear
+// accelerations from an IMU. Because averaged linear acceleration (assuming
+// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
+// though yaw does.
+class ImuTracker {
+ public:
+  ImuTracker(double imu_gravity_time_constant, common::Time time);
+
+  // Advances to the given 'time' and updates the orientation to reflect this.
+  void Advance(common::Time time);
+
+  // Updates from an IMU reading (in the IMU frame).
+  void AddImuLinearAccelerationObservation(
+      const Eigen::Vector3d& imu_linear_acceleration);
+  void AddImuAngularVelocityObservation(
+      const Eigen::Vector3d& imu_angular_velocity);
+
+  // Query the current orientation estimate.
+  Eigen::Quaterniond orientation() { return orientation_; }
+
+ private:
+  const double imu_gravity_time_constant_;
+  common::Time time_;
+  common::Time last_linear_acceleration_time_;
+  Eigen::Quaterniond orientation_;
+  Eigen::Vector3d gravity_vector_;
+  Eigen::Vector3d imu_angular_velocity_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_

cartographer/mapping/odometry_state_tracker.cc

@@ -14,12 +14,12 @@
  * limitations under the License.
  */
 
-#include "cartographer/kalman_filter/odometry_state_tracker.h"
+#include "cartographer/mapping/odometry_state_tracker.h"
 
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
-namespace kalman_filter {
+namespace mapping {
 
 OdometryState::OdometryState(const common::Time time,
                              const transform::Rigid3d& odometer_pose,
@@ -50,5 +50,5 @@ const OdometryState& OdometryStateTracker::newest() const {
   return odometry_states_.back();
 }
 
-}  // namespace kalman_filter
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/odometry_state_tracker.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
+#ifndef CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_
-#define CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
+#define CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_
 
 #include <deque>
 
@@ -23,7 +23,7 @@
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
-namespace kalman_filter {
+namespace mapping {
 
 struct OdometryState {
   OdometryState(common::Time time, const transform::Rigid3d& odometer_pose,
@@ -52,7 +52,7 @@ class OdometryStateTracker {
   // Returns true if no elements are present in the odometry queue.
   bool empty() const;
 
-  // Retrieves the most recent OdometryState or an empty one if non yet present.
+  // Retrieves the most recent OdometryState. Must not be called when empty.
   const OdometryState& newest() const;
 
  private:
@@ -60,7 +60,7 @@ class OdometryStateTracker {
   size_t window_size_;
 };
 
-}  // namespace kalman_filter
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
+#endif  // CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_