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.
master
Wolfgang Hess 2016-09-23 14:27:19 +02:00 committed by GitHub
parent 14355a91a1
commit e526a7022f
10 changed files with 29 additions and 77 deletions

View File

@ -58,7 +58,6 @@ google_library(mapping_2d_scan_matching_fast_correlative_scan_matcher
DEPENDS DEPENDS
common_math common_math
common_port common_port
kalman_filter_pose_tracker
mapping_2d_probability_grid mapping_2d_probability_grid
mapping_2d_scan_matching_correlative_scan_matcher mapping_2d_scan_matching_correlative_scan_matcher
mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
@ -74,7 +73,6 @@ google_library(mapping_2d_scan_matching_fast_global_localizer
HDRS HDRS
fast_global_localizer.h fast_global_localizer.h
DEPENDS DEPENDS
kalman_filter_pose_tracker
mapping_2d_scan_matching_fast_correlative_scan_matcher mapping_2d_scan_matching_fast_correlative_scan_matcher
sensor_voxel_filter sensor_voxel_filter
) )

View File

@ -24,7 +24,6 @@
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -86,8 +85,6 @@ CreateFastCorrelativeScanMatcherOptions(
parameter_dictionary->GetDouble("angular_search_window")); parameter_dictionary->GetDouble("angular_search_window"));
options.set_branch_and_bound_depth( options.set_branch_and_bound_depth(
parameter_dictionary->GetInt("branch_and_bound_depth")); parameter_dictionary->GetInt("branch_and_bound_depth"));
options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale"));
return options; return options;
} }
@ -212,20 +209,18 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
bool FastCorrelativeScanMatcher::Match( bool FastCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const float min_score, const sensor::PointCloud2D& point_cloud, const float min_score,
float* score, transform::Rigid2d* pose_estimate, float* score, transform::Rigid2d* pose_estimate) const {
kalman_filter::Pose2DCovariance* covariance) const {
const SearchParameters search_parameters(options_.linear_search_window(), const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(), options_.angular_search_window(),
point_cloud, limits_.resolution()); point_cloud, limits_.resolution());
return MatchWithSearchParameters(search_parameters, initial_pose_estimate, return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score, pose_estimate, point_cloud, min_score, score,
covariance); pose_estimate);
} }
bool FastCorrelativeScanMatcher::MatchFullSubmap( bool FastCorrelativeScanMatcher::MatchFullSubmap(
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud2D& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate, transform::Rigid2d* pose_estimate) const {
kalman_filter::Pose2DCovariance* covariance) const {
// Compute a search window around the center of the submap that includes it // Compute a search window around the center of the submap that includes it
// fully. // fully.
const SearchParameters search_parameters( const SearchParameters search_parameters(
@ -238,18 +233,16 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
Eigen::Vector2d(limits_.cell_limits().num_y_cells, Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells)); limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud, return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate, covariance); min_score, score, pose_estimate);
} }
bool FastCorrelativeScanMatcher::MatchWithSearchParameters( bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
SearchParameters search_parameters, SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud2D& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate, transform::Rigid2d* pose_estimate) const {
kalman_filter::Pose2DCovariance* covariance) const {
CHECK_NOTNULL(score); CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate); CHECK_NOTNULL(pose_estimate);
CHECK_NOTNULL(covariance);
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud = const sensor::PointCloud2D rotated_point_cloud =
@ -266,22 +259,15 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const std::vector<Candidate> lowest_resolution_candidates = const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters); 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( const Candidate best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates, 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) { if (best_candidate.score > min_score) {
*score = best_candidate.score; *score = best_candidate.score;
*pose_estimate = transform::Rigid2d( *pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x, {initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y}, initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
*covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose());
*covariance *= options_.covariance_scale();
return true; return true;
} }
return false; return false;
@ -361,16 +347,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans, const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters, const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const int candidate_depth, const std::vector<Candidate>& 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) { 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 the best candidate.
return *candidates.begin(); return *candidates.begin();
} }
@ -405,7 +383,7 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
best_high_resolution_candidate, best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters, BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1, higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score, K, u, s)); best_high_resolution_candidate.score));
} }
return best_high_resolution_candidate; return best_high_resolution_candidate;
} }

View File

