Remove UKF related code and configuration. (#378)

cartographer/kalman_filter/gaussian_distribution.h

@@ -1,60 +0,0 @@
-/*
- * 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_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
-#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Geometry"
-
-namespace cartographer {
-namespace kalman_filter {
-
-template <typename T, int N>
-class GaussianDistribution {
- public:
-  GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
-                       const Eigen::Matrix<T, N, N>& covariance)
-      : mean_(mean), covariance_(covariance) {}
-
-  const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }
-
-  const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }
-
- private:
-  Eigen::Matrix<T, N, 1> mean_;
-  Eigen::Matrix<T, N, N> covariance_;
-};
-
-template <typename T, int N>
-GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
-                                     const GaussianDistribution<T, N>& rhs) {
-  return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
-                                    lhs.GetCovariance() + rhs.GetCovariance());
-}
-
-template <typename T, int N, int M>
-GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
-                                     const GaussianDistribution<T, M>& rhs) {
-  return GaussianDistribution<T, N>(
-      lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose());
-}
-
-}  // namespace kalman_filter
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_

cartographer/kalman_filter/gaussian_distribution_test.cc

@@ -1,37 +0,0 @@
-/*
- * 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/kalman_filter/gaussian_distribution.h"
-
-#include "gtest/gtest.h"
-
-namespace cartographer {
-namespace kalman_filter {
-namespace {
-
-TEST(GaussianDistributionTest, testConstructor) {
-  Eigen::Matrix2d covariance;
-  covariance << 1., 2., 3., 4.;
-  GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),
-                                               covariance);
-  EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);
-  EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);
-  EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);
-}
-
-}  // namespace
-}  // namespace kalman_filter
-}  // namespace cartographer

cartographer/kalman_filter/pose_tracker.cc

@@ -1,291 +0,0 @@
-/*
- * 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/kalman_filter/pose_tracker.h"
-
-#include <cmath>
-#include <limits>
-#include <utility>
-
-#include "Eigen/Geometry"
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/math.h"
-#include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/gaussian_distribution.h"
-#include "cartographer/kalman_filter/unscented_kalman_filter.h"
-#include "cartographer/transform/transform.h"
-#include "glog/logging.h"
-
-namespace cartographer {
-namespace kalman_filter {
-
-namespace {
-
-PoseTracker::State AddDelta(const PoseTracker::State& state,
-                            const PoseTracker::State& delta) {
-  PoseTracker::State new_state = state + delta;
-  const Eigen::Quaterniond orientation =
-      transform::AngleAxisVectorToRotationQuaternion(
-          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
-                          state[PoseTracker::kMapOrientationY],
-                          state[PoseTracker::kMapOrientationZ]));
-  const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
-                                        delta[PoseTracker::kMapOrientationY],
-                                        delta[PoseTracker::kMapOrientationZ]);
-  CHECK_LT(rotation_vector.norm(), M_PI / 2.)
-      << "Sigma point is far from the mean, recovered delta may be incorrect.";
-  const Eigen::Quaterniond rotation =
-      transform::AngleAxisVectorToRotationQuaternion(rotation_vector);
-  const Eigen::Vector3d new_orientation =
-      transform::RotationQuaternionToAngleAxisVector(orientation * rotation);
-  new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
-  new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
-  new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
-  return new_state;
-}
-
-PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
-                                const PoseTracker::State& target) {
-  PoseTracker::State delta = target - origin;
-  const Eigen::Quaterniond origin_orientation =
-      transform::AngleAxisVectorToRotationQuaternion(
-          Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
-                          origin[PoseTracker::kMapOrientationY],
-                          origin[PoseTracker::kMapOrientationZ]));
-  const Eigen::Quaterniond target_orientation =
-      transform::AngleAxisVectorToRotationQuaternion(
-          Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
-                          target[PoseTracker::kMapOrientationY],
-                          target[PoseTracker::kMapOrientationZ]));
-  const Eigen::Vector3d rotation =
-      transform::RotationQuaternionToAngleAxisVector(
-          origin_orientation.inverse() * target_orientation);
-  delta[PoseTracker::kMapOrientationX] = rotation.x();
-  delta[PoseTracker::kMapOrientationY] = rotation.y();
-  delta[PoseTracker::kMapOrientationZ] = rotation.z();
-  return delta;
-}
-
-// Build a model matrix for the given time delta.
-PoseTracker::State ModelFunction(const PoseTracker::State& state,
-                                 const double delta_t) {
-  CHECK_GT(delta_t, 0.);
-
-  PoseTracker::State new_state;
-  new_state[PoseTracker::kMapPositionX] =
-      state[PoseTracker::kMapPositionX] +
-      delta_t * state[PoseTracker::kMapVelocityX];
-  new_state[PoseTracker::kMapPositionY] =
-      state[PoseTracker::kMapPositionY] +
-      delta_t * state[PoseTracker::kMapVelocityY];
-  new_state[PoseTracker::kMapPositionZ] =
-      state[PoseTracker::kMapPositionZ] +
-      delta_t * state[PoseTracker::kMapVelocityZ];
-
-  new_state[PoseTracker::kMapOrientationX] =
-      state[PoseTracker::kMapOrientationX];
-  new_state[PoseTracker::kMapOrientationY] =
-      state[PoseTracker::kMapOrientationY];
-  new_state[PoseTracker::kMapOrientationZ] =
-      state[PoseTracker::kMapOrientationZ];
-
-  new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];
-  new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY];
-  new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ];
-
-  return new_state;
-}
-
-}  // namespace
-
-proto::PoseTrackerOptions CreatePoseTrackerOptions(
-    common::LuaParameterDictionary* const parameter_dictionary) {
-  proto::PoseTrackerOptions options;
-  options.set_position_model_variance(
-      parameter_dictionary->GetDouble("position_model_variance"));
-  options.set_orientation_model_variance(
-      parameter_dictionary->GetDouble("orientation_model_variance"));
-  options.set_velocity_model_variance(
-      parameter_dictionary->GetDouble("velocity_model_variance"));
-  options.set_imu_gravity_time_constant(
-      parameter_dictionary->GetDouble("imu_gravity_time_constant"));
-  options.set_num_odometry_states(
-      parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
-  CHECK_GT(options.num_odometry_states(), 0);
-  return options;
-}
-
-PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
-  State initial_state = State::Zero();
-  // We are certain about the complete state at the beginning. We define the
-  // initial pose to be at the origin and axis aligned. Additionally, we claim
-  // that we are not moving.
-  StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
-  return Distribution(initial_state, initial_covariance);
-}
-
-PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
-                         const common::Time time)
-    : options_(options),
-      time_(time),
-      kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
-      imu_tracker_(options.imu_gravity_time_constant(), time),
-      odometry_state_tracker_(options.num_odometry_states()) {}
-
-PoseTracker::~PoseTracker() {}
-
-PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
-  Predict(time);
-  return kalman_filter_.GetBelief();
-}
-
-transform::Rigid3d PoseTracker::GetPoseEstimateMean(const common::Time time) {
-  return RigidFromState(GetBelief(time).GetMean());
-}
-
-const PoseTracker::Distribution PoseTracker::BuildModelNoise(
-    const double delta_t) const {
-  // Position is constant, but orientation changes.
-  StateCovariance model_noise = StateCovariance::Zero();
-
-  model_noise.diagonal() <<
-      // Position in map.
-      options_.position_model_variance() * delta_t,
-      options_.position_model_variance() * delta_t,
-      options_.position_model_variance() * delta_t,
-
-      // Orientation in map.
-      options_.orientation_model_variance() * delta_t,
-      options_.orientation_model_variance() * delta_t,
-      options_.orientation_model_variance() * delta_t,
-
-      // Linear velocities in map.
-      options_.velocity_model_variance() * delta_t,
-      options_.velocity_model_variance() * delta_t,
-      options_.velocity_model_variance() * delta_t;
-
-  return Distribution(State::Zero(), model_noise);
-}
-
-void PoseTracker::Predict(const common::Time time) {
-  imu_tracker_.Advance(time);
-  CHECK_LE(time_, time);
-  const double delta_t = common::ToSeconds(time - time_);
-  if (delta_t == 0.) {
-    return;
-  }
-  kalman_filter_.Predict(
-      [this, delta_t](const State& state) -> State {
-        return ModelFunction(state, delta_t);
-      },
-      BuildModelNoise(delta_t));
-  time_ = time;
-}
-
-void PoseTracker::AddImuLinearAccelerationObservation(
-    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
-  imu_tracker_.Advance(time);
-  imu_tracker_.AddImuLinearAccelerationObservation(imu_linear_acceleration);
-  Predict(time);
-}
-
-void PoseTracker::AddImuAngularVelocityObservation(
-    const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
-  imu_tracker_.Advance(time);
-  imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity);
-  Predict(time);
-}
-
-void PoseTracker::AddPoseObservation(const common::Time time,
-                                     const transform::Rigid3d& pose,
-                                     const PoseCovariance& covariance) {
-  Predict(time);
-
-  // Noise covariance is taken directly from the input values.
-  const GaussianDistribution<double, 6> delta(
-      Eigen::Matrix<double, 6, 1>::Zero(), covariance);
-
-  kalman_filter_.Observe<6>(
-      [this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
-        const transform::Rigid3d state_pose = RigidFromState(state);
-        const Eigen::Vector3d delta_orientation =
-            transform::RotationQuaternionToAngleAxisVector(
-                pose.rotation().inverse() * state_pose.rotation());
-        const Eigen::Vector3d delta_translation =
-            state_pose.translation() - pose.translation();
-        Eigen::Matrix<double, 6, 1> return_value;
-        return_value << delta_translation, delta_orientation;
-        return return_value;
-      },
-      delta);
-}
-
-// Updates from the odometer are in the odometer's map-like frame, called the
-// 'odometry' frame. The odometer_pose converts data from the map frame
-// into the odometry frame.
-void PoseTracker::AddOdometerPoseObservation(
-    const common::Time time, const transform::Rigid3d& odometer_pose,
-    const PoseCovariance& covariance) {
-  if (!odometry_state_tracker_.empty()) {
-    const auto& previous_odometry_state = odometry_state_tracker_.newest();
-    const transform::Rigid3d delta =
-        previous_odometry_state.odometer_pose.inverse() * odometer_pose;
-    const transform::Rigid3d new_pose =
-        previous_odometry_state.state_pose * delta;
-    AddPoseObservation(time, new_pose, covariance);
-  }
-
-  const Distribution belief = GetBelief(time);
-
-  odometry_state_tracker_.AddOdometryState(
-      {time, odometer_pose, RigidFromState(belief.GetMean())});
-}
-
-const mapping::OdometryStateTracker::OdometryStates&
-PoseTracker::odometry_states() const {
-  return odometry_state_tracker_.odometry_states();
-}
-
-transform::Rigid3d PoseTracker::RigidFromState(
-    const PoseTracker::State& state) {
-  return transform::Rigid3d(
-      Eigen::Vector3d(state[PoseTracker::kMapPositionX],
-                      state[PoseTracker::kMapPositionY],
-                      state[PoseTracker::kMapPositionZ]),
-      transform::AngleAxisVectorToRotationQuaternion(
-          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
-                          state[PoseTracker::kMapOrientationY],
-                          state[PoseTracker::kMapOrientationZ])) *
-          imu_tracker_.orientation());
-}
-
-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

@@ -1,129 +0,0 @@
-/*
- * 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_KALMAN_FILTER_POSE_TRACKER_H_
-#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
-
-#include <deque>
-#include <memory>
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/port.h"
-#include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/gaussian_distribution.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"
-
-namespace cartographer {
-namespace kalman_filter {
-
-typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
-
-PoseCovariance BuildPoseCovariance(double translational_variance,
-                                   double rotational_variance);
-
-proto::PoseTrackerOptions CreatePoseTrackerOptions(
-    common::LuaParameterDictionary* parameter_dictionary);
-
-// A Kalman filter for a 3D state of position and orientation.
-// Includes functions to update from IMU and range data matches.
-class PoseTracker {
- public:
-  enum {
-    kMapPositionX = 0,
-    kMapPositionY,
-    kMapPositionZ,
-    kMapOrientationX,
-    kMapOrientationY,
-    kMapOrientationZ,
-    kMapVelocityX,
-    kMapVelocityY,
-    kMapVelocityZ,
-    kDimension  // We terminate loops with this.
-  };
-
-  using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
-  using State = KalmanFilter::StateType;
-  using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
-  using Distribution = GaussianDistribution<double, kDimension>;
-
-  // Create a new Kalman filter starting at the origin with a standard normal
-  // distribution at 'time'.
-  PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
-  virtual ~PoseTracker();
-
-  // 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(
-      common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
-  void AddImuAngularVelocityObservation(
-      common::Time time, const Eigen::Vector3d& imu_angular_velocity);
-
-  // Updates from a pose estimate in the map frame.
-  void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
-                          const PoseCovariance& covariance);
-
-  // Updates from a pose estimate in the odometer's map-like frame.
-  void AddOdometerPoseObservation(common::Time time,
-                                  const transform::Rigid3d& pose,
-                                  const PoseCovariance& covariance);
-
-  common::Time time() const { return time_; }
-
-  // Returns the belief at the 'time' which must be >= to the current time.
-  Distribution GetBelief(common::Time time);
-
-  const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
-
-  Eigen::Quaterniond gravity_orientation() const {
-    return imu_tracker_.orientation();
-  }
-
- private:
-  // Returns the distribution required to initialize the KalmanFilter.
-  static Distribution KalmanFilterInit();
-
-  // Build a model noise distribution (zero mean) for the given time delta.
-  const Distribution BuildModelNoise(double delta_t) const;
-
-  // Predict the state forward in time. This is a no-op if 'time' has not moved
-  // forward.
-  void Predict(common::Time time);
-
-  // Computes a pose combining the given 'state' with the 'imu_tracker_'
-  // orientation.
-  transform::Rigid3d RigidFromState(const PoseTracker::State& state);
-
-  const proto::PoseTrackerOptions options_;
-  common::Time time_;
-  KalmanFilter kalman_filter_;
-  mapping::ImuTracker imu_tracker_;
-  mapping::OdometryStateTracker odometry_state_tracker_;
-};
-
-}  // namespace kalman_filter
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_

cartographer/kalman_filter/pose_tracker_test.cc

@@ -1,219 +0,0 @@
-/*
- * 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/kalman_filter/pose_tracker.h"
-
-#include <random>
-
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
-#include "cartographer/common/make_unique.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/rigid_transform_test_helpers.h"
-#include "gtest/gtest.h"
-
-namespace cartographer {
-namespace kalman_filter {
-namespace {
-
-constexpr double kOdometerVariance = 1e-12;
-
-using transform::IsNearly;
-using transform::Rigid3d;
-using ::testing::Not;
-
-class PoseTrackerTest : public ::testing::Test {
- protected:
-  PoseTrackerTest() {
-    auto parameter_dictionary = common::MakeDictionary(R"text(
-        return {
-            orientation_model_variance = 1e-8,
-            position_model_variance = 1e-8,
-            velocity_model_variance = 1e-8,
-            imu_gravity_time_constant = 100.,
-            num_odometry_states = 1,
-        }
-        )text");
-    const proto::PoseTrackerOptions options =
-        CreatePoseTrackerOptions(parameter_dictionary.get());
-    pose_tracker_ =
-        common::make_unique<PoseTracker>(options, common::FromUniversal(1000));
-  }
-
-  std::unique_ptr<PoseTracker> pose_tracker_;
-};
-
-TEST_F(PoseTrackerTest, SaveAndRestore) {
-  std::vector<Rigid3d> poses(3);
-  poses[0] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(1500));
-
-  pose_tracker_->AddImuLinearAccelerationObservation(
-      common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
-
-  PoseTracker copy_of_pose_tracker = *pose_tracker_;
-
-  const Eigen::Vector3d observation(2, 0, 8);
-  pose_tracker_->AddImuLinearAccelerationObservation(
-      common::FromUniversal(3000), observation);
-
-  poses[1] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(3500));
-
-  copy_of_pose_tracker.AddImuLinearAccelerationObservation(
-      common::FromUniversal(3000), observation);
-  poses[2] =
-      copy_of_pose_tracker.GetPoseEstimateMean(common::FromUniversal(3500));
-
-  EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
-  EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
-}
-
-TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
-  auto time = common::FromUniversal(1000);
-
-  for (int i = 0; i < 300; ++i) {
-    time += std::chrono::seconds(5);
-    pose_tracker_->AddImuLinearAccelerationObservation(
-        time, Eigen::Vector3d(0., 0., 10.));
-  }
-
-  {
-    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"
-                                                 << actual.coeffs();
-  }
-
-  for (int i = 0; i < 300; ++i) {
-    time += std::chrono::seconds(5);
-    pose_tracker_->AddImuLinearAccelerationObservation(
-        time, Eigen::Vector3d(0., 10., 0.));
-  }
-
-  time += std::chrono::milliseconds(5);
-
-  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()));
-  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
-                                               << actual.coeffs();
-}
-
-TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
-  auto time = common::FromUniversal(1000);
-
-  for (int i = 0; i < 300; ++i) {
-    time += std::chrono::milliseconds(5);
-    pose_tracker_->AddImuAngularVelocityObservation(time,
-                                                    Eigen::Vector3d::Zero());
-  }
-
-  {
-    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"
-                                                 << actual.coeffs();
-  }
-
-  const double target_radians = M_PI / 2.;
-  const double num_observations = 300.;
-  const double angular_velocity = target_radians / (num_observations * 5e-3);
-  for (int i = 0; i < num_observations; ++i) {
-    time += std::chrono::milliseconds(5);
-    pose_tracker_->AddImuAngularVelocityObservation(
-        time, Eigen::Vector3d(angular_velocity, 0., 0.));
-  }
-
-  time += std::chrono::milliseconds(5);
-
-  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()));
-  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
-                                               << actual.coeffs();
-}
-
-TEST_F(PoseTrackerTest, AddPoseObservation) {
-  auto time = common::FromUniversal(1000);
-
-  for (int i = 0; i < 300; ++i) {
-    time += std::chrono::milliseconds(5);
-    pose_tracker_->AddPoseObservation(
-        time, Rigid3d::Identity(),
-        Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
-  }
-
-  {
-    const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
-    EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
-  }
-
-  const Rigid3d expected =
-      Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
-      Rigid3d::Rotation(Eigen::AngleAxisd(
-          M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
-
-  for (int i = 0; i < 300; ++i) {
-    time += std::chrono::milliseconds(15);
-    pose_tracker_->AddPoseObservation(
-        time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
-  }
-
-  time += std::chrono::milliseconds(15);
-
-  const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
-  EXPECT_THAT(actual, IsNearly(expected, 1e-3));
-}
-
-TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
-  common::Time time = common::FromUniversal(0);
-
-  std::vector<Rigid3d> odometer_track;
-  odometer_track.push_back(Rigid3d::Identity());
-  odometer_track.push_back(
-      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
-  odometer_track.push_back(
-      Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
-      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
-  odometer_track.push_back(
-      Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
-      Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
-  odometer_track.push_back(
-      Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
-      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
-  odometer_track.push_back(
-      Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
-      Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
-  odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
-
-  Rigid3d actual;
-  for (const Rigid3d& pose : odometer_track) {
-    time += std::chrono::seconds(1);
-    pose_tracker_->AddOdometerPoseObservation(
-        time, pose, kOdometerVariance * PoseCovariance::Identity());
-    actual = pose_tracker_->GetPoseEstimateMean(time);
-    EXPECT_THAT(actual, IsNearly(pose, 1e-2));
-  }
-  // Sanity check that the test has signal:
-  EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
-}
-
-}  // namespace
-}  // namespace kalman_filter
-}  // namespace cartographer

cartographer/kalman_filter/proto/pose_tracker_options.proto

@@ -1,31 +0,0 @@
-// 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.
-
-syntax = "proto2";
-
-package cartographer.kalman_filter.proto;
-
-message PoseTrackerOptions {
-  // Model variances depend linearly on time.
-  optional double position_model_variance = 1;
-  optional double orientation_model_variance = 2;
-  optional double velocity_model_variance = 3;
-
-  // Time constant for the orientation moving average based on observed gravity
-  // via linear acceleration.
-  optional double imu_gravity_time_constant = 4;
-
-  // Maximum number of previous odometry states to keep.
-  optional int32 num_odometry_states = 6;
-}

cartographer/kalman_filter/unscented_kalman_filter.h

@@ -1,291 +0,0 @@
-/*
- * 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_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
-#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
-
-#include <algorithm>
-#include <cmath>
-#include <functional>
-#include <vector>
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "cartographer/kalman_filter/gaussian_distribution.h"
-#include "glog/logging.h"
-
-namespace cartographer {
-namespace kalman_filter {
-
-template <typename FloatType>
-constexpr FloatType sqr(FloatType a) {
-  return a * a;
-}
-
-template <typename FloatType, int N>
-Eigen::Matrix<FloatType, N, N> OuterProduct(
-    const Eigen::Matrix<FloatType, N, 1>& v) {
-  return v * v.transpose();
-}
-
-// Checks if 'A' is a symmetric matrix.
-template <typename FloatType, int N>
-void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
-  // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
-  // symmetric.
-  const FloatType norm = (A - A.transpose()).norm();
-  CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
-      << "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
-      << A;
-}
-
-// Returns the matrix square root of a symmetric positive semidefinite matrix.
-template <typename FloatType, int N>
-Eigen::Matrix<FloatType, N, N> MatrixSqrt(
-    const Eigen::Matrix<FloatType, N, N>& A) {
-  CheckSymmetric(A);
-
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
-      adjoint_eigen_solver((A + A.transpose()) / 2.);
-  const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
-  CHECK_GT(eigenvalues.minCoeff(), -1e-5)
-      << "MatrixSqrt failed with negative eigenvalues: "
-      << eigenvalues.transpose();
-
-  return adjoint_eigen_solver.eigenvectors() *
-         adjoint_eigen_solver.eigenvalues()
-             .cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
-             .cwiseSqrt()
-             .asDiagonal() *
-         adjoint_eigen_solver.eigenvectors().transpose();
-}
-
-// Implementation of a Kalman filter. We follow the nomenclature from
-// Thrun, S. et al., Probabilistic Robotics, 2006.
-//
-// Extended to handle non-additive noise/sensors inspired by Kraft, E., A
-// Quaternion-based Unscented Kalman Filter for Orientation Tracking.
-template <typename FloatType, int N>
-class UnscentedKalmanFilter {
- public:
-  using StateType = Eigen::Matrix<FloatType, N, 1>;
-  using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;
-
-  explicit UnscentedKalmanFilter(
-      const GaussianDistribution<FloatType, N>& initial_belief,
-      std::function<StateType(const StateType& state, const StateType& delta)>
-          add_delta = [](const StateType& state,
-                         const StateType& delta) { return state + delta; },
-      std::function<StateType(const StateType& origin, const StateType& target)>
-          compute_delta =
-              [](const StateType& origin, const StateType& target) {
-                return target - origin;
-              })
-      : belief_(initial_belief),
-        add_delta_(add_delta),
-        compute_delta_(compute_delta) {}
-
-  // Does the control/prediction step for the filter. The control must be
-  // implicitly added by the function g which also does the state transition.
-  // 'epsilon' is the additive combination of control and model noise.
-  void Predict(std::function<StateType(const StateType&)> g,
-               const GaussianDistribution<FloatType, N>& epsilon) {
-    CheckSymmetric(epsilon.GetCovariance());
-
-    // Get the state mean and matrix root of its covariance.
-    const StateType& mu = belief_.GetMean();
-    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
-
-    std::vector<StateType> Y;
-    Y.reserve(2 * N + 1);
-    Y.emplace_back(g(mu));
-
-    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
-    for (int i = 0; i < N; ++i) {
-      // Order does not matter here as all have the same weights in the
-      // summation later on anyways.
-      Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));
-      Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));
-    }
-    const StateType new_mu = ComputeMean(Y);
-
-    StateCovarianceType new_sigma =
-        kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
-    for (int i = 0; i < N; ++i) {
-      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
-                                     compute_delta_(new_mu, Y[2 * i + 1]));
-      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
-                                     compute_delta_(new_mu, Y[2 * i + 2]));
-    }
-    CheckSymmetric(new_sigma);
-
-    belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;
-  }
-
-  // The observation step of the Kalman filter. 'h' transfers the state
-  // into an observation that should be zero, i.e., the sensor readings should
-  // be included in this function already. 'delta' is the measurement noise and
-  // must have zero mean.
-  template <int K>
-  void Observe(
-      std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
-      const GaussianDistribution<FloatType, K>& delta) {
-    CheckSymmetric(delta.GetCovariance());
-    // We expect zero mean delta.
-    CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);
-
-    // Get the state mean and matrix root of its covariance.
-    const StateType& mu = belief_.GetMean();
-    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
-
-    // As in Kraft's paper, we compute W containing the zero-mean sigma points,
-    // since this is all we need.
-    std::vector<StateType> W;
-    W.reserve(2 * N + 1);
-    W.emplace_back(StateType::Zero());
-
-    std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
-    Z.reserve(2 * N + 1);
-    Z.emplace_back(h(mu));
-
-    Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
-    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
-    for (int i = 0; i < N; ++i) {
-      // Order does not matter here as all have the same weights in the
-      // summation later on anyways.
-      W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
-      Z.emplace_back(h(add_delta_(mu, W.back())));
-
-      W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
-      Z.emplace_back(h(add_delta_(mu, W.back())));
-
-      z_hat += kMeanWeightI * Z[2 * i + 1];
-      z_hat += kMeanWeightI * Z[2 * i + 2];
-    }
-
-    Eigen::Matrix<FloatType, K, K> S =
-        kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
-    for (int i = 0; i < N; ++i) {
-      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
-      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
-    }
-    CheckSymmetric(S);
-    S += delta.GetCovariance();
-
-    Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
-        kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
-    for (int i = 0; i < N; ++i) {
-      sigma_bar_xz +=
-          kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
-      sigma_bar_xz +=
-          kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
-    }
-
-    const Eigen::Matrix<FloatType, N, K> kalman_gain =
-        sigma_bar_xz * S.inverse();
-    const StateCovarianceType new_sigma =
-        belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
-    CheckSymmetric(new_sigma);
-
-    belief_ = GaussianDistribution<FloatType, N>(
-        add_delta_(mu, kalman_gain * -z_hat), new_sigma);
-  }
-
-  const GaussianDistribution<FloatType, N>& GetBelief() const {
-    return belief_;
-  }
-
- private:
-  StateType ComputeWeightedError(const StateType& mean_estimate,
-                                 const std::vector<StateType>& states) {
-    StateType weighted_error =
-        kMeanWeight0 * compute_delta_(mean_estimate, states[0]);
-    for (int i = 1; i != 2 * N + 1; ++i) {
-      weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);
-    }
-    return weighted_error;
-  }
-
-  // Algorithm for computing the mean of non-additive states taken from Kraft's
-  // Section 3.4, adapted to our implementation.
-  StateType ComputeMean(const std::vector<StateType>& states) {
-    CHECK_EQ(states.size(), 2 * N + 1);
-    StateType current_estimate = states[0];
-    StateType weighted_error = ComputeWeightedError(current_estimate, states);
-    int iterations = 0;
-    while (weighted_error.norm() > 1e-9) {
-      double step_size = 1.;
-      while (true) {
-        const StateType next_estimate =
-            add_delta_(current_estimate, step_size * weighted_error);
-        const StateType next_error =
-            ComputeWeightedError(next_estimate, states);
-        if (next_error.norm() < weighted_error.norm()) {
-          current_estimate = next_estimate;
-          weighted_error = next_error;
-          break;
-        }
-        step_size *= 0.5;
-        CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";
-      }
-      ++iterations;
-      CHECK_LT(iterations, 20) << "Too many iterations.";
-    }
-    return current_estimate;
-  }
-
-  // According to Wikipedia these are the normal values. Thrun does not
-  // mention those.
-  constexpr static FloatType kAlpha = 1e-3;
-  constexpr static FloatType kKappa = 0.;
-  constexpr static FloatType kBeta = 2.;
-  constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;
-  constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);
-  constexpr static FloatType kCovWeight0 =
-      kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);
-  constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));
-  constexpr static FloatType kCovWeightI = kMeanWeightI;
-
-  GaussianDistribution<FloatType, N> belief_;
-  const std::function<StateType(const StateType& state, const StateType& delta)>
-      add_delta_;
-  const std::function<StateType(const StateType& origin,
-                                const StateType& target)>
-      compute_delta_;
-};
-
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kAlpha;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kKappa;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kBeta;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kLambda;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeight0;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeight0;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeightI;
-template <typename FloatType, int N>
-constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeightI;
-
-}  // namespace kalman_filter
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_

cartographer/kalman_filter/unscented_kalman_filter_test.cc

@@ -1,74 +0,0 @@
-/*
- * 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/kalman_filter/unscented_kalman_filter.h"
-
-#include "cartographer/kalman_filter/gaussian_distribution.h"
-#include "gtest/gtest.h"
-
-namespace cartographer {
-namespace kalman_filter {
-namespace {
-
-Eigen::Matrix<double, 1, 1> Scalar(double value) {
-  return value * Eigen::Matrix<double, 1, 1>::Identity();
-}
-
-// A simple model that copies the first to the second state variable.
-Eigen::Matrix<double, 2, 1> g(const Eigen::Matrix<double, 2, 1>& state) {
-  Eigen::Matrix<double, 2, 1> new_state;
-  new_state << state[0], state[0];
-  return new_state;
-}
-
-// A simple observation of the first state variable.
-Eigen::Matrix<double, 1, 1> h(const Eigen::Matrix<double, 2, 1>& state) {
-  return Scalar(state[0]) - Scalar(5.);
-}
-
-TEST(KalmanFilterTest, testConstructor) {
-  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
-      Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
-  EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9);
-}
-
-TEST(KalmanFilterTest, testPredict) {
-  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
-      Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity()));
-  filter.Predict(
-      g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
-                                         Eigen::Matrix2d::Identity() * 1e-9));
-  EXPECT_NEAR(filter.GetBelief().GetMean()[0], 42., 1e-2);
-  EXPECT_NEAR(filter.GetBelief().GetMean()[1], 42., 1e-2);
-}
-
-TEST(KalmanFilterTest, testObserve) {
-  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
-      Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
-  for (int i = 0; i < 500; ++i) {
-    filter.Predict(
-        g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
-                                           Eigen::Matrix2d::Identity() * 1e-9));
-    filter.Observe<1>(
-        h, GaussianDistribution<double, 1>(Scalar(0.), Scalar(1e-2)));
-  }
-  EXPECT_NEAR(filter.GetBelief().GetMean()[0], 5, 1e-2);
-  EXPECT_NEAR(filter.GetBelief().GetMean()[1], 5, 1e-2);
-}
-
-}  // namespace
-}  // namespace kalman_filter
-}  // namespace cartographer

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -150,6 +150,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
   if (last_scan_match_time_ > common::Time::min() &&
       time > last_scan_match_time_) {
     const double delta_t = common::ToSeconds(time - last_scan_match_time_);
+    // This adds the observed difference in velocity that would have reduced the
+    // error to zero.
     velocity_estimate_ += (pose_estimate_.translation().head<2>() -
                            model_prediction.translation().head<2>()) /
                           delta_t;

cartographer/mapping_2d/proto/local_trajectory_builder_options.proto

@@ -36,14 +36,13 @@ message LocalTrajectoryBuilderOptions {
   // cropping.
   optional float voxel_filter_size = 3;
 
-  // Whether to solve the online scan matching first using the correlative scan
-  // matcher to generate a good starting point for Ceres.
-  optional bool use_online_correlative_scan_matching = 5;
-
   // Voxel filter used to compute a sparser point cloud for matching.
   optional sensor.proto.AdaptiveVoxelFilterOptions
       adaptive_voxel_filter_options = 6;
 
+  // Whether to solve the online scan matching first using the correlative scan
+  // matcher to generate a good starting point for Ceres.
+  optional bool use_online_correlative_scan_matching = 5;
   optional scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
       real_time_correlative_scan_matcher_options = 7;
   optional scan_matching.proto.CeresScanMatcherOptions

cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc

@@ -1,49 +0,0 @@
-/*
- * 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_3d/kalman_local_trajectory_builder_options.h"
-
-#include "cartographer/kalman_filter/pose_tracker.h"
-#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-proto::KalmanLocalTrajectoryBuilderOptions
-CreateKalmanLocalTrajectoryBuilderOptions(
-    common::LuaParameterDictionary* const parameter_dictionary) {
-  proto::KalmanLocalTrajectoryBuilderOptions options;
-  options.set_use_online_correlative_scan_matching(
-      parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
-  *options.mutable_real_time_correlative_scan_matcher_options() =
-      mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
-          parameter_dictionary
-              ->GetDictionary("real_time_correlative_scan_matcher")
-              .get());
-  *options.mutable_pose_tracker_options() =
-      kalman_filter::CreatePoseTrackerOptions(
-          parameter_dictionary->GetDictionary("pose_tracker").get());
-  options.set_scan_matcher_variance(
-      parameter_dictionary->GetDouble("scan_matcher_variance"));
-  options.set_odometer_translational_variance(
-      parameter_dictionary->GetDouble("odometer_translational_variance"));
-  options.set_odometer_rotational_variance(
-      parameter_dictionary->GetDouble("odometer_rotational_variance"));
-  return options;
-}
-
-}  // namespace mapping_3d
-}  // namespace cartographer

cartographer/mapping_3d/kalman_local_trajectory_builder_options.h

@@ -1,33 +0,0 @@
-/*
- * 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_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
-#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
-
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-proto::KalmanLocalTrajectoryBuilderOptions
-CreateKalmanLocalTrajectoryBuilderOptions(
-    common::LuaParameterDictionary* parameter_dictionary);
-
-}  // namespace mapping_3d
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -18,7 +18,6 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
 #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/proto/submaps_options.pb.h"
@@ -35,13 +34,10 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
       motion_filter_(options.motion_filter_options()),
       real_time_correlative_scan_matcher_(
           common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
-              options_.kalman_local_trajectory_builder_options()
+              options_.real_time_correlative_scan_matcher_options())),
-                  .real_time_correlative_scan_matcher_options())),
       ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
           options_.ceres_scan_matcher_options())),
-      odometry_state_tracker_(options_.kalman_local_trajectory_builder_options()
+      odometry_state_tracker_(options_.num_odometry_states()),
-                                  .pose_tracker_options()
-                                  .num_odometry_states()),
       accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
@@ -49,27 +45,26 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 void LocalTrajectoryBuilder::AddImuData(
     const common::Time time, const Eigen::Vector3d& linear_acceleration,
     const Eigen::Vector3d& angular_velocity) {
-  if (!imu_tracker_) {
+  const bool initial_imu_data = (imu_tracker_ == nullptr);
+  if (initial_imu_data) {
     imu_tracker_ = common::make_unique<mapping::ImuTracker>(
-        options_.kalman_local_trajectory_builder_options()
+        options_.imu_gravity_time_constant(), time);
-            .pose_tracker_options()
-            .imu_gravity_time_constant(),
-        time);
-    // Use the accelerometer to gravity align the first pose.
-    imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
-    pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
   }
-
   Predict(time);
   imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
   imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
+  if (initial_imu_data) {
+    // This uses the first accelerometer measurement to approximately align the
+    // first pose to gravity.
+    pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
+  }
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
                                            const Eigen::Vector3f& origin,
                                            const sensor::PointCloud& ranges) {
-  if (!imu_tracker_) {
+  if (imu_tracker_ == nullptr) {
     LOG(INFO) << "ImuTracker not yet initialized.";
     return nullptr;
   }
@@ -160,8 +155,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_point_cloud_in_tracking =
       adaptive_voxel_filter.Filter(filtered_range_data.returns);
-  if (options_.kalman_local_trajectory_builder_options()
+  if (options_.use_online_correlative_scan_matching()) {
-          .use_online_correlative_scan_matching()) {
     // We take a copy since we use 'intial_ceres_pose' as an output argument.
     const transform::Rigid3d initial_pose = initial_ceres_pose;
     real_time_correlative_scan_matcher_->Match(
@@ -200,6 +194,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   if (last_scan_match_time_ > common::Time::min() &&
       time > last_scan_match_time_) {
     const double delta_t = common::ToSeconds(time - last_scan_match_time_);
+    // This adds the observed difference in velocity that would have reduced the
+    // error to zero.
     velocity_estimate_ +=
         (pose_estimate_.translation() - model_prediction.translation()) /
         delta_t;

cartographer/mapping_3d/local_trajectory_builder.h

@@ -20,8 +20,9 @@
 #include <memory>
 
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping/imu_tracker.h"
+#include "cartographer/mapping/odometry_state_tracker.h"
 #include "cartographer/mapping_3d/motion_filter.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"

cartographer/mapping_3d/local_trajectory_builder_options.cc

@@ -16,7 +16,7 @@
 
 #include "cartographer/mapping_3d/local_trajectory_builder_options.h"
 
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h"
+#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
 #include "cartographer/mapping_3d/motion_filter.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_3d/submaps.h"
@@ -45,17 +45,25 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
           parameter_dictionary
               ->GetDictionary("low_resolution_adaptive_voxel_filter")
               .get());
+  options.set_use_online_correlative_scan_matching(
+      parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
+  *options.mutable_real_time_correlative_scan_matcher_options() =
+      mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
+          parameter_dictionary
+              ->GetDictionary("real_time_correlative_scan_matcher")
+              .get());
   *options.mutable_ceres_scan_matcher_options() =
       scan_matching::CreateCeresScanMatcherOptions(
           parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
   *options.mutable_motion_filter_options() = CreateMotionFilterOptions(
       parameter_dictionary->GetDictionary("motion_filter").get());
+  options.set_imu_gravity_time_constant(
+      parameter_dictionary->GetDouble("imu_gravity_time_constant"));
+  options.set_num_odometry_states(
+      parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
+  CHECK_GT(options.num_odometry_states(), 0);
   *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions(
       parameter_dictionary->GetDictionary("submaps").get());
-  *options.mutable_kalman_local_trajectory_builder_options() =
-      CreateKalmanLocalTrajectoryBuilderOptions(
-          parameter_dictionary->GetDictionary("kalman_local_trajectory_builder")
-              .get());
   return options;
 }
 

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -64,6 +64,14 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
             max_range = 50.,
           },
 
+          use_online_correlative_scan_matching = false,
+          real_time_correlative_scan_matcher = {
+            linear_search_window = 0.2,
+            angular_search_window = math.rad(1.),
+            translation_delta_cost_weight = 1e-1,
+            rotation_delta_cost_weight = 1.,
+          },
+
           ceres_scan_matcher = {
             occupied_space_weight_0 = 5.,
             occupied_space_weight_1 = 20.,
@@ -76,11 +84,16 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
               num_threads = 1,
             },
           },
+
           motion_filter = {
             max_time_seconds = 0.2,
             max_distance_meters = 0.02,
             max_angle_radians = 0.001,
           },
+
+          imu_gravity_time_constant = 1.,
+          num_odometry_states = 1,
+
           submaps = {
             high_resolution = 0.2,
             high_resolution_max_range = 50.,
@@ -92,27 +105,6 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
               num_free_space_voxels = 0,
             },
           },
-
-          kalman_local_trajectory_builder =  {
-            use_online_correlative_scan_matching = false,
-            real_time_correlative_scan_matcher = {
-              linear_search_window = 0.2,
-              angular_search_window = math.rad(1.),
-              translation_delta_cost_weight = 1e-1,
-              rotation_delta_cost_weight = 1.,
-            },
-            pose_tracker = {
-              orientation_model_variance = 5e-4,
-              position_model_variance = 0.000654766,
-              velocity_model_variance = 0.053926,
-              imu_gravity_time_constant = 1.,
-              num_odometry_states = 1,
-            },
-
-            scan_matcher_variance = 1e-6,
-            odometer_translational_variance = 1e-7,
-            odometer_rotational_variance = 1e-7,
-          },
         }
         )text");
     return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());

cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto

@@ -1,34 +0,0 @@
-// 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.
-
-syntax = "proto2";
-
-import "cartographer/kalman_filter/proto/pose_tracker_options.proto";
-import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
-
-package cartographer.mapping_3d.proto;
-
-message KalmanLocalTrajectoryBuilderOptions {
-  // Whether to solve the online scan matching first using the correlative scan
-  // matcher to generate a good starting point for Ceres.
-  optional bool use_online_correlative_scan_matching = 1;
-
-  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 scan_matcher_variance = 6;
-  optional double odometer_translational_variance = 4;
-  optional double odometer_rotational_variance = 5;
-}

cartographer/mapping_3d/proto/local_trajectory_builder_options.proto

@@ -16,7 +16,7 @@ syntax = "proto2";
 
 import "cartographer/mapping_3d/proto/motion_filter_options.proto";
 import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
-import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto";
+import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
 import "cartographer/mapping_3d/proto/submaps_options.proto";
 import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
 
@@ -41,11 +41,25 @@ message LocalTrajectoryBuilderOptions {
   optional sensor.proto.AdaptiveVoxelFilterOptions
       low_resolution_adaptive_voxel_filter_options = 12;
 
+  // Whether to solve the online scan matching first using the correlative scan
+  // matcher to generate a good starting point for Ceres.
+  optional bool use_online_correlative_scan_matching = 13;
+  optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
+      real_time_correlative_scan_matcher_options = 14;
   optional scan_matching.proto.CeresScanMatcherOptions
       ceres_scan_matcher_options = 6;
   optional MotionFilterOptions motion_filter_options = 7;
-  optional SubmapsOptions submaps_options = 8;
 
-  optional KalmanLocalTrajectoryBuilderOptions
+  // Time constant in seconds for the orientation moving average based on
-      kalman_local_trajectory_builder_options = 10;
+  // observed gravity via the IMU. It should be chosen so that the error
+  // 1. from acceleration measurements not due to gravity (which gets worse when
+  // the constant is reduced) and
+  // 2. from integration of angular velocities (which gets worse when the
+  // constant is increased) is balanced.
+  optional double imu_gravity_time_constant = 15;
+
+  // Maximum number of previous odometry states to keep.
+  optional int32 num_odometry_states = 16;
+
+  optional SubmapsOptions submaps_options = 8;
 }

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -29,7 +29,6 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/thread_pool.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
 #include "cartographer/transform/transform.h"
@@ -72,8 +71,7 @@ void ConstraintBuilder::MaybeAddConstraint(
     ++pending_computations_[current_computation_];
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-        submap_id, submap_nodes, submap,
+        submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
-        [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, node_id,
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
@@ -96,8 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      submap_id, submap_nodes, submap,
+      submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
-      [=]() EXCLUDES(mutex_) {
         ComputeConstraint(
             submap_id, submap, node_id, true, /* match_full_submap */
             trajectory_connectivity, compressed_point_cloud,
@@ -232,8 +229,7 @@ void ConstraintBuilder::ComputeConstraint(
 
   ceres::Solver::Summary unused_summary;
   transform::Rigid3d constraint_transform;
-  ceres_scan_matcher_.Match(
+  ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
-      pose_estimate, pose_estimate,
                             {{&high_resolution_point_cloud,
                               submap_scan_matcher->high_resolution_hybrid_grid},
                              {&low_resolution_point_cloud,

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -121,8 +121,7 @@ class ConstraintBuilder {
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       const mapping::SubmapId& submap_id,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const Submap* submap, std::function<void()> work_item)
+      const Submap* submap, std::function<void()> work_item) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
   void ConstructSubmapScanMatcher(

configuration_files/trajectory_builder_2d.lua

@@ -21,13 +21,13 @@ TRAJECTORY_BUILDER_2D = {
   missing_data_ray_length = 5.,
   voxel_filter_size = 0.025,
 
-  use_online_correlative_scan_matching = false,
   adaptive_voxel_filter = {
     max_length = 0.5,
     min_num_points = 200,
     max_range = 50.,
   },
 
+  use_online_correlative_scan_matching = false,
   real_time_correlative_scan_matcher = {
     linear_search_window = 0.1,
     angular_search_window = math.rad(20.),

configuration_files/trajectory_builder_3d.lua

@@ -32,6 +32,14 @@ TRAJECTORY_BUILDER_3D = {
     max_range = MAX_3D_RANGE,
   },
 
+  use_online_correlative_scan_matching = false,
+  real_time_correlative_scan_matcher = {
+    linear_search_window = 0.15,
+    angular_search_window = math.rad(1.),
+    translation_delta_cost_weight = 1e-1,
+    rotation_delta_cost_weight = 1e-1,
+  },
+
   ceres_scan_matcher = {
     occupied_space_weight_0 = 1.,
     occupied_space_weight_1 = 6.,
@@ -51,6 +59,9 @@ TRAJECTORY_BUILDER_3D = {
     max_angle_radians = 0.004,
   },
 
+  imu_gravity_time_constant = 10.,
+  num_odometry_states = 1,
+
   submaps = {
     high_resolution = 0.10,
     high_resolution_max_range = 20.,
@@ -62,26 +73,4 @@ TRAJECTORY_BUILDER_3D = {
       num_free_space_voxels = 2,
     },
   },
-
-  kalman_local_trajectory_builder = {
-    pose_tracker = {
-      orientation_model_variance = 5e-3,
-      position_model_variance = 0.00654766,
-      velocity_model_variance = 0.53926,
-      imu_gravity_time_constant = 10.,
-      num_odometry_states = 1,
-    },
-
-    use_online_correlative_scan_matching = false,
-    real_time_correlative_scan_matcher = {
-      linear_search_window = 0.15,
-      angular_search_window = math.rad(1.),
-      translation_delta_cost_weight = 1e-1,
-      rotation_delta_cost_weight = 1e-1,
-    },
-
-    scan_matcher_variance = 2.34e-9,
-    odometer_translational_variance = 1e-7,
-    odometer_rotational_variance = 1e-7,
-  },
 }

docs/source/configuration.rst

@@ -33,26 +33,6 @@ int32 num_threads
   Not yet documented.
 
 
-cartographer.kalman_filter.proto.PoseTrackerOptions
-===================================================
-
-double position_model_variance
-  Model variances depend linearly on time.
-
-double orientation_model_variance
-  Not yet documented.
-
-double velocity_model_variance
-  Not yet documented.
-
-double imu_gravity_time_constant
-  Time constant for the orientation moving average based on observed gravity
-  via linear acceleration.
-
-int32 num_odometry_states
-  Maximum number of previous odometry states to keep.
-
-
 cartographer.mapping.proto.MapBuilderOptions
 ============================================
 
@@ -206,13 +186,13 @@ float voxel_filter_size
   Voxel filter that gets applied to the range data immediately after
   cropping.
 
+cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
+  Voxel filter used to compute a sparser point cloud for matching.
+
 bool use_online_correlative_scan_matching
   Whether to solve the online scan matching first using the correlative scan
   matcher to generate a good starting point for Ceres.
 
-cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
-  Voxel filter used to compute a sparser point cloud for matching.
-
 cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
   Not yet documented.
 
@@ -324,29 +304,6 @@ double rotation_delta_cost_weight
   Not yet documented.
 
 
-cartographer.mapping_3d.proto.KalmanLocalTrajectoryBuilderOptions
-=================================================================
-
-bool use_online_correlative_scan_matching
-  Whether to solve the online scan matching first using the correlative scan
-  matcher to generate a good starting point for Ceres.
-
-cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
-  Not yet documented.
-
-cartographer.kalman_filter.proto.PoseTrackerOptions pose_tracker_options
-  Not yet documented.
-
-double scan_matcher_variance
-  Not yet documented.
-
-double odometer_translational_variance
-  Not yet documented.
-
-double odometer_rotational_variance
-  Not yet documented.
-
-
 cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions
 ===========================================================
 
@@ -370,16 +327,31 @@ cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_vo
 cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options
   Not yet documented.
 
+bool use_online_correlative_scan_matching
+  Whether to solve the online scan matching first using the correlative scan
+  matcher to generate a good starting point for Ceres.
+
+cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
+  Not yet documented.
+
 cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options
   Not yet documented.
 
 cartographer.mapping_3d.proto.MotionFilterOptions motion_filter_options
   Not yet documented.
 
-cartographer.mapping_3d.proto.SubmapsOptions submaps_options
+double imu_gravity_time_constant
-  Not yet documented.
+  Time constant in seconds for the orientation moving average based on
+  observed gravity via the IMU. It should be chosen so that the error
+  1. from acceleration measurements not due to gravity (which gets worse when
+  the constant is reduced) and
+  2. from integration of angular velocities (which gets worse when the
+  constant is increased) is balanced.
 
-cartographer.mapping_3d.proto.KalmanLocalTrajectoryBuilderOptions kalman_local_trajectory_builder_options
+int32 num_odometry_states
+  Maximum number of previous odometry states to keep.
+
+cartographer.mapping_3d.proto.SubmapsOptions submaps_options
   Not yet documented.