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

Simplify the handling of weights in the optimization problem. (#309) 

This replaces the 3x3 (2D) or 6x6 (3D) matrix by the weights
for the translational and rotational component. The matrices
only contain these values now anyway, so this simplifies
the code and increases performance.

Also removes 2D from the UKF implementation which has not been
used for quite some time now.
master
Wolfgang Hess 2017-06-01 17:03:49 +02:00 committed by GitHub
parent 92f154f561
commit 565e9c3eff
17 changed files with 59 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/kalman_filter/pose_tracker.cc
View File

@ -80,8 +80,8 @@ PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
}
// Build a model matrix for the given time delta.
PoseTracker::State ModelFunction3D(const PoseTracker::State& state,
const double delta_t) {
PoseTracker::State ModelFunction(const PoseTracker::State& state,
const double delta_t) {
CHECK_GT(delta_t, 0.);
PoseTracker::State new_state;
@ -109,18 +109,6 @@ PoseTracker::State ModelFunction3D(const PoseTracker::State& state,
return new_state;
}
// A specialization of ModelFunction3D that limits the z-component of position
// and velocity to 0.
PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
const double delta_t) {
auto new_state = ModelFunction3D(state, delta_t);
new_state[PoseTracker::kMapPositionZ] = 0.;
new_state[PoseTracker::kMapVelocityZ] = 0.;
new_state[PoseTracker::kMapOrientationX] = 0.;
new_state[PoseTracker::kMapOrientationY] = 0.;
return new_state;
}
} // namespace
PoseAndCovariance operator*(const transform::Rigid3d& transform,
@ -163,10 +151,8 @@ PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
}
PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
const ModelFunction& model_function,
const common::Time time)
: options_(options),
model_function_(model_function),
time_(time),
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
imu_tracker_(options.imu_gravity_time_constant(), time),
@ -228,14 +214,7 @@ void PoseTracker::Predict(const common::Time time) {
}
kalman_filter_.Predict(
[this, delta_t](const State& state) -> State {
switch (model_function_) {
case ModelFunction::k2D:
return ModelFunction2D(state, delta_t);
case ModelFunction::k3D:
return ModelFunction3D(state, delta_t);
default:
LOG(FATAL);
}
return ModelFunction(state, delta_t);
},
BuildModelNoise(delta_t));
time_ = time;
@ -318,31 +297,6 @@ transform::Rigid3d PoseTracker::RigidFromState(
imu_tracker_.orientation());
}
Pose2DCovariance Project2D(const PoseCovariance& covariance) {
Pose2DCovariance projected_covariance;
projected_covariance.block<2, 2>(0, 0) = covariance.block<2, 2>(0, 0);
projected_covariance.block<2, 1>(0, 2) = covariance.block<2, 1>(0, 5);
projected_covariance.block<1, 2>(2, 0) = covariance.block<1, 2>(5, 0);
projected_covariance(2, 2) = covariance(5, 5);
return projected_covariance;
}
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
const double position_variance,
const double orientation_variance) {
PoseCovariance covariance;
covariance.setZero();
covariance.block<2, 2>(0, 0) = embedded_covariance.block<2, 2>(0, 0);
covariance.block<2, 1>(0, 5) = embedded_covariance.block<2, 1>(0, 2);
covariance.block<1, 2>(5, 0) = embedded_covariance.block<1, 2>(2, 0);
covariance(5, 5) = embedded_covariance(2, 2);
covariance(2, 2) = position_variance;
covariance(3, 3) = orientation_variance;
covariance(4, 4) = orientation_variance;
return covariance;
}
PoseCovariance BuildPoseCovariance(const double translational_variance,
const double rotational_variance) {
const Eigen::Matrix3d translational =

13
cartographer/kalman_filter/pose_tracker.h
View File

@ -47,13 +47,6 @@ struct PoseAndCovariance {
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance);
// Projects a 3D pose covariance into a 2D pose covariance.
Pose2DCovariance Project2D(const PoseCovariance& embedded_covariance);
// Embeds a 2D pose covariance into a 3D pose covariance.
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
double position_variance, double orientation_variance);
PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance);
@ -77,8 +70,6 @@ class PoseTracker {
kDimension // We terminate loops with this.
};
enum class ModelFunction { k2D, k3D };
using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
using State = KalmanFilter::StateType;
using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
@ -86,8 +77,7 @@ class PoseTracker {
// Create a new Kalman filter starting at the origin with a standard normal
// distribution at 'time'.
PoseTracker(const proto::PoseTrackerOptions& options,
const ModelFunction& model_function, common::Time time);
PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
virtual ~PoseTracker();
// Sets 'pose' and 'covariance' to the mean and covariance of the belief at
@ -138,7 +128,6 @@ class PoseTracker {
transform::Rigid3d RigidFromState(const PoseTracker::State& state);
const proto::PoseTrackerOptions options_;
const ModelFunction model_function_;
common::Time time_;
KalmanFilter kalman_filter_;
mapping::ImuTracker imu_tracker_;

21
cartographer/kalman_filter/pose_tracker_test.cc
View File

@ -50,30 +50,13 @@ class PoseTrackerTest : public ::testing::Test {
)text");
const proto::PoseTrackerOptions options =
CreatePoseTrackerOptions(parameter_dictionary.get());
pose_tracker_ = common::make_unique<PoseTracker>(
options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000));
pose_tracker_ =
common::make_unique<PoseTracker>(options, common::FromUniversal(1000));
}
std::unique_ptr<PoseTracker> pose_tracker_;
};
TEST(CovarianceTest, EmbedAndProjectCovariance) {
std::mt19937 prng(42);
std::uniform_real_distribution<float> distribution(-10.f, 10.f);
for (int i = 0; i < 100; ++i) {
Pose2DCovariance covariance;
for (int row = 0; row < 3; ++row) {
for (int column = 0; column < 3; ++column) {
covariance(row, column) = distribution(prng);
}
}
const PoseCovariance embedded_covariance =
Embed3D(covariance, distribution(prng), distribution(prng));
EXPECT_TRUE(
(Project2D(embedded_covariance).array() == covariance.array()).all());
}
}
TEST_F(PoseTrackerTest, SaveAndRestore) {
std::vector<Rigid3d> poses(3);
std::vector<PoseCovariance> covariances(3);

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

@ -44,9 +44,10 @@ message SparsePoseGraph {
// Pose of the scan relative to submap, i.e. taking data from the scan frame
// into the submap frame.
optional transform.proto.Rigid3d relative_pose = 3;
// 6x6 square root inverse of the covariance matrix. This is stored in
// column-major order for ease of use with Eigen::Map.
repeated double sqrt_Lambda = 4 [packed = true]; // NOLINT
// Weight of the translational part of the constraint.
optional double translation_weight = 6;
// Weight of the rotational part of the constraint.
optional double rotation_weight = 7;
optional Tag tag = 5;
}

25
cartographer/mapping/sparse_pose_graph.cc
View File

@ -58,20 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
return options;
}
Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
const double translation_weight, const double rotation_weight) {
Eigen::Matrix<double, 6, 6> result;
// clang-format off
result << translation_weight, 0., 0., 0., 0., 0.,
0., translation_weight, 0., 0., 0., 0.,
0., 0., translation_weight, 0., 0., 0.,
0., 0., 0., rotation_weight, 0., 0.,
0., 0., 0., 0., rotation_weight, 0.,
0., 0., 0., 0., 0., rotation_weight;
// clang-format on
return result;
}
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
@ -79,14 +65,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
transform::ToProto(constraint.pose.zbar_ij);
constraint_proto->mutable_sqrt_lambda()->Reserve(36);
for (int i = 0; i != 36; ++i) {
constraint_proto->mutable_sqrt_lambda()->Add(0.);
}
Eigen::Map<Eigen::Matrix<double, 6, 6>>(
constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
constraint.pose.sqrt_Lambda_ij;
constraint_proto->set_translation_weight(
constraint.pose.translation_weight);
constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
constraint_proto->mutable_submap_id()->set_trajectory_id(
constraint.submap_id.trajectory_id);
constraint_proto->mutable_submap_id()->set_submap_index(

7
cartographer/mapping/sparse_pose_graph.h
View File

@ -35,10 +35,6 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// TODO(whess): Change to two doubles for performance.
Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
double translation_weight, double rotation_weight);
class SparsePoseGraph {
public:
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
@ -47,7 +43,8 @@ class SparsePoseGraph {
struct Constraint {
struct Pose {
transform::Rigid3d zbar_ij;
Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
double translation_weight;
double rotation_weight;
};
mapping::SubmapId submap_id; // 'i' in the paper.

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

@ -25,9 +25,6 @@ message CeresScanMatcherOptions {
optional double translation_weight = 2;
optional double rotation_weight = 3;
// Scale applied to the covariance estimate from Ceres.
optional double covariance_scale = 4;
// Configure the Ceres solver. See the Ceres documentation for more
// information: https://code.google.com/p/ceres-solver/
optional common.proto.CeresSolverOptions ceres_solver_options = 9;

14
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -234,14 +234,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_states_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constraints_.push_back(
Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
mapping::FromTranslationRotationWeights(
options_.matcher_translation_weight(),
options_.matcher_rotation_weight())},
Constraint::INTRA_SUBMAP});
constraints_.push_back(Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();

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

@ -217,14 +217,12 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(
new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
mapping::FromTranslationRotationWeights(
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight())},
Constraint::INTER_SUBMAP});
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
if (options_.log_matches()) {
std::ostringstream info;

12
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -155,17 +155,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
// Add penalties for changes between consecutive scans.
const Eigen::DiagonalMatrix<double, 3> consecutive_pose_change_penalty_matrix(
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor());
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
++node_index) {
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
@ -173,9 +166,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
.initial_point_cloud_pose.inverse() *
node_data_[trajectory_id][node_index]
.initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */,
C_nodes[trajectory_id][node_index - 1].data(),
C_nodes[trajectory_id][node_index].data());

12
cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h
View File

@ -22,7 +22,6 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -59,16 +58,17 @@ class SpaCostFunction {
h[2])}};
}
// Computes the error scaled by 'sqrt_Lambda_ij', storing it in 'e'.
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i, const T* const c_j,
T* const e) {
std::array<T, 3> e_ij =
const std::array<T, 3> e_ij =
ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
(Eigen::Map<Eigen::Matrix<T, 3, 1>>(e)) =
kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast<T>() *
Eigen::Map<Eigen::Matrix<T, 3, 1>>(e_ij.data());
e[0] = e_ij[0] * T(pose.translation_weight);
e[1] = e_ij[1] * T(pose.translation_weight);
e[2] = e_ij[2] * T(pose.rotation_weight);
}
template <typename T>

