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. // Build a model matrix for the given time delta.
PoseTracker::State ModelFunction3D(const PoseTracker::State& state, PoseTracker::State ModelFunction(const PoseTracker::State& state,
const double delta_t) { const double delta_t) {
CHECK_GT(delta_t, 0.); CHECK_GT(delta_t, 0.);
PoseTracker::State new_state; PoseTracker::State new_state;
@ -109,18 +109,6 @@ PoseTracker::State ModelFunction3D(const PoseTracker::State& state,
return new_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 } // namespace
PoseAndCovariance operator*(const transform::Rigid3d& transform, PoseAndCovariance operator*(const transform::Rigid3d& transform,
@ -163,10 +151,8 @@ PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
} }
PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options, PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
const ModelFunction& model_function,
const common::Time time) const common::Time time)
: options_(options), : options_(options),
model_function_(model_function),
time_(time), time_(time),
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta), kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
imu_tracker_(options.imu_gravity_time_constant(), time), imu_tracker_(options.imu_gravity_time_constant(), time),
@ -228,14 +214,7 @@ void PoseTracker::Predict(const common::Time time) {
} }
kalman_filter_.Predict( kalman_filter_.Predict(
[this, delta_t](const State& state) -> State { [this, delta_t](const State& state) -> State {
switch (model_function_) { return ModelFunction(state, delta_t);
case ModelFunction::k2D:
return ModelFunction2D(state, delta_t);
case ModelFunction::k3D:
return ModelFunction3D(state, delta_t);
default:
LOG(FATAL);
}
}, },
BuildModelNoise(delta_t)); BuildModelNoise(delta_t));
time_ = time; time_ = time;
@ -318,31 +297,6 @@ transform::Rigid3d PoseTracker::RigidFromState(
imu_tracker_.orientation()); 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, PoseCovariance BuildPoseCovariance(const double translational_variance,
const double rotational_variance) { const double rotational_variance) {
const Eigen::Matrix3d translational = 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, PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance); 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, PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance); double rotational_variance);
@ -77,8 +70,6 @@ class PoseTracker {
kDimension // We terminate loops with this. kDimension // We terminate loops with this.
}; };
enum class ModelFunction { k2D, k3D };
using KalmanFilter = UnscentedKalmanFilter<double, kDimension>; using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
using State = KalmanFilter::StateType; using State = KalmanFilter::StateType;
using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>; 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 // Create a new Kalman filter starting at the origin with a standard normal
// distribution at 'time'. // distribution at 'time'.
PoseTracker(const proto::PoseTrackerOptions& options, PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
const ModelFunction& model_function, common::Time time);
virtual ~PoseTracker(); virtual ~PoseTracker();
// Sets 'pose' and 'covariance' to the mean and covariance of the belief at // 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); transform::Rigid3d RigidFromState(const PoseTracker::State& state);
const proto::PoseTrackerOptions options_; const proto::PoseTrackerOptions options_;
const ModelFunction model_function_;
common::Time time_; common::Time time_;
KalmanFilter kalman_filter_; KalmanFilter kalman_filter_;
mapping::ImuTracker imu_tracker_; mapping::ImuTracker imu_tracker_;

21
cartographer/kalman_filter/pose_tracker_test.cc
View File

