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

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

9
cartographer/common/math.h
View File

@ -82,21 +82,20 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
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'.
template <int N>
Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse(
const Eigen::Matrix<double, N, N>& A, const double scale,
const double lower_eigenvalue_bound) {
const Eigen::Matrix<double, N, N>& A, const double lower_eigenvalue_bound) {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
covariance_eigen_solver(A / scale);
covariance_eigen_solver(A);
if (covariance_eigen_solver.info() != Eigen::Success) {
LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A;
return Eigen::Matrix<double, N, N>::Identity();
}
// Since we compute the inverse, we do not allow smaller values to avoid
// 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() *
covariance_eigen_solver.eigenvalues()
.cwiseMax(relative_lower_bound)

1
cartographer/mapping/CMakeLists.txt
View File

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

3
cartographer/mapping/map_builder.cc
View File

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

2
cartographer/mapping/sparse_pose_graph/constraint_builder.cc
View File

@ -38,8 +38,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
options.set_min_score(parameter_dictionary->GetDouble("min_score"));
options.set_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(
parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
options.set_log_matches(parameter_dictionary->GetBool("log_matches"));

4
cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto
View File

@ -41,10 +41,6 @@ message ConstraintBuilderOptions {
// Threshold below which global localizations are not trusted.
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.
optional double lower_covariance_eigenvalue_bound = 7;

1
cartographer/mapping_2d/CMakeLists.txt
View File

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

15
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -32,6 +32,7 @@
#include "cartographer/common/math.h"
#include "cartographer/common/time.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/voxel_filter.h"
#include "glog/logging.h"
@ -171,12 +172,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constraints_.push_back(
Constraint2D{submap_index,
scan_index,
{constraint_transform,
constraint_builder_.ComputeSqrtLambda(covariance)},
Constraint2D::INTRA_SUBMAP});
constraints_.push_back(Constraint2D{
submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint2D::INTRA_SUBMAP});
}
// Determine if this scan should be globally localized.

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

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

3
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -106,9 +106,6 @@ class ConstraintBuilder {
// computations triggered by MaybeAddConstraint() have finished.
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.
int GetNumFinishedScans();

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

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

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

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

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

@ -153,13 +153,6 @@ void ConstraintBuilder::WhenDone(
[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(
const int submap_index,
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
// to the local Ceres estimate of covariance).
ceres::Solver::Summary unused_summary;
// TODO(hrapp): Compute covariance instead of relying on Ceres.
kalman_filter::PoseCovariance covariance;
ceres_scan_matcher_.Match(
pose_estimate, pose_estimate,
{{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
&pose_estimate, &covariance, &unused_summary);
// 'covariance' is unchanged as (submap <- map) is a translation.
if (covariance.trace() > options_.max_covariance_trace()) {
return;
}
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{
submap_index,
scan_index,
{constraint_transform, ComputeSqrtLambda(covariance)},
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.lower_covariance_eigenvalue_bound())},
OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) {

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

@ -102,10 +102,6 @@ class ConstraintBuilder {
// computations triggered by MaybeAddConstraint() have finished.
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.
int GetNumFinishedScans();

11
configuration_files/sparse_pose_graph.lua
View File

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