From b2175f0d939896637c6a18dd6ecd957492b4cf15 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 8 Feb 2017 15:22:28 +0100 Subject: [PATCH] Remove covariance from the 3D Ceres scan matcher. (#200) Instead of passing around fake covariances generated by the scan matcher, we introduce appropriate values where they are needed. --- cartographer/mapping_2d/sparse_pose_graph_test.cc | 1 - .../mapping_3d/kalman_local_trajectory_builder.cc | 11 ++++++----- .../kalman_local_trajectory_builder_options.cc | 2 ++ .../kalman_local_trajectory_builder_test.cc | 2 +- .../kalman_local_trajectory_builder_options.proto | 1 + .../mapping_3d/scan_matching/ceres_scan_matcher.cc | 7 ------- .../mapping_3d/scan_matching/ceres_scan_matcher.h | 4 +--- .../scan_matching/ceres_scan_matcher_test.cc | 4 +--- .../proto/ceres_scan_matcher_options.proto | 3 --- .../sparse_pose_graph/constraint_builder.cc | 12 ++++-------- configuration_files/sparse_pose_graph.lua | 1 - configuration_files/trajectory_builder_3d.lua | 2 +- docs/source/configuration.rst | 6 +++--- 13 files changed, 20 insertions(+), 36 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index d589924..d46b85e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -109,7 +109,6 @@ class SparsePoseGraphTest : public ::testing::Test { occupied_space_weight_0 = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1., only_optimize_yaw = true, ceres_solver_options = { use_nonmonotonic_steps = true, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 8299a2c..bf6e023 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -154,7 +154,6 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( } transform::Rigid3d pose_observation; - kalman_filter::PoseCovariance covariance_observation; ceres::Solver::Summary summary; sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( @@ -166,10 +165,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan( &submaps_->high_resolution_matching_grid()}, {&low_resolution_point_cloud_in_tracking, &submaps_->low_resolution_matching_grid()}}, - &pose_observation, &covariance_observation, - &summary); - pose_tracker_->AddPoseObservation(time, pose_observation, - covariance_observation); + &pose_observation, &summary); + pose_tracker_->AddPoseObservation( + time, pose_observation, + options_.kalman_local_trajectory_builder_options() + .scan_matcher_variance() * + kalman_filter::PoseCovariance::Identity()); kalman_filter::PoseCovariance covariance_estimate; pose_tracker_->GetPoseEstimateMeanAndCovariance( diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc index 0a1c97a..c6df383 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc @@ -36,6 +36,8 @@ CreateKalmanLocalTrajectoryBuilderOptions( *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( diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index a1fd5e0..d251f92 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -69,7 +69,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { occupied_space_weight_1 = 20., translation_weight = 0.1, rotation_weight = 0.3, - covariance_scale = 1e-1, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = true, @@ -112,6 +111,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { num_odometry_states = 1, }, + scan_matcher_variance = 1e-6, odometer_translational_variance = 1e-7, odometer_rotational_variance = 1e-7, }, diff --git a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto index 2a28f04..b4be184 100644 --- a/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto @@ -28,6 +28,7 @@ message KalmanLocalTrajectoryBuilderOptions { 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/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index 41ce5cd..f1e75e9 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -67,8 +67,6 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); - options.set_covariance_scale( - parameter_dictionary->GetDouble("covariance_scale")); options.set_only_optimize_yaw( parameter_dictionary->GetBool("only_optimize_yaw")); *options.mutable_ceres_solver_options() = @@ -90,7 +88,6 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* const pose_estimate, - kalman_filter::PoseCovariance* const covariance, ceres::Solver::Summary* const summary) { ceres::Problem problem; CeresPose ceres_pose( @@ -136,10 +133,6 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, ceres::Solve(ceres_solver_options_, &problem, summary); *pose_estimate = ceres_pose.ToRigid(); - - // TODO(whess): Remove once the UKF is gone. - *covariance = 1e-5 * options_.covariance_scale() * - kalman_filter::PoseCovariance::Identity(); } } // namespace scan_matching diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h index d12ea7f..15724c8 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h @@ -22,7 +22,6 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" @@ -47,14 +46,13 @@ class CeresScanMatcher { CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_clouds' within the 'hybrid_grids' given an - // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and + // 'initial_pose_estimate' and returns 'pose_estimate', and // the solver 'summary'. void Match(const transform::Rigid3d& previous_pose, const transform::Rigid3d& initial_pose_estimate, const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* pose_estimate, - kalman_filter::PoseCovariance* covariance, ceres::Solver::Summary* summary); private: diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc index 11f61d2..939209f 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc @@ -53,7 +53,6 @@ class CeresScanMatcherTest : public ::testing::Test { occupied_space_weight_0 = 1., translation_weight = 0.01, rotation_weight = 0.1, - covariance_scale = 10., only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = true, @@ -67,12 +66,11 @@ class CeresScanMatcherTest : public ::testing::Test { void TestFromInitialPose(const transform::Rigid3d& initial_pose) { transform::Rigid3d pose; - kalman_filter::PoseCovariance covariance; ceres::Solver::Summary summary; ceres_scan_matcher_->Match(initial_pose, initial_pose, {{&point_cloud_, &hybrid_grid_}}, &pose, - &covariance, &summary); + &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } diff --git a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto index 1e71a6e..a17ca86 100644 --- a/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto @@ -25,9 +25,6 @@ message CeresScanMatcherOptions { optional double translation_weight = 2; optional double rotation_weight = 3; - // Scale applied to the covariance estimate from Ceres. - optional double covariance_scale = 4; - // Whether only to allow changes to yaw, keeping roll/pitch constant. optional bool only_optimize_yaw = 5; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 24aca59..7b3d0c0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -250,12 +250,10 @@ void ConstraintBuilder::ComputeConstraint( // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; - kalman_filter::PoseCovariance covariance; ceres_scan_matcher_.Match( pose_estimate, pose_estimate, {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, - &pose_estimate, &covariance, &unused_summary); - // 'covariance' is unchanged as (submap <- map) is a translation. + &pose_estimate, &unused_summary); const transform::Rigid3d constraint_transform = submap->local_pose().inverse() * pose_estimate; @@ -263,8 +261,8 @@ void ConstraintBuilder::ComputeConstraint( submap_index, scan_index, {constraint_transform, - common::ComputeSpdMatrixSqrtInverse( - covariance, options_.lower_covariance_eigenvalue_bound())}, + 1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) * + kalman_filter::PoseCovariance::Identity()}, OptimizationProblem::Constraint::INTER_SUBMAP}); if (options_.log_matches()) { @@ -276,9 +274,7 @@ void ConstraintBuilder::ComputeConstraint( << " differs by translation " << std::fixed << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << transform::GetAngle(difference) - << " with score " << std::setprecision(1) << 100. * score - << "% covariance trace " << std::scientific << std::setprecision(4) - << covariance.trace() << "."; + << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } } diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 9a35254..7dcb784 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -55,7 +55,6 @@ SPARSE_POSE_GRAPH = { occupied_space_weight_0 = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1e-6, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = false, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index cc61e99..bd795ce 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -37,7 +37,6 @@ TRAJECTORY_BUILDER_3D = { occupied_space_weight_1 = 30., translation_weight = 0.3, rotation_weight = 2e3, - covariance_scale = 2.34e-4, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = false, @@ -84,6 +83,7 @@ TRAJECTORY_BUILDER_3D = { 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 e521963..3100096 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -324,6 +324,9 @@ cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOption 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. @@ -422,9 +425,6 @@ double translation_weight double rotation_weight Not yet documented. -double covariance_scale - Scale applied to the covariance estimate from Ceres. - bool only_optimize_yaw Whether only to allow changes to yaw, keeping roll/pitch constant.