Removes max_covariance_trace option. (#25)

This option is no longer really useful to predict outliers.
It is removed and other parameters scaled to minimize the change
in behavior. And related cleanup.
master
Wolfgang Hess 2016-09-22 11:35:47 +02:00 committed by GitHub
parent 83a29df102
commit 0796d50b00
14 changed files with 29 additions and 62 deletions

View File

@ -82,21 +82,20 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
return ceres::atan2(vector.y(), vector.x()); return ceres::atan2(vector.y(), vector.x());
} }
// Computes (A/'scale')^{-1/2} for A being symmetric, positive-semidefinite. // Computes 'A'^{-1/2} for A being symmetric, positive-semidefinite.
// Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'. // Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'.
template <int N> template <int N>
Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse( Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse(
const Eigen::Matrix<double, N, N>& A, const double scale, const Eigen::Matrix<double, N, N>& A, const double lower_eigenvalue_bound) {
const double lower_eigenvalue_bound) {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>> Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
covariance_eigen_solver(A / scale); covariance_eigen_solver(A);
if (covariance_eigen_solver.info() != Eigen::Success) { if (covariance_eigen_solver.info() != Eigen::Success) {
LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A; LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A;
return Eigen::Matrix<double, N, N>::Identity(); return Eigen::Matrix<double, N, N>::Identity();
} }
// Since we compute the inverse, we do not allow smaller values to avoid // Since we compute the inverse, we do not allow smaller values to avoid
// infinity and NaN. // infinity and NaN.
const double relative_lower_bound = lower_eigenvalue_bound / scale; const double relative_lower_bound = lower_eigenvalue_bound;
return covariance_eigen_solver.eigenvectors() * return covariance_eigen_solver.eigenvectors() *
covariance_eigen_solver.eigenvalues() covariance_eigen_solver.eigenvalues()
.cwiseMax(relative_lower_bound) .cwiseMax(relative_lower_bound)

View File

@ -37,6 +37,7 @@ google_library(mapping_map_builder
map_builder.h map_builder.h
DEPENDS DEPENDS
common_lua_parameter_dictionary common_lua_parameter_dictionary
common_make_unique
common_thread_pool common_thread_pool
mapping_2d_global_trajectory_builder mapping_2d_global_trajectory_builder
mapping_2d_local_trajectory_builder mapping_2d_local_trajectory_builder

View File

@ -60,8 +60,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options, MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* constant_data) std::deque<TrajectoryNode::ConstantData>* constant_data)
: options_(options), : options_(options), thread_pool_(options.num_background_threads()) {
thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>( sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_, constant_data); options_.sparse_pose_graph_options(), &thread_pool_, constant_data);

View File

@ -38,8 +38,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
options.set_min_score(parameter_dictionary->GetDouble("min_score")); options.set_min_score(parameter_dictionary->GetDouble("min_score"));
options.set_global_localization_min_score( options.set_global_localization_min_score(
parameter_dictionary->GetDouble("global_localization_min_score")); parameter_dictionary->GetDouble("global_localization_min_score"));
options.set_max_covariance_trace(
parameter_dictionary->GetDouble("max_covariance_trace"));
options.set_lower_covariance_eigenvalue_bound( options.set_lower_covariance_eigenvalue_bound(
parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound")); parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
options.set_log_matches(parameter_dictionary->GetBool("log_matches")); options.set_log_matches(parameter_dictionary->GetBool("log_matches"));

View File

@ -41,10 +41,6 @@ message ConstraintBuilderOptions {
// Threshold below which global localizations are not trusted. // Threshold below which global localizations are not trusted.
optional double global_localization_min_score = 5; optional double global_localization_min_score = 5;
// Threshold for the covariance trace above which a match is not considered.
// High traces indicate that the match is not very unique.
optional double max_covariance_trace = 6;
// Lower bound for covariance eigenvalues to limit the weight of matches. // Lower bound for covariance eigenvalues to limit the weight of matches.
optional double lower_covariance_eigenvalue_bound = 7; optional double lower_covariance_eigenvalue_bound = 7;

View File

@ -126,6 +126,7 @@ google_library(mapping_2d_sparse_pose_graph
mapping_2d_submaps mapping_2d_submaps
mapping_proto_scan_matching_progress mapping_proto_scan_matching_progress
mapping_sparse_pose_graph mapping_sparse_pose_graph
mapping_sparse_pose_graph_proto_constraint_builder_options
mapping_trajectory_connectivity mapping_trajectory_connectivity
sensor_compressed_point_cloud sensor_compressed_point_cloud
sensor_point_cloud sensor_point_cloud

View File

@ -32,6 +32,7 @@
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/proto/scan_matching_progress.pb.h" #include "cartographer/mapping/proto/scan_matching_progress.pb.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -171,11 +172,13 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation. // Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constraints_.push_back( constraints_.push_back(Constraint2D{
Constraint2D{submap_index, submap_index,
scan_index, scan_index,
{constraint_transform, {constraint_transform,
constraint_builder_.ComputeSqrtLambda(covariance)}, common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint2D::INTRA_SUBMAP}); Constraint2D::INTRA_SUBMAP});
} }

View File

@ -129,13 +129,6 @@ void ConstraintBuilder::WhenDone(
[this, current_computation] { FinishComputation(current_computation); }); [this, current_computation] { FinishComputation(current_computation); });
} }
Eigen::Matrix3d ConstraintBuilder::ComputeSqrtLambda(
const Eigen::Matrix3d& covariance) const {
return common::ComputeSpdMatrixSqrtInverse(
covariance, options_.max_covariance_trace(),
options_.lower_covariance_eigenvalue_bound());
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const int submap_index, const ProbabilityGrid* const submap, const int submap_index, const ProbabilityGrid* const submap,
const std::function<void()> work_item) { const std::function<void()> work_item) {
@ -217,10 +210,6 @@ void ConstraintBuilder::ComputeConstraint(
// 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. // 'covariance' is unchanged as (submap <- map) is a translation.
if (covariance.trace() > options_.max_covariance_trace()) {
return;
}
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
score_histogram_.Add(score); score_histogram_.Add(score);
@ -244,7 +233,9 @@ void ConstraintBuilder::ComputeConstraint(
constraint->reset(new OptimizationProblem::Constraint{ constraint->reset(new OptimizationProblem::Constraint{
submap_index, submap_index,
scan_index, scan_index,
{constraint_transform, ComputeSqrtLambda(covariance)}, {constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.lower_covariance_eigenvalue_bound())},
OptimizationProblem::Constraint::INTER_SUBMAP}); OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {

View File

@ -106,9 +106,6 @@ class ConstraintBuilder {
// computations triggered by MaybeAddConstraint() have finished. // computations triggered by MaybeAddConstraint() have finished.
void WhenDone(std::function<void(const Result&)> callback); void WhenDone(std::function<void(const Result&)> callback);
// Computes the inverse square root of 'covariance'.
Eigen::Matrix3d ComputeSqrtLambda(const Eigen::Matrix3d& covariance) const;
// Returns the number of consecutive finished scans. // Returns the number of consecutive finished scans.
int GetNumFinishedScans(); int GetNumFinishedScans();

View File

@ -79,7 +79,6 @@ class SparsePoseGraphTest : public ::testing::Test {
}, },
min_score = 0.5, min_score = 0.5,
global_localization_min_score = 0.6, global_localization_min_score = 0.6,
max_covariance_trace = 10.,
lower_covariance_eigenvalue_bound = 1e-6, lower_covariance_eigenvalue_bound = 1e-6,
log_matches = true, log_matches = true,
fast_correlative_scan_matcher = { fast_correlative_scan_matcher = {

View File

@ -189,9 +189,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
scan_index, scan_index,
{constraint_transform, {constraint_transform,
common::ComputeSpdMatrixSqrtInverse( common::ComputeSpdMatrixSqrtInverse(
covariance, covariance, options_.constraint_builder_options()
options_.constraint_builder_options().max_covariance_trace(),
options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())}, .lower_covariance_eigenvalue_bound())},
Constraint3D::INTRA_SUBMAP}); Constraint3D::INTRA_SUBMAP});
} }

View File

@ -153,13 +153,6 @@ void ConstraintBuilder::WhenDone(
[this, current_computation] { FinishComputation(current_computation); }); [this, current_computation] { FinishComputation(current_computation); });
} }
Eigen::Matrix<double, 6, 6> ConstraintBuilder::ComputeSqrtLambda(
const kalman_filter::PoseCovariance& covariance) const {
return common::ComputeSpdMatrixSqrtInverse(
covariance, options_.max_covariance_trace(),
options_.lower_covariance_eigenvalue_bound());
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const int submap_index, const int submap_index,
const std::vector<mapping::TrajectoryNode>& submap_nodes, const std::vector<mapping::TrajectoryNode>& submap_nodes,
@ -248,24 +241,21 @@ void ConstraintBuilder::ComputeConstraint(
// closure constraints since it is representative of a larger area (as opposed // closure constraints since it is representative of a larger area (as opposed
// to the local Ceres estimate of covariance). // to the local Ceres estimate of covariance).
ceres::Solver::Summary unused_summary; ceres::Solver::Summary unused_summary;
// TODO(hrapp): Compute covariance instead of relying on Ceres.
kalman_filter::PoseCovariance covariance; kalman_filter::PoseCovariance covariance;
ceres_scan_matcher_.Match( ceres_scan_matcher_.Match(
pose_estimate, pose_estimate, pose_estimate, pose_estimate,
{{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
&pose_estimate, &covariance, &unused_summary); &pose_estimate, &covariance, &unused_summary);
// 'covariance' is unchanged as (submap <- map) is a translation. // 'covariance' is unchanged as (submap <- map) is a translation.
if (covariance.trace() > options_.max_covariance_trace()) {
return;
}
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose_estimate; submap->local_pose().inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{ constraint->reset(new OptimizationProblem::Constraint{
submap_index, submap_index,
scan_index, scan_index,
{constraint_transform, ComputeSqrtLambda(covariance)}, {constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.lower_covariance_eigenvalue_bound())},
OptimizationProblem::Constraint::INTER_SUBMAP}); OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {

View File

@ -102,10 +102,6 @@ class ConstraintBuilder {
// computations triggered by MaybeAddConstraint() have finished. // computations triggered by MaybeAddConstraint() have finished.
void WhenDone(std::function<void(const Result&)> callback); void WhenDone(std::function<void(const Result&)> callback);
// Computes the inverse square root of 'covariance'.
Eigen::Matrix<double, 6, 6> ComputeSqrtLambda(
const kalman_filter::PoseCovariance& covariance) const;
// Returns the number of consecutive finished scans. // Returns the number of consecutive finished scans.
int GetNumFinishedScans(); int GetNumFinishedScans();

View File

@ -25,7 +25,6 @@ SPARSE_POSE_GRAPH = {
}, },
min_score = 0.55, min_score = 0.55,
global_localization_min_score = 0.6, global_localization_min_score = 0.6,
max_covariance_trace = 1e-8,
lower_covariance_eigenvalue_bound = 1e-11, lower_covariance_eigenvalue_bound = 1e-11,
log_matches = false, log_matches = false,
fast_correlative_scan_matcher = { fast_correlative_scan_matcher = {
@ -68,11 +67,11 @@ SPARSE_POSE_GRAPH = {
}, },
}, },
optimization_problem = { optimization_problem = {
huber_scale = 5e-2, huber_scale = 5e2,
acceleration_scale = 7., acceleration_scale = 7e4,
rotation_scale = 3e2, rotation_scale = 3e6,
consecutive_scan_translation_penalty_factor = 1e1, consecutive_scan_translation_penalty_factor = 1e5,
consecutive_scan_rotation_penalty_factor = 1e1, consecutive_scan_rotation_penalty_factor = 1e5,
log_solver_summary = false, log_solver_summary = false,
log_residual_histograms = false, log_residual_histograms = false,
ceres_solver_options = { ceres_solver_options = {