12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/scan_matching/CMakeLists.txt
View File

@ -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
)

40
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -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<Candidate> 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<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
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) {
// 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;
}

22
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -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<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters) const;
@ -146,9 +142,7 @@ class FastCorrelativeScanMatcher {
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& 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_;

7
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -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<float>(), 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<float>(), 0.03f))

6
cartographer/mapping_2d/scan_matching/fast_global_localizer.cc
View File

@ -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;
}
}

3
cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto
View File

@ -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;
};

16
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -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;

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -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.,

4
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -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(

5
configuration_files/sparse_pose_graph.lua
View File

@ -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,