@ -50,30 +50,13 @@ class PoseTrackerTest : public ::testing::Test {
)text"); )text");
const proto::PoseTrackerOptions options = const proto::PoseTrackerOptions options =
CreatePoseTrackerOptions(parameter_dictionary.get()); CreatePoseTrackerOptions(parameter_dictionary.get());
pose_tracker_ = common::make_unique<PoseTracker>( pose_tracker_ =
options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000)); common::make_unique<PoseTracker>(options, common::FromUniversal(1000));
} }
std::unique_ptr<PoseTracker> pose_tracker_; 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) { TEST_F(PoseTrackerTest, SaveAndRestore) {
std::vector<Rigid3d> poses(3); std::vector<Rigid3d> poses(3);
std::vector<PoseCovariance> covariances(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 // Pose of the scan relative to submap, i.e. taking data from the scan frame
// into the submap frame. // into the submap frame.
optional transform.proto.Rigid3d relative_pose = 3; optional transform.proto.Rigid3d relative_pose = 3;
// 6x6 square root inverse of the covariance matrix. This is stored in // Weight of the translational part of the constraint.
// column-major order for ease of use with Eigen::Map. optional double translation_weight = 6;
repeated double sqrt_Lambda = 4 [packed = true]; // NOLINT // Weight of the rotational part of the constraint.
optional double rotation_weight = 7;
optional Tag tag = 5; optional Tag tag = 5;
} }

25
cartographer/mapping/sparse_pose_graph.cc
View File

@ -58,20 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
return options; 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 SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto; proto::SparsePoseGraph proto;
@ -79,14 +65,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
auto* const constraint_proto = proto.add_constraint(); auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() = *constraint_proto->mutable_relative_pose() =
transform::ToProto(constraint.pose.zbar_ij); transform::ToProto(constraint.pose.zbar_ij);
constraint_proto->mutable_sqrt_lambda()->Reserve(36); constraint_proto->set_translation_weight(
for (int i = 0; i != 36; ++i) { constraint.pose.translation_weight);
constraint_proto->mutable_sqrt_lambda()->Add(0.); constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
}
Eigen::Map<Eigen::Matrix<double, 6, 6>>(
constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
constraint.pose.sqrt_Lambda_ij;
constraint_proto->mutable_submap_id()->set_trajectory_id( constraint_proto->mutable_submap_id()->set_trajectory_id(
constraint.submap_id.trajectory_id); constraint.submap_id.trajectory_id);
constraint_proto->mutable_submap_id()->set_submap_index( 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( proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary); 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 { class SparsePoseGraph {
public: public:
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
@ -47,7 +43,8 @@ class SparsePoseGraph {
struct Constraint { struct Constraint {
struct Pose { struct Pose {
transform::Rigid3d zbar_ij; 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. 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 translation_weight = 2;
optional double rotation_weight = 3; 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 // Configure the Ceres solver. See the Ceres documentation for more
// information: https://code.google.com/p/ceres-solver/ // information: https://code.google.com/p/ceres-solver/
optional common.proto.CeresSolverOptions ceres_solver_options = 9; 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); submap_states_.at(submap_id).node_ids.emplace(node_id);
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(Constraint{submap_id,
Constraint{submap_id, node_id,
node_id, {transform::Embed3D(constraint_transform),
{transform::Embed3D(constraint_transform), options_.matcher_translation_weight(),
mapping::FromTranslationRotationWeights( options_.matcher_rotation_weight()},
options_.matcher_translation_weight(), Constraint::INTRA_SUBMAP});
options_.matcher_rotation_weight())},
Constraint::INTRA_SUBMAP});
} }
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); 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 = const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate; ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset( constraint->reset(new Constraint{submap_id,
new Constraint{submap_id, node_id,
node_id, {transform::Embed3D(constraint_transform),
{transform::Embed3D(constraint_transform), options_.loop_closure_translation_weight(),
mapping::FromTranslationRotationWeights( options_.loop_closure_rotation_weight()},
options_.loop_closure_translation_weight(), Constraint::INTER_SUBMAP});
options_.loop_closure_rotation_weight())},
Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {
std::ostringstream info; 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. // 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(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_index = 1; node_index < node_data_[trajectory_id].size(); for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
++node_index) { ++node_index) {
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
@ -173,9 +166,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
.initial_point_cloud_pose.inverse() * .initial_point_cloud_pose.inverse() *
node_data_[trajectory_id][node_index] node_data_[trajectory_id][node_index]
.initial_point_cloud_pose), .initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, options_.consecutive_scan_translation_penalty_factor(),
kUnusedPositionPenalty, options_.consecutive_scan_rotation_penalty_factor()})),
kUnusedOrientationPenalty)})),
nullptr /* loss function */, nullptr /* loss function */,
C_nodes[trajectory_id][node_index - 1].data(), C_nodes[trajectory_id][node_index - 1].data(),
C_nodes[trajectory_id][node_index].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/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -59,16 +58,17 @@ class SpaCostFunction {
h[2])}}; 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> template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose, static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i, const T* const c_j, const T* const c_i, const T* const c_j,
T* const e) { 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); ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
(Eigen::Map<Eigen::Matrix<T, 3, 1>>(e)) = e[0] = e_ij[0] * T(pose.translation_weight);
kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast<T>() * e[1] = e_ij[1] * T(pose.translation_weight);
Eigen::Map<Eigen::Matrix<T, 3, 1>>(e_ij.data()); e[2] = e_ij[2] * T(pose.rotation_weight);
} }
template <typename T> 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>( pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
options_.kalman_local_trajectory_builder_options() options_.kalman_local_trajectory_builder_options()
.pose_tracker_options(), .pose_tracker_options(),
kalman_filter::PoseTracker::ModelFunction::k3D, time); time);
} }
pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
@ -196,7 +196,7 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData(
pose_tracker_.reset(new kalman_filter::PoseTracker( pose_tracker_.reset(new kalman_filter::PoseTracker(
options_.kalman_local_trajectory_builder_options() options_.kalman_local_trajectory_builder_options()
.pose_tracker_options(), .pose_tracker_options(),
kalman_filter::PoseTracker::ModelFunction::k3D, time)); time));
} }
pose_tracker_->AddOdometerPoseObservation( pose_tracker_->AddOdometerPoseObservation(
time, pose, 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); submap_states_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose.inverse() * pose; submap->local_pose.inverse() * pose;
constraints_.push_back(Constraint{ constraints_.push_back(
submap_id, Constraint{submap_id,
node_id, node_id,
{constraint_transform, mapping::FromTranslationRotationWeights( {constraint_transform, options_.matcher_translation_weight(),
options_.matcher_translation_weight(), options_.matcher_rotation_weight()},
options_.matcher_rotation_weight())}, Constraint::INTRA_SUBMAP});
Constraint::INTRA_SUBMAP});
} }
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); 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{ constraint->reset(new OptimizationProblem::Constraint{
submap_id, submap_id,
node_id, node_id,
{constraint_transform, mapping::FromTranslationRotationWeights( {constraint_transform, options_.loop_closure_translation_weight(),
options_.loop_closure_translation_weight(), options_.loop_closure_rotation_weight()},
options_.loop_closure_rotation_weight())},
OptimizationProblem::Constraint::INTER_SUBMAP}); OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) { 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{ constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 0}, mapping::NodeId{0, j}, mapping::SubmapId{0, 0}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{ OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1.,
Eigen::Matrix<double, 6, 6>::Identity()}}); 1.}});
// We add an additional independent, but equally noisy observation. // We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{ constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 1}, mapping::NodeId{0, j}, mapping::SubmapId{0, 1}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{ OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)), RandomYawOnlyTransform(0.2, 0.3)),
Eigen::Matrix<double, 6, 6>::Identity()}}); 1., 1.}});
// We add very noisy data with high covariance (i.e. small Lambda) to verify // We add very noisy data with a low weight to verify it is mostly ignored.
// it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{ constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 2}, mapping::NodeId{0, j}, mapping::SubmapId{0, 2}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{ OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.), RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}}); 1e-9, 1e-9}});
} }
double translation_error_before = 0.; 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]}}; 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> template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose, static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i_rotation, const T* const c_i_rotation,
const T* const c_i_translation, const T* const c_i_translation,
const T* const c_j_rotation, const T* const c_j_rotation,
const T* const c_j_translation, T* const e) { 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, ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation); c_j_rotation, c_j_translation);
(Eigen::Map<Eigen::Matrix<T, 6, 1>>(e)) = for (int ij : {0, 1, 2}) {
pose.sqrt_Lambda_ij.cast<T>() * e[ij] = e_ij[ij] * T(pose.translation_weight);
Eigen::Map<Eigen::Matrix<T, 6, 1>>(e_ij.data()); }
for (int ij : {3, 4, 5}) {
e[ij] = e_ij[ij] * T(pose.rotation_weight);
}
} }
template <typename T> template <typename T>

1
cartographer/sensor/data.h
View File

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