@ -30,7 +30,6 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.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" #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 // Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
// is possible, true is returned, and 'score', 'pose_estimate' and // is possible, true is returned, and 'score' and 'pose_estimate' are updated
// 'covariance' are updated with the result. // with the result.
bool Match(const transform::Rigid2d& initial_pose_estimate, bool Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, const sensor::PointCloud2D& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate, float* score, transform::Rigid2d* pose_estimate) const;
kalman_filter::Pose2DCovariance* covariance) const;
// Aligns 'point_cloud' within the full 'probability_grid', i.e., not // Aligns 'point_cloud' within the full 'probability_grid', i.e., not
// restricted to the configured search window. If a score above 'min_score' // restricted to the configured search window. If a score above 'min_score'
// (excluding equality) is possible, true is returned, and 'score', // (excluding equality) is possible, true is returned, and 'score' and
// 'pose_estimate' and 'covariance' are updated with the result. // 'pose_estimate' are updated with the result.
bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score, bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate, float* score, transform::Rigid2d* pose_estimate) const;
kalman_filter::Pose2DCovariance* covariance) const;
private: private:
// The actual implementation of the scan matcher, called by Match() and // The actual implementation of the scan matcher, called by Match() and
@ -132,8 +129,7 @@ class FastCorrelativeScanMatcher {
SearchParameters search_parameters, SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud2D& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate, transform::Rigid2d* pose_estimate) const;
kalman_filter::Pose2DCovariance* covariance) const;
std::vector<Candidate> ComputeLowestResolutionCandidates( std::vector<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans, const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters) const; const SearchParameters& search_parameters) const;
@ -146,9 +142,7 @@ class FastCorrelativeScanMatcher {
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans, Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters, const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const std::vector<Candidate>& candidates,
int candidate_depth, float min_score, int candidate_depth, float min_score) const;
Eigen::Matrix3d* K, Eigen::Vector3d* u,
double* s) const;
const proto::FastCorrelativeScanMatcherOptions options_; const proto::FastCorrelativeScanMatcherOptions options_;
MapLimits limits_; MapLimits limits_;

View File

@ -111,7 +111,6 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
return { return {
linear_search_window = 3., linear_search_window = 3.,
angular_search_window = 1., angular_search_window = 1.,
covariance_scale = 1.,
branch_and_bound_depth = )text" + branch_and_bound_depth = )text" +
std::to_string(branch_and_bound_depth) + "}"); std::to_string(branch_and_bound_depth) + "}");
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
@ -159,11 +158,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options); options);
transform::Rigid2d pose_estimate; transform::Rigid2d pose_estimate;
kalman_filter::Pose2DCovariance unused_covariance;
float score; float score;
EXPECT_TRUE(fast_correlative_scan_matcher.Match( EXPECT_TRUE(fast_correlative_scan_matcher.Match(
transform::Rigid2d::Identity(), point_cloud, kMinScore, &score, transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
&pose_estimate, &unused_covariance)); &pose_estimate));
EXPECT_LT(kMinScore, score); EXPECT_LT(kMinScore, score);
EXPECT_THAT(expected_pose, EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.03f)) transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
@ -211,10 +209,9 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options); options);
transform::Rigid2d pose_estimate; transform::Rigid2d pose_estimate;
kalman_filter::Pose2DCovariance unused_covariance;
float score; float score;
EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( 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_LT(kMinScore, score);
EXPECT_THAT(expected_pose, EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.03f)) transform::IsNearly(pose_estimate.cast<float>(), 0.03f))

View File

@ -16,7 +16,6 @@
#include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h" #include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
@ -36,7 +35,6 @@ bool PerformGlobalLocalization(
CHECK(best_score != nullptr) << "Need a non-null best_score!"; CHECK(best_score != nullptr) << "Need a non-null best_score!";
*best_score = cutoff; *best_score = cutoff;
transform::Rigid2d pose_estimate; transform::Rigid2d pose_estimate;
kalman_filter::Pose2DCovariance best_pose_estimate_covariance;
const sensor::PointCloud2D filtered_point_cloud = const sensor::PointCloud2D filtered_point_cloud =
voxel_filter.Filter(point_cloud); voxel_filter.Filter(point_cloud);
bool success = false; bool success = false;
@ -47,13 +45,11 @@ bool PerformGlobalLocalization(
for (auto& matcher : matchers) { for (auto& matcher : matchers) {
float score = -1; float score = -1;
transform::Rigid2d pose_estimate; transform::Rigid2d pose_estimate;
kalman_filter::Pose2DCovariance pose_estimate_covariance;
if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score,
&pose_estimate, &pose_estimate_covariance)) { &pose_estimate)) {
CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; CHECK_GT(score, *best_score) << "MatchFullSubmap lied!";
*best_score = score; *best_score = score;
*best_pose_estimate = pose_estimate; *best_pose_estimate = pose_estimate;
best_pose_estimate_covariance = pose_estimate_covariance;
success = true; success = true;
} }
} }

