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

Also use fixed covariances in 3D SLAM. (#308) 

This follows #307.
master
Wolfgang Hess 2017-06-01 15:04:07 +02:00 committed by GitHub
parent a2e52f81cf
commit 92f154f561
15 changed files with 68 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cartographer/common/math.h
View File

@ -82,29 +82,6 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
return ceres::atan2(vector.y(), vector.x());
}
// 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 lower_eigenvalue_bound) {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
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;
return covariance_eigen_solver.eigenvectors() *
covariance_eigen_solver.eigenvalues()
.cwiseMax(relative_lower_bound)
.cwiseInverse()
.cwiseSqrt()
.asDiagonal() *
covariance_eigen_solver.eigenvectors().inverse();
}
} // namespace common
} // namespace cartographer

7
cartographer/mapping/proto/sparse_pose_graph_options.proto
View File

@ -28,9 +28,12 @@ message SparsePoseGraphOptions {
optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
constraint_builder_options = 3;
// Weights used in the optimization problem for non-loop-closure scan matcher
// constraints.
// Weight used in the optimization problem for the translational component of
// non-loop-closure scan matcher constraints.
optional double matcher_translation_weight = 7;
// Weight used in the optimization problem for the rotational component of
// non-loop-closure scan matcher constraints.
optional double matcher_rotation_weight = 8;
// Options for the optimization problem.

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_lower_covariance_eigenvalue_bound(
parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
options.set_loop_closure_translation_weight(
parameter_dictionary->GetDouble("loop_closure_translation_weight"));
options.set_loop_closure_rotation_weight(

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

@ -41,12 +41,12 @@ message ConstraintBuilderOptions {
// Threshold below which global localizations are not trusted.
optional double global_localization_min_score = 5;
// Lower bound for covariance eigenvalues to limit the weight of matches.
// TODO(whess): Deprecated and only used in 3D. Replace usage and remove.
optional double lower_covariance_eigenvalue_bound = 7;
// Weights used in the optimization problem for loop closure constraints.
// Weight used in the optimization problem for the translational component of
// loop closure constraints.
optional double loop_closure_translation_weight = 13;
// Weight used in the optimization problem for the rotational component of
// loop closure constraints.
optional double loop_closure_rotation_weight = 14;
// If enabled, logs information of loop-closing constraints for debugging.

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -78,7 +78,6 @@ class SparsePoseGraphTest : public ::testing::Test {
},
min_score = 0.5,
global_localization_min_score = 0.6,
lower_covariance_eigenvalue_bound = 1e-6,
loop_closure_translation_weight = 1.,
loop_closure_rotation_weight = 1.,
log_matches = true,

5
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -55,9 +55,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, insertion_result->covariance_estimate,
trajectory_id_, insertion_result->matching_submap,
insertion_result->insertion_submaps);
insertion_result->pose_observation, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

14
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -178,17 +178,16 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
.scan_matcher_variance() *
kalman_filter::PoseCovariance::Identity());
kalman_filter::PoseCovariance covariance_estimate;
kalman_filter::PoseCovariance unused_covariance_estimate;
pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &scan_matcher_pose_estimate_, &covariance_estimate);
time, &scan_matcher_pose_estimate_, &unused_covariance_estimate);
last_pose_estimate_ = {
time, scan_matcher_pose_estimate_,
sensor::TransformPointCloud(filtered_range_data.returns,
pose_observation.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, pose_observation,
covariance_estimate);
return InsertIntoSubmap(time, filtered_range_data, pose_observation);
}
void KalmanLocalTrajectoryBuilder::AddOdometerData(
@ -216,8 +215,7 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const {
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate) {
const transform::Rigid3d& pose_observation) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
@ -231,8 +229,8 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
pose_tracker_->gravity_orientation());
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate,
return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation,
matching_submap, insertion_submaps});
}

3
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -62,8 +62,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate);
const transform::Rigid3d& pose_observation);
const proto::LocalTrajectoryBuilderOptions options_;
std::unique_ptr<mapping_3d::Submaps> submaps_;

1
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -37,7 +37,6 @@ class LocalTrajectoryBuilderInterface {
common::Time time;
sensor::RangeData range_data_in_tracking;
transform::Rigid3d pose_observation;
kalman_filter::PoseCovariance covariance_estimate;
const Submap* matching_submap;
std::vector<const Submap*> insertion_submaps;
};

