From 54bd81a78b4b7a1e5c10078eda06a07c851d51b3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 30 Jun 2017 13:19:13 +0200 Subject: [PATCH] Remove UKF related code and configuration. (#378) --- .../kalman_filter/gaussian_distribution.h | 60 ---- .../gaussian_distribution_test.cc | 37 --- cartographer/kalman_filter/pose_tracker.cc | 291 ------------------ cartographer/kalman_filter/pose_tracker.h | 129 -------- .../kalman_filter/pose_tracker_test.cc | 219 ------------- .../proto/pose_tracker_options.proto | 31 -- .../kalman_filter/unscented_kalman_filter.h | 291 ------------------ .../unscented_kalman_filter_test.cc | 74 ----- .../mapping_2d/local_trajectory_builder.cc | 2 + .../local_trajectory_builder_options.proto | 7 +- ...kalman_local_trajectory_builder_options.cc | 49 --- .../kalman_local_trajectory_builder_options.h | 33 -- .../mapping_3d/local_trajectory_builder.cc | 32 +- .../mapping_3d/local_trajectory_builder.h | 3 +- .../local_trajectory_builder_options.cc | 18 +- .../local_trajectory_builder_test.cc | 34 +- ...man_local_trajectory_builder_options.proto | 34 -- .../local_trajectory_builder_options.proto | 22 +- .../sparse_pose_graph/constraint_builder.cc | 20 +- .../sparse_pose_graph/constraint_builder.h | 3 +- configuration_files/trajectory_builder_2d.lua | 2 +- configuration_files/trajectory_builder_3d.lua | 33 +- docs/source/configuration.rst | 70 ++--- 23 files changed, 107 insertions(+), 1387 deletions(-) delete mode 100644 cartographer/kalman_filter/gaussian_distribution.h delete mode 100644 cartographer/kalman_filter/gaussian_distribution_test.cc delete mode 100644 cartographer/kalman_filter/pose_tracker.cc delete mode 100644 cartographer/kalman_filter/pose_tracker.h delete mode 100644 cartographer/kalman_filter/pose_tracker_test.cc delete mode 100644 cartographer/kalman_filter/proto/pose_tracker_options.proto delete mode 100644 cartographer/kalman_filter/unscented_kalman_filter.h delete mode 100644 cartographer/kalman_filter/unscented_kalman_filter_test.cc delete mode 100644 cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc delete mode 100644 cartographer/mapping_3d/kalman_local_trajectory_builder_options.h delete mode 100644 cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto diff --git a/cartographer/kalman_filter/gaussian_distribution.h b/cartographer/kalman_filter/gaussian_distribution.h deleted file mode 100644 index 45f905a..0000000 --- a/cartographer/kalman_filter/gaussian_distribution.h +++ /dev/null @@ -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 -class GaussianDistribution { - public: - GaussianDistribution(const Eigen::Matrix& mean, - const Eigen::Matrix& covariance) - : mean_(mean), covariance_(covariance) {} - - const Eigen::Matrix& GetMean() const { return mean_; } - - const Eigen::Matrix& GetCovariance() const { return covariance_; } - - private: - Eigen::Matrix mean_; - Eigen::Matrix covariance_; -}; - -template -GaussianDistribution operator+(const GaussianDistribution& lhs, - const GaussianDistribution& rhs) { - return GaussianDistribution(lhs.GetMean() + rhs.GetMean(), - lhs.GetCovariance() + rhs.GetCovariance()); -} - -template -GaussianDistribution operator*(const Eigen::Matrix& lhs, - const GaussianDistribution& rhs) { - return GaussianDistribution( - lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose()); -} - -} // namespace kalman_filter -} // namespace cartographer - -#endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ diff --git a/cartographer/kalman_filter/gaussian_distribution_test.cc b/cartographer/kalman_filter/gaussian_distribution_test.cc deleted file mode 100644 index c5e5e1d..0000000 --- a/cartographer/kalman_filter/gaussian_distribution_test.cc +++ /dev/null @@ -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 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 diff --git a/cartographer/kalman_filter/pose_tracker.cc b/cartographer/kalman_filter/pose_tracker.cc deleted file mode 100644 index caba4fe..0000000 --- a/cartographer/kalman_filter/pose_tracker.cc +++ /dev/null @@ -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 -#include -#include - -#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 delta( - Eigen::Matrix::Zero(), covariance); - - kalman_filter_.Observe<6>( - [this, &pose](const State& state) -> Eigen::Matrix { - 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 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 diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h deleted file mode 100644 index cc5d465..0000000 --- a/cartographer/kalman_filter/pose_tracker.h +++ /dev/null @@ -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 -#include - -#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 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; - using State = KalmanFilter::StateType; - using StateCovariance = Eigen::Matrix; - using Distribution = GaussianDistribution; - - // 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_ diff --git a/cartographer/kalman_filter/pose_tracker_test.cc b/cartographer/kalman_filter/pose_tracker_test.cc deleted file mode 100644 index 290a028..0000000 --- a/cartographer/kalman_filter/pose_tracker_test.cc +++ /dev/null @@ -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 - -#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(options, common::FromUniversal(1000)); - } - - std::unique_ptr pose_tracker_; -}; - -TEST_F(PoseTrackerTest, SaveAndRestore) { - std::vector 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::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::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 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 diff --git a/cartographer/kalman_filter/proto/pose_tracker_options.proto b/cartographer/kalman_filter/proto/pose_tracker_options.proto deleted file mode 100644 index 6b6fa87..0000000 --- a/cartographer/kalman_filter/proto/pose_tracker_options.proto +++ /dev/null @@ -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; -} diff --git a/cartographer/kalman_filter/unscented_kalman_filter.h b/cartographer/kalman_filter/unscented_kalman_filter.h deleted file mode 100644 index d62c1b1..0000000 --- a/cartographer/kalman_filter/unscented_kalman_filter.h +++ /dev/null @@ -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 -#include -#include -#include - -#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 -constexpr FloatType sqr(FloatType a) { - return a * a; -} - -template -Eigen::Matrix OuterProduct( - const Eigen::Matrix& v) { - return v * v.transpose(); -} - -// Checks if 'A' is a symmetric matrix. -template -void CheckSymmetric(const Eigen::Matrix& 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 -Eigen::Matrix MatrixSqrt( - const Eigen::Matrix& A) { - CheckSymmetric(A); - - Eigen::SelfAdjointEigenSolver> - 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::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 -class UnscentedKalmanFilter { - public: - using StateType = Eigen::Matrix; - using StateCovarianceType = Eigen::Matrix; - - explicit UnscentedKalmanFilter( - const GaussianDistribution& initial_belief, - std::function - add_delta = [](const StateType& state, - const StateType& delta) { return state + delta; }, - std::function - 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 g, - const GaussianDistribution& 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 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(compute_delta_(new_mu, Y[0])); - for (int i = 0; i < N; ++i) { - new_sigma += kCovWeightI * OuterProduct( - compute_delta_(new_mu, Y[2 * i + 1])); - new_sigma += kCovWeightI * OuterProduct( - compute_delta_(new_mu, Y[2 * i + 2])); - } - CheckSymmetric(new_sigma); - - belief_ = GaussianDistribution(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 - void Observe( - std::function(const StateType&)> h, - const GaussianDistribution& 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 W; - W.reserve(2 * N + 1); - W.emplace_back(StateType::Zero()); - - std::vector> Z; - Z.reserve(2 * N + 1); - Z.emplace_back(h(mu)); - - Eigen::Matrix 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 S = - kCovWeight0 * OuterProduct(Z[0] - z_hat); - for (int i = 0; i < N; ++i) { - S += kCovWeightI * OuterProduct(Z[2 * i + 1] - z_hat); - S += kCovWeightI * OuterProduct(Z[2 * i + 2] - z_hat); - } - CheckSymmetric(S); - S += delta.GetCovariance(); - - Eigen::Matrix 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 kalman_gain = - sigma_bar_xz * S.inverse(); - const StateCovarianceType new_sigma = - belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose(); - CheckSymmetric(new_sigma); - - belief_ = GaussianDistribution( - add_delta_(mu, kalman_gain * -z_hat), new_sigma); - } - - const GaussianDistribution& GetBelief() const { - return belief_; - } - - private: - StateType ComputeWeightedError(const StateType& mean_estimate, - const std::vector& 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& 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 belief_; - const std::function - add_delta_; - const std::function - compute_delta_; -}; - -template -constexpr FloatType UnscentedKalmanFilter::kAlpha; -template -constexpr FloatType UnscentedKalmanFilter::kKappa; -template -constexpr FloatType UnscentedKalmanFilter::kBeta; -template -constexpr FloatType UnscentedKalmanFilter::kLambda; -template -constexpr FloatType UnscentedKalmanFilter::kMeanWeight0; -template -constexpr FloatType UnscentedKalmanFilter::kCovWeight0; -template -constexpr FloatType UnscentedKalmanFilter::kMeanWeightI; -template -constexpr FloatType UnscentedKalmanFilter::kCovWeightI; - -} // namespace kalman_filter -} // namespace cartographer - -#endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_ diff --git a/cartographer/kalman_filter/unscented_kalman_filter_test.cc b/cartographer/kalman_filter/unscented_kalman_filter_test.cc deleted file mode 100644 index 08d3658..0000000 --- a/cartographer/kalman_filter/unscented_kalman_filter_test.cc +++ /dev/null @@ -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 Scalar(double value) { - return value * Eigen::Matrix::Identity(); -} - -// A simple model that copies the first to the second state variable. -Eigen::Matrix g(const Eigen::Matrix& state) { - Eigen::Matrix new_state; - new_state << state[0], state[0]; - return new_state; -} - -// A simple observation of the first state variable. -Eigen::Matrix h(const Eigen::Matrix& state) { - return Scalar(state[0]) - Scalar(5.); -} - -TEST(KalmanFilterTest, testConstructor) { - UnscentedKalmanFilter filter(GaussianDistribution( - Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); - EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9); -} - -TEST(KalmanFilterTest, testPredict) { - UnscentedKalmanFilter filter(GaussianDistribution( - Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity())); - filter.Predict( - g, GaussianDistribution(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 filter(GaussianDistribution( - Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); - for (int i = 0; i < 500; ++i) { - filter.Predict( - g, GaussianDistribution(Eigen::Vector2d(0., 0.), - Eigen::Matrix2d::Identity() * 1e-9)); - filter.Observe<1>( - h, GaussianDistribution(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 diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index a6ac95b..bdf1fd3 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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; diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index f16504a..6ca1c0c 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/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 diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc deleted file mode 100644 index c6df383..0000000 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc +++ /dev/null @@ -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 diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h deleted file mode 100644 index 4ec7a6e..0000000 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h +++ /dev/null @@ -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_ diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 07238ea..e4b1bce 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/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( - options_.kalman_local_trajectory_builder_options() - .real_time_correlative_scan_matcher_options())), + options_.real_time_correlative_scan_matcher_options())), ceres_scan_matcher_(common::make_unique( options_.ceres_scan_matcher_options())), - odometry_state_tracker_(options_.kalman_local_trajectory_builder_options() - .pose_tracker_options() - .num_odometry_states()), + odometry_state_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( - options_.kalman_local_trajectory_builder_options() - .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()); + options_.imu_gravity_time_constant(), time); } - 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::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() - .use_online_correlative_scan_matching()) { + if (options_.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; diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 8306110..818fc29 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -20,8 +20,9 @@ #include #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" diff --git a/cartographer/mapping_3d/local_trajectory_builder_options.cc b/cartographer/mapping_3d/local_trajectory_builder_options.cc index 637d702..fb692f0 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options.cc +++ b/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; } diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index 0688b70..ec455ff 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/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()); diff --git a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto deleted file mode 100644 index b4be184..0000000 --- a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto +++ /dev/null @@ -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; -} diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto index 782e05d..36b9745 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto +++ b/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 - kalman_local_trajectory_builder_options = 10; + // 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. + 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; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index e995676..f6767ab 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/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, - [=]() EXCLUDES(mutex_) { + submap_id, submap_nodes, submap, [=]() 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, - [=]() EXCLUDES(mutex_) { + submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint( submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, compressed_point_cloud, @@ -232,13 +229,12 @@ void ConstraintBuilder::ComputeConstraint( ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; - ceres_scan_matcher_.Match( - pose_estimate, pose_estimate, - {{&high_resolution_point_cloud, - submap_scan_matcher->high_resolution_hybrid_grid}, - {&low_resolution_point_cloud, - submap_scan_matcher->low_resolution_hybrid_grid}}, - &constraint_transform, &unused_summary); + ceres_scan_matcher_.Match(pose_estimate, pose_estimate, + {{&high_resolution_point_cloud, + submap_scan_matcher->high_resolution_hybrid_grid}, + {&low_resolution_point_cloud, + submap_scan_matcher->low_resolution_hybrid_grid}}, + &constraint_transform, &unused_summary); constraint->reset(new OptimizationProblem::Constraint{ submap_id, diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index ab3d060..7adbdf5 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/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& submap_nodes, - const Submap* submap, std::function work_item) - REQUIRES(mutex_); + const Submap* submap, std::function work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 4a5b98e..8b06f50 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/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.), diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 825f392..d371d14 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/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, - }, } diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 1b2f851..694383d 100644 --- a/docs/source/configuration.rst +++ b/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 - Not yet documented. +double imu_gravity_time_constant + 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.