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

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

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.

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(

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.

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,

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,

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

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

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

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

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

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,

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()) {

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,

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.