6
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -19,7 +19,6 @@
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/rotation_cost_function.h"
#include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h"
@ -424,12 +423,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
pose_observation.cast<float>()),
kFakeGravityOrientation);
const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity();
return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation,
kCovariance, matching_submap, insertion_submaps});
matching_submap, insertion_submaps});
}
OptimizingLocalTrajectoryBuilder::State

20
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -88,8 +88,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance, const int trajectory_id,
const transform::Rigid3d& pose, const int trajectory_id,
const Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
@ -124,7 +123,7 @@ void SparsePoseGraph::AddScan(
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(matching_submap, insertion_submaps,
finished_submap, pose, covariance);
finished_submap, pose);
});
}
@ -224,8 +223,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
void SparsePoseGraph::ComputeConstraintsForScan(
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
const Submap* finished_submap, const transform::Rigid3d& pose) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
const transform::Rigid3d optimized_pose =
@ -251,16 +249,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id).finished);
submap_states_.at(submap_id).node_ids.emplace(node_id);
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform =
submap->local_pose.inverse() * pose;
constraints_.push_back(
Constraint{submap_id,
constraints_.push_back(Constraint{
submap_id,
node_id,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
{constraint_transform, mapping::FromTranslationRotationWeights(
options_.matcher_translation_weight(),
options_.matcher_rotation_weight())},
Constraint::INTRA_SUBMAP});
}

14
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -30,7 +30,6 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
@ -67,9 +66,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// 'insertion_submaps'.
void AddScan(common::Time time,
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
int trajectory_id, const Submap* matching_submap,
const transform::Rigid3d& pose, int trajectory_id,
const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
@ -120,11 +118,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
const Submap* matching_submap,
void ComputeConstraintsForScan(const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
const Submap* finished_submap,
const transform::Rigid3d& pose)
REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const mapping::NodeId& node_id,

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

@ -225,9 +225,9 @@ void ConstraintBuilder::ComputeConstraint(
constraint->reset(new OptimizationProblem::Constraint{
submap_id,
node_id,
{constraint_transform,
1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
kalman_filter::PoseCovariance::Identity()},
{constraint_transform, mapping::FromTranslationRotationWeights(
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight())},
OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) {

5
configuration_files/sparse_pose_graph.lua
View File

@ -24,7 +24,6 @@ SPARSE_POSE_GRAPH = {
},
min_score = 0.55,
global_localization_min_score = 0.6,
lower_covariance_eigenvalue_bound = 1e-11,
loop_closure_translation_weight = 1.1e4,
loop_closure_rotation_weight = 1e5,
log_matches = true,
@ -68,8 +67,8 @@ SPARSE_POSE_GRAPH = {
matcher_rotation_weight = 1.6e3,
optimization_problem = {
huber_scale = 1e1,
acceleration_weight = 1e4,
rotation_weight = 3e6,
acceleration_weight = 1e3,
rotation_weight = 3e5,
consecutive_scan_translation_penalty_factor = 1e5,
consecutive_scan_rotation_penalty_factor = 1e5,
log_solver_summary = false,

33
docs/source/configuration.rst
View File

@ -62,15 +62,9 @@ cartographer.mapping.proto.MapBuilderOptions
bool use_trajectory_builder_2d
Not yet documented.
cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options
Not yet documented.
bool use_trajectory_builder_3d
Not yet documented.
cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options
Not yet documented.
int32 num_background_threads
Number of threads to use for background computations.
@ -88,6 +82,14 @@ int32 optimize_every_n_scans
cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options
Options for the constraint builder.
double matcher_translation_weight
Weight used in the optimization problem for the translational component of
non-loop-closure scan matcher constraints.
double matcher_rotation_weight
Weight used in the optimization problem for the rotational component of
non-loop-closure scan matcher constraints.
cartographer.mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options
Options for the optimization problem.
@ -100,6 +102,16 @@ double global_sampling_ratio
localization.
cartographer.mapping.proto.TrajectoryBuilderOptions
===================================================
cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options
Not yet documented.
cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options
Not yet documented.
cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
=====================================================================
@ -120,8 +132,13 @@ double min_score
double global_localization_min_score
Threshold below which global localizations are not trusted.
double lower_covariance_eigenvalue_bound
Lower bound for covariance eigenvalues to limit the weight of matches.
double loop_closure_translation_weight
Weight used in the optimization problem for the translational component of
loop closure constraints.
double loop_closure_rotation_weight
Weight used in the optimization problem for the rotational component of
loop closure constraints.
bool log_matches
If enabled, logs information of loop-closing constraints for debugging.