View File

@ -27,7 +27,4 @@ message FastCorrelativeScanMatcherOptions {
// Number of precomputed grids to use. // Number of precomputed grids to use.
optional int32 branch_and_bound_depth = 2; optional int32 branch_and_bound_depth = 2;
// Covariance estimate is multiplied by this scale.
optional double covariance_scale = 5;
}; };

View File

@ -191,12 +191,11 @@ void ConstraintBuilder::ComputeConstraint(
// - the ComputeSubmapPose() (map <- i) // - the ComputeSubmapPose() (map <- i)
float score = 0.; float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
kalman_filter::Pose2DCovariance covariance;
if (match_full_submap) { if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
filtered_point_cloud, options_.global_localization_min_score(), filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate, &covariance)) { &score, &pose_estimate)) {
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
} else { } else {
return; return;
@ -204,12 +203,11 @@ void ConstraintBuilder::ComputeConstraint(
} else { } else {
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, options_.min_score(), &score, initial_pose, filtered_point_cloud, options_.min_score(), &score,
&pose_estimate, &covariance)) { &pose_estimate)) {
return; return;
} }
// We've reported a successful local match. // We've reported a successful local match.
CHECK_GT(score, options_.min_score()); CHECK_GT(score, options_.min_score());
// 'covariance' is unchanged as (submap <- map) is a translation.
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
score_histogram_.Add(score); 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 // 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 // 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 // CSM estimate.
// closure constraints since it is representative of a larger area (as opposed
// to the local Ceres estimate of covariance).
ceres::Solver::Summary unused_summary; 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, ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
*submap_scan_matcher->probability_grid, *submap_scan_matcher->probability_grid,
&pose_estimate, &unused_covariance, &pose_estimate, &covariance, &unused_summary);
&unused_summary); // 'covariance' is unchanged as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate; ComputeSubmapPose(*submap).inverse() * pose_estimate;

View File

@ -85,7 +85,6 @@ class SparsePoseGraphTest : public ::testing::Test {
linear_search_window = 3., linear_search_window = 3.,
angular_search_window = 0.1, angular_search_window = 0.1,
branch_and_bound_depth = 3, branch_and_bound_depth = 3,
covariance_scale = 1.,
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight = 20., occupied_space_cost_functor_weight = 20.,

View File

@ -237,9 +237,7 @@ void ConstraintBuilder::ComputeConstraint(
// Use the CSM estimate as both the initial and previous pose. This has the // 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 // 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 // CSM estimate.
// closure constraints since it is representative of a larger area (as opposed
// to the local Ceres estimate of covariance).
ceres::Solver::Summary unused_summary; ceres::Solver::Summary unused_summary;
kalman_filter::PoseCovariance covariance; kalman_filter::PoseCovariance covariance;
ceres_scan_matcher_.Match( ceres_scan_matcher_.Match(

View File

@ -31,13 +31,12 @@ SPARSE_POSE_GRAPH = {
linear_search_window = 7., linear_search_window = 7.,
angular_search_window = math.rad(30.), angular_search_window = math.rad(30.),
branch_and_bound_depth = 7, branch_and_bound_depth = 7,
covariance_scale = 1e-6,
}, },
ceres_scan_matcher = { ceres_scan_matcher = {
occupied_space_cost_functor_weight = 20., occupied_space_cost_functor_weight = 20.,
previous_pose_translation_delta_cost_functor_weight = 10., previous_pose_translation_delta_cost_functor_weight = 10.,
initial_pose_estimate_rotation_delta_cost_functor_weight = 1., initial_pose_estimate_rotation_delta_cost_functor_weight = 1.,
covariance_scale = 1e-6, covariance_scale = 1e-4,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,
max_num_iterations = 10, max_num_iterations = 10,
@ -67,7 +66,7 @@ SPARSE_POSE_GRAPH = {
}, },
}, },
optimization_problem = { optimization_problem = {
huber_scale = 5e2, huber_scale = 1e1,
acceleration_scale = 7e4, acceleration_scale = 7e4,
rotation_scale = 3e6, rotation_scale = 3e6,
consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_translation_penalty_factor = 1e5,