4
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -57,7 +57,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
options_.kalman_local_trajectory_builder_options()
.pose_tracker_options(),
kalman_filter::PoseTracker::ModelFunction::k3D, time);
time);
}
pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
@ -196,7 +196,7 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData(
pose_tracker_.reset(new kalman_filter::PoseTracker(
options_.kalman_local_trajectory_builder_options()
.pose_tracker_options(),
kalman_filter::PoseTracker::ModelFunction::k3D, time));
time));
}
pose_tracker_->AddOdometerPoseObservation(
time, pose,

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -251,13 +251,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_states_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform =
submap->local_pose.inverse() * pose;
constraints_.push_back(Constraint{
submap_id,
node_id,
{constraint_transform, mapping::FromTranslationRotationWeights(
options_.matcher_translation_weight(),
options_.matcher_rotation_weight())},
Constraint::INTRA_SUBMAP});
constraints_.push_back(
Constraint{submap_id,
node_id,
{constraint_transform, options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();

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

@ -225,9 +225,8 @@ void ConstraintBuilder::ComputeConstraint(
constraint->reset(new OptimizationProblem::Constraint{
submap_id,
node_id,
{constraint_transform, mapping::FromTranslationRotationWeights(
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight())},
{constraint_transform, options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) {

11
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -134,23 +134,22 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 0}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}});
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1.,
1.}});
// We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 1}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
Eigen::Matrix<double, 6, 6>::Identity()}});
// We add very noisy data with high covariance (i.e. small Lambda) to verify
// it is mostly ignored.
1., 1.}});
// We add very noisy data with a low weight to verify it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 2}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
1e-9, 1e-9}});
}
double translation_error_before = 0.;

14
cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h
View File

@ -71,19 +71,23 @@ class SpaCostFunction {
angle_axis_difference[2]}};
}
// Computes the error scaled by 'sqrt_Lambda_ij', storing it in 'e'.
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i_rotation,
const T* const c_i_translation,
const T* const c_j_rotation,
const T* const c_j_translation, T* const e) {
std::array<T, 6> e_ij =
const std::array<T, 6> e_ij =
ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation);
(Eigen::Map<Eigen::Matrix<T, 6, 1>>(e)) =
pose.sqrt_Lambda_ij.cast<T>() *
Eigen::Map<Eigen::Matrix<T, 6, 1>>(e_ij.data());
for (int ij : {0, 1, 2}) {
e[ij] = e_ij[ij] * T(pose.translation_weight);
}
for (int ij : {3, 4, 5}) {
e[ij] = e_ij[ij] * T(pose.rotation_weight);
}
}
template <typename T>

1
cartographer/sensor/data.h
View File

@ -18,7 +18,6 @@
#define CARTOGRAPHER_MAPPING_DATA_H_
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"