From e526a7022f28430abf640057b1032d5663d7d159 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 23 Sep 2016 14:27:19 +0200 Subject: [PATCH] Remove covariance computation from branch-and-bound. (#27) These covariances were only used in 2D and are only an estimate. Following 3D, we change 2D to use the (local) covariance computed using Ceres. --- .../mapping_2d/scan_matching/CMakeLists.txt | 2 - .../fast_correlative_scan_matcher.cc | 40 +++++-------------- .../fast_correlative_scan_matcher.h | 22 ++++------ .../fast_correlative_scan_matcher_test.cc | 7 +--- .../scan_matching/fast_global_localizer.cc | 6 +-- ...ast_correlative_scan_matcher_options.proto | 3 -- .../sparse_pose_graph/constraint_builder.cc | 16 +++----- .../mapping_2d/sparse_pose_graph_test.cc | 1 - .../sparse_pose_graph/constraint_builder.cc | 4 +- configuration_files/sparse_pose_graph.lua | 5 +-- 10 files changed, 29 insertions(+), 77 deletions(-) diff --git a/cartographer/mapping_2d/scan_matching/CMakeLists.txt b/cartographer/mapping_2d/scan_matching/CMakeLists.txt index 5cd313e..92ed70c 100644 --- a/cartographer/mapping_2d/scan_matching/CMakeLists.txt +++ b/cartographer/mapping_2d/scan_matching/CMakeLists.txt @@ -58,7 +58,6 @@ google_library(mapping_2d_scan_matching_fast_correlative_scan_matcher DEPENDS common_math common_port - kalman_filter_pose_tracker mapping_2d_probability_grid mapping_2d_scan_matching_correlative_scan_matcher mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options @@ -74,7 +73,6 @@ google_library(mapping_2d_scan_matching_fast_global_localizer HDRS fast_global_localizer.h DEPENDS - kalman_filter_pose_tracker mapping_2d_scan_matching_fast_correlative_scan_matcher sensor_voxel_filter ) diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc index b17e513..bf2dc5e 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -24,7 +24,6 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" @@ -86,8 +85,6 @@ CreateFastCorrelativeScanMatcherOptions( parameter_dictionary->GetDouble("angular_search_window")); options.set_branch_and_bound_depth( parameter_dictionary->GetInt("branch_and_bound_depth")); - options.set_covariance_scale( - parameter_dictionary->GetDouble("covariance_scale")); return options; } @@ -212,20 +209,18 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud2D& point_cloud, const float min_score, - float* score, transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const { + float* score, transform::Rigid2d* pose_estimate) const { const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(), point_cloud, limits_.resolution()); return MatchWithSearchParameters(search_parameters, initial_pose_estimate, - point_cloud, min_score, score, pose_estimate, - covariance); + point_cloud, min_score, score, + pose_estimate); } bool FastCorrelativeScanMatcher::MatchFullSubmap( const sensor::PointCloud2D& point_cloud, float min_score, float* score, - transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const { + transform::Rigid2d* pose_estimate) const { // Compute a search window around the center of the submap that includes it // fully. const SearchParameters search_parameters( @@ -238,18 +233,16 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( Eigen::Vector2d(limits_.cell_limits().num_y_cells, limits_.cell_limits().num_x_cells)); return MatchWithSearchParameters(search_parameters, center, point_cloud, - min_score, score, pose_estimate, covariance); + min_score, score, pose_estimate); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud2D& point_cloud, float min_score, float* score, - transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const { + transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); - CHECK_NOTNULL(covariance); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const sensor::PointCloud2D rotated_point_cloud = @@ -266,22 +259,15 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(discrete_scans, search_parameters); - // The following are intermediate results for computing covariance. See - // "Real-Time Correlative Scan Matching" by Olson. - Eigen::Matrix3d K = Eigen::Matrix3d::Zero(); - Eigen::Vector3d u = Eigen::Vector3d::Zero(); - double s = 0.; const Candidate best_candidate = BranchAndBound( discrete_scans, search_parameters, lowest_resolution_candidates, - precomputation_grid_stack_->max_depth(), min_score, &K, &u, &s); + precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { *score = best_candidate.score; *pose_estimate = transform::Rigid2d( {initial_pose_estimate.translation().x() + best_candidate.x, initial_pose_estimate.translation().y() + best_candidate.y}, initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); - *covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose()); - *covariance *= options_.covariance_scale(); return true; } return false; @@ -361,16 +347,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( const std::vector& discrete_scans, const SearchParameters& search_parameters, const std::vector& candidates, const int candidate_depth, - float min_score, Eigen::Matrix3d* K, Eigen::Vector3d* u, double* s) const { + float min_score) const { if (candidate_depth == 0) { - // Update the covariance computation intermediate values. - for (const Candidate& candidate : candidates) { - const double p = candidate.score; - const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation); - *K += (xi * xi.transpose()) * p; - *u += xi * p; - *s += p; - } // Return the best candidate. return *candidates.begin(); } @@ -405,7 +383,7 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound( best_high_resolution_candidate, BranchAndBound(discrete_scans, search_parameters, higher_resolution_candidates, candidate_depth - 1, - best_high_resolution_candidate.score, K, u, s)); + best_high_resolution_candidate.score)); } return best_high_resolution_candidate; } diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h index e39eca7..5602e7e 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h @@ -30,7 +30,6 @@ #include "Eigen/Core" #include "cartographer/common/port.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" @@ -109,20 +108,18 @@ class FastCorrelativeScanMatcher { // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) - // is possible, true is returned, and 'score', 'pose_estimate' and - // 'covariance' are updated with the result. + // is possible, true is returned, and 'score' and 'pose_estimate' are updated + // with the result. bool Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud2D& point_cloud, float min_score, - float* score, transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const; + float* score, transform::Rigid2d* pose_estimate) const; // Aligns 'point_cloud' within the full 'probability_grid', i.e., not // restricted to the configured search window. If a score above 'min_score' - // (excluding equality) is possible, true is returned, and 'score', - // 'pose_estimate' and 'covariance' are updated with the result. + // (excluding equality) is possible, true is returned, and 'score' and + // 'pose_estimate' are updated with the result. bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score, - float* score, transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const; + float* score, transform::Rigid2d* pose_estimate) const; private: // The actual implementation of the scan matcher, called by Match() and @@ -132,8 +129,7 @@ class FastCorrelativeScanMatcher { SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud2D& point_cloud, float min_score, float* score, - transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const; + transform::Rigid2d* pose_estimate) const; std::vector ComputeLowestResolutionCandidates( const std::vector& discrete_scans, const SearchParameters& search_parameters) const; @@ -146,9 +142,7 @@ class FastCorrelativeScanMatcher { Candidate BranchAndBound(const std::vector& discrete_scans, const SearchParameters& search_parameters, const std::vector& candidates, - int candidate_depth, float min_score, - Eigen::Matrix3d* K, Eigen::Vector3d* u, - double* s) const; + int candidate_depth, float min_score) const; const proto::FastCorrelativeScanMatcherOptions options_; MapLimits limits_; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index f9e635b..5549140 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -111,7 +111,6 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { return { linear_search_window = 3., angular_search_window = 1., - covariance_scale = 1., branch_and_bound_depth = )text" + std::to_string(branch_and_bound_depth) + "}"); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); @@ -159,11 +158,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); transform::Rigid2d pose_estimate; - kalman_filter::Pose2DCovariance unused_covariance; float score; EXPECT_TRUE(fast_correlative_scan_matcher.Match( transform::Rigid2d::Identity(), point_cloud, kMinScore, &score, - &pose_estimate, &unused_covariance)); + &pose_estimate)); EXPECT_LT(kMinScore, score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.03f)) @@ -211,10 +209,9 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); transform::Rigid2d pose_estimate; - kalman_filter::Pose2DCovariance unused_covariance; float score; EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( - point_cloud, kMinScore, &score, &pose_estimate, &unused_covariance)); + point_cloud, kMinScore, &score, &pose_estimate)); EXPECT_LT(kMinScore, score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.03f)) diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc index b5f4120..dbd6ba0 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc +++ b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc @@ -16,7 +16,6 @@ #include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h" -#include "cartographer/kalman_filter/pose_tracker.h" #include "glog/logging.h" namespace cartographer { @@ -36,7 +35,6 @@ bool PerformGlobalLocalization( CHECK(best_score != nullptr) << "Need a non-null best_score!"; *best_score = cutoff; transform::Rigid2d pose_estimate; - kalman_filter::Pose2DCovariance best_pose_estimate_covariance; const sensor::PointCloud2D filtered_point_cloud = voxel_filter.Filter(point_cloud); bool success = false; @@ -47,13 +45,11 @@ bool PerformGlobalLocalization( for (auto& matcher : matchers) { float score = -1; transform::Rigid2d pose_estimate; - kalman_filter::Pose2DCovariance pose_estimate_covariance; if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, - &pose_estimate, &pose_estimate_covariance)) { + &pose_estimate)) { CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; *best_score = score; *best_pose_estimate = pose_estimate; - best_pose_estimate_covariance = pose_estimate_covariance; success = true; } } diff --git a/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto index b788269..b28ad1a 100644 --- a/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto +++ b/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto @@ -27,7 +27,4 @@ message FastCorrelativeScanMatcherOptions { // Number of precomputed grids to use. optional int32 branch_and_bound_depth = 2; - - // Covariance estimate is multiplied by this scale. - optional double covariance_scale = 5; }; diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index fe92823..24df77f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -191,12 +191,11 @@ void ConstraintBuilder::ComputeConstraint( // - the ComputeSubmapPose() (map <- i) float score = 0.; transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); - kalman_filter::Pose2DCovariance covariance; if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( filtered_point_cloud, options_.global_localization_min_score(), - &score, &pose_estimate, &covariance)) { + &score, &pose_estimate)) { trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); } else { return; @@ -204,12 +203,11 @@ void ConstraintBuilder::ComputeConstraint( } else { if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, options_.min_score(), &score, - &pose_estimate, &covariance)) { + &pose_estimate)) { return; } // We've reported a successful local match. CHECK_GT(score, options_.min_score()); - // 'covariance' is unchanged as (submap <- map) is a translation. { common::MutexLocker locker(&mutex_); score_histogram_.Add(score); @@ -218,15 +216,13 @@ void ConstraintBuilder::ComputeConstraint( // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original - // CSM estimate. We also prefer to use the CSM covariance estimate for loop - // closure constraints since it is representative of a larger area (as opposed - // to the local Ceres estimate of covariance). + // CSM estimate. ceres::Solver::Summary unused_summary; - kalman_filter::Pose2DCovariance unused_covariance; + kalman_filter::Pose2DCovariance covariance; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, *submap_scan_matcher->probability_grid, - &pose_estimate, &unused_covariance, - &unused_summary); + &pose_estimate, &covariance, &unused_summary); + // 'covariance' is unchanged as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index ba44bfe..8ebbe15 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -85,7 +85,6 @@ class SparsePoseGraphTest : public ::testing::Test { linear_search_window = 3., angular_search_window = 0.1, branch_and_bound_depth = 3, - covariance_scale = 1., }, ceres_scan_matcher = { occupied_space_cost_functor_weight = 20., diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 2ac0482..0f15101 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -237,9 +237,7 @@ void ConstraintBuilder::ComputeConstraint( // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original - // CSM estimate. We also prefer to use the CSM covariance estimate for loop - // closure constraints since it is representative of a larger area (as opposed - // to the local Ceres estimate of covariance). + // CSM estimate. ceres::Solver::Summary unused_summary; kalman_filter::PoseCovariance covariance; ceres_scan_matcher_.Match( diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 7b304f9..769d1d6 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -31,13 +31,12 @@ SPARSE_POSE_GRAPH = { linear_search_window = 7., angular_search_window = math.rad(30.), branch_and_bound_depth = 7, - covariance_scale = 1e-6, }, ceres_scan_matcher = { occupied_space_cost_functor_weight = 20., previous_pose_translation_delta_cost_functor_weight = 10., initial_pose_estimate_rotation_delta_cost_functor_weight = 1., - covariance_scale = 1e-6, + covariance_scale = 1e-4, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, @@ -67,7 +66,7 @@ SPARSE_POSE_GRAPH = { }, }, optimization_problem = { - huber_scale = 5e2, + huber_scale = 1e1, acceleration_scale = 7e4, rotation_scale = 3e6, consecutive_scan_translation_penalty_factor = 1e5,