From 574a56bbbca71ebf9db735cc75de070b14d4e934 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 23 Nov 2016 12:37:55 +0100 Subject: [PATCH] Remove the UKF from local 2D SLAM. (#140) Changes default configuration to log loop closure matches. --- cartographer/mapping_2d/CMakeLists.txt | 3 +- .../mapping_2d/local_trajectory_builder.cc | 178 +++++++++++------- .../mapping_2d/local_trajectory_builder.h | 24 ++- cartographer/mapping_2d/proto/CMakeLists.txt | 1 - .../local_trajectory_builder_options.proto | 17 +- cartographer/sensor/CMakeLists.txt | 1 + cartographer/transform/rigid_transform.h | 2 +- cartographer/transform/transform.h | 17 +- configuration_files/sparse_pose_graph.lua | 2 +- configuration_files/trajectory_builder_2d.lua | 22 +-- 10 files changed, 160 insertions(+), 107 deletions(-) diff --git a/cartographer/mapping_2d/CMakeLists.txt b/cartographer/mapping_2d/CMakeLists.txt index 5070487..2e74a1b 100644 --- a/cartographer/mapping_2d/CMakeLists.txt +++ b/cartographer/mapping_2d/CMakeLists.txt @@ -54,13 +54,14 @@ google_library(mapping_2d_local_trajectory_builder common_lua_parameter_dictionary common_make_unique common_time - kalman_filter_pose_tracker mapping_2d_proto_local_trajectory_builder_options mapping_2d_scan_matching_ceres_scan_matcher mapping_2d_scan_matching_real_time_correlative_scan_matcher mapping_2d_submaps mapping_3d_motion_filter mapping_global_trajectory_builder_interface + mapping_imu_tracker + mapping_odometry_state_tracker sensor_configuration sensor_laser sensor_voxel_filter diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index dc54329..1b317e6 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -53,16 +53,14 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( *options.mutable_motion_filter_options() = mapping_3d::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); - *options.mutable_pose_tracker_options() = - kalman_filter::CreatePoseTrackerOptions( - parameter_dictionary->GetDictionary("pose_tracker").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() = CreateSubmapsOptions( parameter_dictionary->GetDictionary("submaps").get()); options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); - options.set_odometer_translational_variance( - parameter_dictionary->GetDouble("odometer_translational_variance")); - options.set_odometer_rotational_variance( - parameter_dictionary->GetDouble("odometer_rotational_variance")); return options; } @@ -70,11 +68,11 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), submaps_(options.submaps_options()), - scan_matcher_pose_estimate_(transform::Rigid3d::Identity()), motion_filter_(options_.motion_filter_options()), real_time_correlative_scan_matcher_( options_.real_time_correlative_scan_matcher_options()), - ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {} + ceres_scan_matcher_(options_.ceres_scan_matcher_options()), + odometry_state_tracker_(options_.num_odometry_states()) {} LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} @@ -135,45 +133,42 @@ void LocalTrajectoryBuilder::ScanMatch( transform::Rigid2d tracking_2d_to_map; kalman_filter::Pose2DCovariance covariance_observation_2d; ceres::Solver::Summary summary; - ceres_scan_matcher_.Match( - transform::Project2D(scan_matcher_pose_estimate_ * - tracking_to_tracking_2d.inverse()), - initial_ceres_pose, filtered_point_cloud_in_tracking_2d, probability_grid, - &tracking_2d_to_map, &covariance_observation_2d, &summary); + ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose, + filtered_point_cloud_in_tracking_2d, + probability_grid, &tracking_2d_to_map, + &covariance_observation_2d, &summary); - CHECK(pose_tracker_ != nullptr); - - *pose_observation = transform::Embed3D(tracking_2d_to_map); + *pose_observation = + transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d; // This covariance is used for non-yaw rotation and the fake height of 0. constexpr double kFakePositionCovariance = 1.; constexpr double kFakeOrientationCovariance = 1.; *covariance_observation = kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance, kFakeOrientationCovariance); - pose_tracker_->AddPoseObservation( - time, (*pose_observation) * tracking_to_tracking_2d, - *covariance_observation); } std::unique_ptr LocalTrajectoryBuilder::AddHorizontalLaserFan( const common::Time time, const sensor::LaserFan& laser_fan) { - // Initialize pose tracker now if we do not ever use an IMU. + // Initialize IMU tracker now if we do not ever use an IMU. if (!options_.use_imu_data()) { - InitializePoseTracker(time); + InitializeImuTracker(time); } - if (pose_tracker_ == nullptr) { - // Until we've initialized the UKF with our first IMU message, we cannot - // compute the orientation of the laser scanner. - LOG(INFO) << "PoseTracker not yet initialized."; + if (imu_tracker_ == nullptr) { + // Until we've initialized the IMU tracker with our first IMU message, we + // cannot compute the orientation of the laser scanner. + LOG(INFO) << "ImuTracker not yet initialized."; return nullptr; } - transform::Rigid3d pose_prediction; - kalman_filter::PoseCovariance covariance_prediction; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction, - &covariance_prediction); + Predict(time); + const transform::Rigid3d odometry_prediction = + pose_estimate_ * odometry_correction_; + const transform::Rigid3d model_prediction = pose_estimate_; + // TODO(whess): Prefer IMU over odom orientation if configured? + const transform::Rigid3d pose_prediction = odometry_prediction; // Computes the rotation without yaw, as defined by GetYaw(). const transform::Rigid3d tracking_to_tracking_2d = @@ -190,30 +185,42 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( return nullptr; } - transform::Rigid3d pose_observation; kalman_filter::PoseCovariance covariance_observation; ScanMatch(time, pose_prediction, tracking_to_tracking_2d, - laser_fan_in_tracking_2d, &pose_observation, - &covariance_observation); + laser_fan_in_tracking_2d, &pose_estimate_, &covariance_observation); + odometry_correction_ = transform::Rigid3d::Identity(); + if (!odometry_state_tracker_.empty()) { + // We add an odometry state, so that the correction from the scan matching + // is not removed by the next odometry data we get. + odometry_state_tracker_.AddOdometryState( + {time, odometry_state_tracker_.newest().odometer_pose, + odometry_state_tracker_.newest().state_pose * + odometry_prediction.inverse() * pose_estimate_}); + } - kalman_filter::PoseCovariance covariance_estimate; - pose_tracker_->GetPoseEstimateMeanAndCovariance( - time, &scan_matcher_pose_estimate_, &covariance_estimate); + // Improve the velocity estimate. + if (last_scan_match_time_ > common::Time::min()) { + const double delta_t = common::ToSeconds(time - last_scan_match_time_); + velocity_estimate_ += (pose_estimate_.translation().head<2>() - + model_prediction.translation().head<2>()) / + delta_t; + } + last_scan_match_time_ = time_; // Remove the untracked z-component which floats around 0 in the UKF. - const auto translation = scan_matcher_pose_estimate_.translation(); - scan_matcher_pose_estimate_ = transform::Rigid3d( + const auto translation = pose_estimate_.translation(); + pose_estimate_ = transform::Rigid3d( transform::Rigid3d::Vector(translation.x(), translation.y(), 0.), - scan_matcher_pose_estimate_.rotation()); + pose_estimate_.rotation()); const transform::Rigid3d tracking_2d_to_map = - scan_matcher_pose_estimate_ * tracking_to_tracking_2d.inverse(); + pose_estimate_ * tracking_to_tracking_2d.inverse(); last_pose_estimate_ = { time, - {pose_prediction, covariance_prediction}, - {pose_observation, covariance_observation}, - {scan_matcher_pose_estimate_, covariance_estimate}, - scan_matcher_pose_estimate_, + {pose_prediction, covariance_observation}, + {pose_estimate_, covariance_observation}, + {pose_estimate_, covariance_observation}, + pose_estimate_, sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns, tracking_2d_to_map.cast())}; @@ -229,13 +236,14 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } - submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d, - tracking_2d_to_map.cast())); + submaps_.InsertLaserFan( + TransformLaserFan(laser_fan_in_tracking_2d, + transform::Embed3D(pose_estimate_2d.cast()))); return common::make_unique(InsertionResult{ time, &submaps_, matching_submap, insertion_submaps, tracking_to_tracking_2d, tracking_2d_to_map, laser_fan_in_tracking_2d, - pose_estimate_2d, covariance_estimate}); + pose_estimate_2d, covariance_observation}); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& @@ -248,19 +256,15 @@ void LocalTrajectoryBuilder::AddImuData( const Eigen::Vector3d& angular_velocity) { CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; - InitializePoseTracker(time); - pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); - pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity); - - transform::Rigid3d pose_estimate; - kalman_filter::PoseCovariance unused_covariance_estimate; - pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate, - &unused_covariance_estimate); + InitializeImuTracker(time); + Predict(time); + imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); + imu_tracker_->AddImuAngularVelocityObservation(angular_velocity); // Log a warning if the backpack inclination exceeds 20 degrees. In these // cases, it's very likely that 2D SLAM will fail. const Eigen::Vector3d gravity_direction = - Eigen::Quaterniond(pose_estimate.rotation()) * Eigen::Vector3d::UnitZ(); + imu_tracker_->orientation() * Eigen::Vector3d::UnitZ(); const double inclination = std::acos(gravity_direction.z()); constexpr double kMaxInclination = common::DegToRad(20.); LOG_IF_EVERY_N(WARNING, inclination > kMaxInclination, 1000) @@ -268,26 +272,56 @@ void LocalTrajectoryBuilder::AddImuData( << common::RadToDeg(kMaxInclination); } -void LocalTrajectoryBuilder::AddOdometerData(const common::Time time, - const transform::Rigid3d& pose) { - if (pose_tracker_ == nullptr) { - // Until we've initialized the UKF with our first IMU message, we cannot - // process odometer poses. - LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized."; - } else { - pose_tracker_->AddOdometerPoseObservation( - time, pose, kalman_filter::BuildPoseCovariance( - options_.odometer_translational_variance(), - options_.odometer_rotational_variance())); +void LocalTrajectoryBuilder::AddOdometerData( + const common::Time time, const transform::Rigid3d& odometer_pose) { + if (imu_tracker_ == nullptr) { + // Until we've initialized the IMU tracker we do not want to call Predict(). + LOG(INFO) << "ImuTracker not yet initialized."; + return; + } + + Predict(time); + 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; + odometry_correction_ = pose_estimate_.inverse() * new_pose; + } + odometry_state_tracker_.AddOdometryState( + {time, odometer_pose, pose_estimate_ * odometry_correction_}); +} + +void LocalTrajectoryBuilder::InitializeImuTracker(const common::Time time) { + if (imu_tracker_ == nullptr) { + imu_tracker_ = common::make_unique( + options_.imu_gravity_time_constant(), time); } } -void LocalTrajectoryBuilder::InitializePoseTracker(const common::Time time) { - if (pose_tracker_ == nullptr) { - pose_tracker_ = common::make_unique( - options_.pose_tracker_options(), - kalman_filter::PoseTracker::ModelFunction::k2D, time); +void LocalTrajectoryBuilder::Predict(const common::Time time) { + CHECK(imu_tracker_ != nullptr); + CHECK_LE(time_, time); + const double last_yaw = transform::GetYaw(imu_tracker_->orientation()); + imu_tracker_->Advance(time); + if (time_ > common::Time::min()) { + const double delta_t = common::ToSeconds(time - time_); + // Constant velocity model. + const Eigen::Vector3d translation = + pose_estimate_.translation() + + delta_t * + Eigen::Vector3d(velocity_estimate_.x(), velocity_estimate_.y(), 0.); + // Use the current IMU tracker roll and pitch for gravity alignment, and + // apply its change in yaw. + const Eigen::Quaterniond rotation = + Eigen::AngleAxisd( + transform::GetYaw(pose_estimate_.rotation()) - last_yaw, + Eigen::Vector3d::UnitZ()) * + imu_tracker_->orientation(); + pose_estimate_ = transform::Rigid3d(translation, rotation); } + time_ = time; } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index a594cf9..ee9c6b9 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -21,8 +21,9 @@ #include "cartographer/common/lua_parameter_dictionary.h" #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_2d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" @@ -84,22 +85,33 @@ class LocalTrajectoryBuilder { transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation); - // Lazily constructs a PoseTracker. - void InitializePoseTracker(common::Time time); + // Lazily constructs an ImuTracker. + void InitializeImuTracker(common::Time time); + + // Updates the current estimate to reflect the given 'time'. + void Predict(common::Time time); const proto::LocalTrajectoryBuilderOptions options_; Submaps submaps_; mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_; - // Pose of the last computed scan match. - transform::Rigid3d scan_matcher_pose_estimate_; + // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'. + common::Time time_ = common::Time::min(); + transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity(); + Eigen::Vector2d velocity_estimate_ = Eigen::Vector2d::Zero(); + common::Time last_scan_match_time_ = common::Time::min(); + // This is the difference between the model (constant velocity, IMU) + // prediction 'pose_estimate_' and the odometry prediction. To get the + // odometry prediction, right-multiply this to 'pose_estimate_'. + transform::Rigid3d odometry_correction_ = transform::Rigid3d::Identity(); mapping_3d::MotionFilter motion_filter_; scan_matching::RealTimeCorrelativeScanMatcher real_time_correlative_scan_matcher_; scan_matching::CeresScanMatcher ceres_scan_matcher_; - std::unique_ptr pose_tracker_; + std::unique_ptr imu_tracker_; + mapping::OdometryStateTracker odometry_state_tracker_; }; } // namespace mapping_2d diff --git a/cartographer/mapping_2d/proto/CMakeLists.txt b/cartographer/mapping_2d/proto/CMakeLists.txt index 3a6342c..e5b23f6 100644 --- a/cartographer/mapping_2d/proto/CMakeLists.txt +++ b/cartographer/mapping_2d/proto/CMakeLists.txt @@ -21,7 +21,6 @@ google_proto_library(mapping_2d_proto_local_trajectory_builder_options SRCS local_trajectory_builder_options.proto DEPENDS - kalman_filter_proto_pose_tracker_options mapping_2d_proto_submaps_options mapping_2d_scan_matching_proto_ceres_scan_matcher_options mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index eca575d..03db854 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto @@ -16,7 +16,6 @@ syntax = "proto2"; package cartographer.mapping_2d.proto; -import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; import "cartographer/mapping_3d/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/proto/submaps_options.proto"; @@ -51,12 +50,20 @@ message LocalTrajectoryBuilderOptions { optional scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 8; optional mapping_3d.proto.MotionFilterOptions motion_filter_options = 13; - optional kalman_filter.proto.PoseTrackerOptions pose_tracker_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 = 17; + + // Maximum number of previous odometry states to keep. + optional int32 num_odometry_states = 18; + optional mapping_2d.proto.SubmapsOptions submaps_options = 11; // True if IMU data should be expected and used. optional bool use_imu_data = 12; - - optional double odometer_translational_variance = 17; - optional double odometer_rotational_variance = 18; } diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index cc380ed..74330df 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -60,6 +60,7 @@ google_library(sensor_data common_time kalman_filter_pose_tracker sensor_laser + sensor_point_cloud transform_rigid_transform ) diff --git a/cartographer/transform/rigid_transform.h b/cartographer/transform/rigid_transform.h index 98ec7e9..8db3daf 100644 --- a/cartographer/transform/rigid_transform.h +++ b/cartographer/transform/rigid_transform.h @@ -201,7 +201,7 @@ Rigid3 operator*(const Rigid3& lhs, const Rigid3& rhs) { return Rigid3( lhs.rotation() * rhs.translation() + lhs.translation(), - lhs.rotation() * rhs.rotation()); + (lhs.rotation() * rhs.rotation()).normalized()); } template diff --git a/cartographer/transform/transform.h b/cartographer/transform/transform.h index 8c2ba76..cac08bd 100644 --- a/cartographer/transform/transform.h +++ b/cartographer/transform/transform.h @@ -36,14 +36,21 @@ FloatType GetAngle(const Rigid3& transform) { std::abs(transform.rotation().w())); } +// Returns the yaw component in radians of the given 3D 'rotation'. Assuming +// 'rotation' is composed of three rotations around X, then Y, then Z, returns +// the angle of the Z rotation. +template +T GetYaw(const Eigen::Quaternion& rotation) { + const Eigen::Matrix direction = + rotation * Eigen::Matrix::UnitX(); + return atan2(direction.y(), direction.x()); +} + // Returns the yaw component in radians of the given 3D transformation -// 'transform'. Assuming three rotations around X, then Y, then Z axis resulted -// in the rotational component, the yaw is the angle of the Z rotation. +// 'transform'. template T GetYaw(const Rigid3& transform) { - const Eigen::Matrix direction = - transform.rotation() * Eigen::Matrix::UnitX(); - return atan2(direction.y(), direction.x()); + return GetYaw(transform.rotation()); } // Returns an angle-axis vector (a vector with the length of the rotation angle diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 16d4269..84ecb04 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -25,7 +25,7 @@ SPARSE_POSE_GRAPH = { min_score = 0.55, global_localization_min_score = 0.6, lower_covariance_eigenvalue_bound = 1e-11, - log_matches = false, + log_matches = true, fast_correlative_scan_matcher = { linear_search_window = 7., angular_search_window = math.rad(30.), diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 066b89a..3e1e33e 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -20,8 +20,6 @@ TRAJECTORY_BUILDER_2D = { laser_max_z = 2., laser_missing_echo_ray_length = 5., laser_voxel_filter_size = 0.025, - odometer_translational_variance = 1e-7, - odometer_rotational_variance = 1e-7, use_online_correlative_scan_matching = false, adaptive_voxel_filter = { @@ -38,13 +36,13 @@ TRAJECTORY_BUILDER_2D = { }, ceres_scan_matcher = { - occupied_space_weight = 20., - translation_weight = 1., + occupied_space_weight = 1e1, + translation_weight = 1e1, rotation_weight = 1e2, - covariance_scale = 2.34e-4, + covariance_scale = 1e-2, ceres_solver_options = { - use_nonmonotonic_steps = true, - max_num_iterations = 50, + use_nonmonotonic_steps = false, + max_num_iterations = 20, num_threads = 1, }, }, @@ -55,14 +53,8 @@ TRAJECTORY_BUILDER_2D = { max_angle_radians = math.rad(1.), }, - pose_tracker = { - orientation_model_variance = 5e-4, - position_model_variance = 0.000654766, - velocity_model_variance = 0.053926, - imu_gravity_time_constant = 10., - imu_gravity_variance = 1e-6, - num_odometry_states = 1000, - }, + imu_gravity_time_constant = 10., + num_odometry_states = 1000, submaps = { resolution = 0.05,