From a2e52f81cfde4e3a4a6e751af4eb2d9c51354c36 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 31 May 2017 11:56:32 +0200 Subject: [PATCH] Remove 2D scan matcher covariances. (#307) This replaces the scaled covariances derived from the Ceres scan matcher by directly configurable weights. Using covariances did not provide the expected benefit, and replacing the scaling matrix by two values will allow a faster evaluation of the cost function in the future. --- .../proto/sparse_pose_graph_options.proto | 5 ++++ cartographer/mapping/sparse_pose_graph.cc | 19 +++++++++++- cartographer/mapping/sparse_pose_graph.h | 4 +++ .../sparse_pose_graph/constraint_builder.cc | 4 +++ .../proto/constraint_builder_options.proto | 5 ++++ .../mapping_2d/global_trajectory_builder.cc | 6 ++-- .../mapping_2d/local_trajectory_builder.cc | 19 +++--------- .../mapping_2d/local_trajectory_builder.h | 8 ++--- .../scan_matching/ceres_scan_matcher.cc | 15 ---------- .../scan_matching/ceres_scan_matcher.h | 6 ++-- .../scan_matching/ceres_scan_matcher_test.cc | 4 +-- ...real_time_correlative_scan_matcher_test.cc | 1 - cartographer/mapping_2d/sparse_pose_graph.cc | 29 +++++++------------ cartographer/mapping_2d/sparse_pose_graph.h | 10 +++---- .../sparse_pose_graph/constraint_builder.cc | 28 +++++++----------- .../mapping_2d/sparse_pose_graph_test.cc | 14 ++++----- .../scan_matching/ceres_scan_matcher.h | 4 +-- configuration_files/sparse_pose_graph.lua | 5 +++- configuration_files/trajectory_builder_2d.lua | 1 - 19 files changed, 86 insertions(+), 101 deletions(-) diff --git a/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/mapping/proto/sparse_pose_graph_options.proto index 7436231..dc7e8a3 100644 --- a/cartographer/mapping/proto/sparse_pose_graph_options.proto +++ b/cartographer/mapping/proto/sparse_pose_graph_options.proto @@ -28,6 +28,11 @@ message SparsePoseGraphOptions { optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options = 3; + // Weights used in the optimization problem for non-loop-closure scan matcher + // constraints. + optional double matcher_translation_weight = 7; + optional double matcher_rotation_weight = 8; + // Options for the optimization problem. optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options = 4; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index a420ae0..0b46a3a 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -16,7 +16,6 @@ #include "cartographer/mapping/sparse_pose_graph.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/transform/transform.h" @@ -44,6 +43,10 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( *options.mutable_constraint_builder_options() = sparse_pose_graph::CreateConstraintBuilderOptions( parameter_dictionary->GetDictionary("constraint_builder").get()); + options.set_matcher_translation_weight( + parameter_dictionary->GetDouble("matcher_translation_weight")); + options.set_matcher_rotation_weight( + parameter_dictionary->GetDouble("matcher_rotation_weight")); *options.mutable_optimization_problem_options() = sparse_pose_graph::CreateOptimizationProblemOptions( parameter_dictionary->GetDictionary("optimization_problem").get()); @@ -55,6 +58,20 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( return options; } +Eigen::Matrix FromTranslationRotationWeights( + const double translation_weight, const double rotation_weight) { + Eigen::Matrix result; + // clang-format off + result << translation_weight, 0., 0., 0., 0., 0., + 0., translation_weight, 0., 0., 0., 0., + 0., 0., translation_weight, 0., 0., 0., + 0., 0., 0., rotation_weight, 0., 0., + 0., 0., 0., 0., rotation_weight, 0., + 0., 0., 0., 0., 0., rotation_weight; + // clang-format on + return result; +} + proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 280172b..83915d6 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -35,6 +35,10 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); +// TODO(whess): Change to two doubles for performance. +Eigen::Matrix FromTranslationRotationWeights( + double translation_weight, double rotation_weight); + class SparsePoseGraph { public: // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index fa11215..273a3c3 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -40,6 +40,10 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( parameter_dictionary->GetDouble("global_localization_min_score")); options.set_lower_covariance_eigenvalue_bound( parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound")); + options.set_loop_closure_translation_weight( + parameter_dictionary->GetDouble("loop_closure_translation_weight")); + options.set_loop_closure_rotation_weight( + parameter_dictionary->GetDouble("loop_closure_rotation_weight")); options.set_log_matches(parameter_dictionary->GetBool("log_matches")); *options.mutable_fast_correlative_scan_matcher_options() = mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions( diff --git a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto index 4115dbe..8d32849 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -42,8 +42,13 @@ message ConstraintBuilderOptions { optional double global_localization_min_score = 5; // Lower bound for covariance eigenvalues to limit the weight of matches. + // TODO(whess): Deprecated and only used in 3D. Replace usage and remove. optional double lower_covariance_eigenvalue_bound = 7; + // Weights used in the optimization problem for loop closure constraints. + optional double loop_closure_translation_weight = 13; + optional double loop_closure_rotation_weight = 14; + // If enabled, logs information of loop-closing constraints for debugging. optional bool log_matches = 8; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 64aa15a..88c6552 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -42,10 +42,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData( sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->range_data_in_tracking_2d, - insertion_result->pose_estimate_2d, - kalman_filter::Project2D(insertion_result->covariance_estimate), - trajectory_id_, insertion_result->matching_submap, - insertion_result->insertion_submaps); + insertion_result->pose_estimate_2d, trajectory_id_, + insertion_result->matching_submap, insertion_result->insertion_submaps); } } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index a128b95..5b1f9aa 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -69,8 +69,7 @@ void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, - transform::Rigid3d* pose_observation, - kalman_filter::PoseCovariance* covariance_observation) { + transform::Rigid3d* pose_observation) { const ProbabilityGrid& probability_grid = submaps_.Get(submaps_.matching_index())->probability_grid; transform::Rigid2d pose_prediction_2d = @@ -89,21 +88,13 @@ void LocalTrajectoryBuilder::ScanMatch( } transform::Rigid2d tracking_2d_to_map; - kalman_filter::Pose2DCovariance covariance_observation_2d; ceres::Solver::Summary 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); + probability_grid, &tracking_2d_to_map, &summary); *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); } std::unique_ptr @@ -144,10 +135,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return nullptr; } - kalman_filter::PoseCovariance covariance_observation; ScanMatch(time, pose_prediction, tracking_to_tracking_2d, - range_data_in_tracking_2d, &pose_estimate_, - &covariance_observation); + range_data_in_tracking_2d, &pose_estimate_); odometry_correction_ = transform::Rigid3d::Identity(); if (!odometry_state_tracker_.empty()) { // We add an odometry state, so that the correction from the scan matching @@ -199,7 +188,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return common::make_unique(InsertionResult{ time, matching_submap, insertion_submaps, tracking_to_tracking_2d, - range_data_in_tracking_2d, pose_estimate_2d, covariance_observation}); + range_data_in_tracking_2d, pose_estimate_2d}); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index fa0cb96..56ba529 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -46,7 +46,6 @@ class LocalTrajectoryBuilder { transform::Rigid3d tracking_to_tracking_2d; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; - kalman_filter::PoseCovariance covariance_estimate; }; explicit LocalTrajectoryBuilder( @@ -71,13 +70,12 @@ class LocalTrajectoryBuilder { const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const; - // Scan match 'range_data_in_tracking_2d' and fill in the - // 'pose_observation' and 'covariance_observation' with the result. + // Scan matches 'range_data_in_tracking_2d' and fill in the 'pose_observation' + // with the result. void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, - transform::Rigid3d* pose_observation, - kalman_filter::PoseCovariance* covariance_observation); + transform::Rigid3d* pose_observation); // Lazily constructs an ImuTracker. void InitializeImuTracker(common::Time time); diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index ae61c52..d55f0b7 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -22,7 +22,6 @@ #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" @@ -44,8 +43,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.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); @@ -67,7 +64,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* const pose_estimate, - kalman_filter::Pose2DCovariance* const covariance, ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), @@ -100,17 +96,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, *pose_estimate = transform::Rigid2d( {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); - - ceres::Covariance::Options options; - ceres::Covariance covariance_computer(options); - std::vector> covariance_blocks; - covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate); - CHECK(covariance_computer.Compute(covariance_blocks, &problem)); - double ceres_covariance[3 * 3]; - covariance_computer.GetCovarianceBlock(ceres_pose_estimate, - ceres_pose_estimate, ceres_covariance); - *covariance = Eigen::Map(ceres_covariance); - *covariance *= options_.covariance_scale(); } } // namespace scan_matching diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h index 3abd49f..955121f 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_2d/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_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" @@ -45,14 +44,13 @@ class CeresScanMatcher { CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an - // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and - // the solver 'summary'. + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. void Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance, ceres::Solver::Summary* summary) const; private: diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 3cde0c4..495ea4f 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -47,7 +47,6 @@ class CeresScanMatcherTest : public ::testing::Test { occupied_space_weight = 1., translation_weight = 0.1, rotation_weight = 1.5, - covariance_scale = 10., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, @@ -61,12 +60,11 @@ class CeresScanMatcherTest : public ::testing::Test { void TestFromInitialPose(const transform::Rigid2d& initial_pose) { transform::Rigid2d pose; - kalman_filter::Pose2DCovariance covariance; const transform::Rigid2d expected_pose = transform::Rigid2d::Translation({-0.5, 0.5}); ceres::Solver::Summary summary; ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_, - probability_grid_, &pose, &covariance, &summary); + probability_grid_, &pose, &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) << "Actual: " << transform::ToProto(pose).DebugString() diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index a78d5a2..004a7f5 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -22,7 +22,6 @@ #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 635ddf3..1da0f24 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -90,8 +90,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id, - const mapping::Submap* const matching_submap, + const int trajectory_id, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); @@ -128,7 +127,7 @@ void SparsePoseGraph::AddScan( AddWorkItem([=]() REQUIRES(mutex_) { ComputeConstraintsForScan(matching_submap, insertion_submaps, - finished_submap, pose, covariance); + finished_submap, pose); }); } @@ -207,8 +206,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( void SparsePoseGraph::ComputeConstraintsForScan( const mapping::Submap* matching_submap, std::vector insertion_submaps, - const mapping::Submap* finished_submap, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance) { + const mapping::Submap* finished_submap, const transform::Rigid2d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid2d optimized_pose = @@ -234,21 +232,16 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id).finished); submap_states_.at(submap_id).node_ids.emplace(node_id); - // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; - constexpr double kFakePositionCovariance = 1e-6; - constexpr double kFakeOrientationCovariance = 1e-6; - constraints_.push_back(Constraint{ - submap_id, - node_id, - {transform::Embed3D(constraint_transform), - common::ComputeSpdMatrixSqrtInverse( - kalman_filter::Embed3D(covariance, kFakePositionCovariance, - kFakeOrientationCovariance), - options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint::INTRA_SUBMAP}); + constraints_.push_back( + Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + mapping::FromTranslationRotationWeights( + options_.matcher_translation_weight(), + options_.matcher_rotation_weight())}, + Constraint::INTRA_SUBMAP}); } for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index dbdddc0..a6e9b38 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -30,7 +30,6 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" @@ -68,9 +67,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // into the 'insertion_submaps'. void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, - const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& pose_covariance, - int trajectory_id, const mapping::Submap* matching_submap, + const transform::Rigid2d& pose, int trajectory_id, + const mapping::Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -125,8 +123,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void ComputeConstraintsForScan( const mapping::Submap* matching_submap, std::vector insertion_submaps, - const mapping::Submap* finished_submap, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); + const mapping::Submap* finished_submap, const transform::Rigid2d& pose) + REQUIRES(mutex_); // Computes constraints for a scan and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index d602b91..aa5547f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/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_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" @@ -212,25 +211,20 @@ void ConstraintBuilder::ComputeConstraint( // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; - kalman_filter::Pose2DCovariance covariance; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, *submap_scan_matcher->probability_grid, - &pose_estimate, &covariance, &unused_summary); - // 'covariance' is unchanged as (submap <- map) is a translation. + &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - constexpr double kFakePositionCovariance = 1e-6; - constexpr double kFakeOrientationCovariance = 1e-6; - constraint->reset(new Constraint{ - submap_id, - node_id, - {transform::Embed3D(constraint_transform), - common::ComputeSpdMatrixSqrtInverse( - kalman_filter::Embed3D(covariance, kFakePositionCovariance, - kFakeOrientationCovariance), - options_.lower_covariance_eigenvalue_bound())}, - Constraint::INTER_SUBMAP}); + constraint->reset( + new Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + mapping::FromTranslationRotationWeights( + options_.loop_closure_translation_weight(), + options_.loop_closure_rotation_weight())}, + Constraint::INTER_SUBMAP}); if (options_.log_matches()) { std::ostringstream info; @@ -245,9 +239,7 @@ void ConstraintBuilder::ComputeConstraint( << difference.translation().norm() << " rotation " << std::setprecision(3) << std::abs(difference.normalized_angle()); } - info << " with score " << std::setprecision(1) << 100. * score - << "% covariance trace " << std::scientific << std::setprecision(4) - << covariance.trace() << "."; + info << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 77bc740..f3efff5 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -79,6 +79,8 @@ class SparsePoseGraphTest : public ::testing::Test { min_score = 0.5, global_localization_min_score = 0.6, lower_covariance_eigenvalue_bound = 1e-6, + loop_closure_translation_weight = 1., + loop_closure_rotation_weight = 1., log_matches = true, fast_correlative_scan_matcher = { linear_search_window = 3., @@ -89,7 +91,6 @@ class SparsePoseGraphTest : public ::testing::Test { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, @@ -117,6 +118,8 @@ class SparsePoseGraphTest : public ::testing::Test { }, }, }, + matcher_translation_weight = 1., + matcher_rotation_weight = 1., optimization_problem = { acceleration_weight = 1., rotation_weight = 1e2, @@ -147,8 +150,6 @@ class SparsePoseGraphTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - kalman_filter::Pose2DCovariance covariance = - kalman_filter::Pose2DCovariance::Identity(); const mapping::Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); std::vector insertion_submaps; @@ -161,10 +162,9 @@ class SparsePoseGraphTest : public ::testing::Test { constexpr int kTrajectoryId = 0; submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - sparse_pose_graph_->AddScan(common::FromUniversal(0), - transform::Rigid3d::Identity(), range_data, - pose_estimate, covariance, kTrajectoryId, - matching_submap, insertion_submaps); + sparse_pose_graph_->AddScan( + common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, + pose_estimate, kTrajectoryId, matching_submap, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h index 15724c8..2ee349f 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h @@ -46,8 +46,8 @@ class CeresScanMatcher { CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_clouds' within the 'hybrid_grids' given an - // 'initial_pose_estimate' and returns 'pose_estimate', and - // the solver 'summary'. + // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver + // 'summary'. void Match(const transform::Rigid3d& previous_pose, const transform::Rigid3d& initial_pose_estimate, const std::vector& diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 7dcb784..8a6e2db 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -25,6 +25,8 @@ SPARSE_POSE_GRAPH = { min_score = 0.55, global_localization_min_score = 0.6, lower_covariance_eigenvalue_bound = 1e-11, + loop_closure_translation_weight = 1.1e4, + loop_closure_rotation_weight = 1e5, log_matches = true, fast_correlative_scan_matcher = { linear_search_window = 7., @@ -35,7 +37,6 @@ SPARSE_POSE_GRAPH = { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., - covariance_scale = 1e-4, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, @@ -63,6 +64,8 @@ SPARSE_POSE_GRAPH = { }, }, }, + matcher_translation_weight = 5e2, + matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, acceleration_weight = 1e4, diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index a650c77..f491536 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -39,7 +39,6 @@ TRAJECTORY_BUILDER_2D = { occupied_space_weight = 1e1, translation_weight = 1e1, rotation_weight = 1e2, - covariance_scale = 1e-2, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 20,