diff --git a/cartographer/kalman_filter/pose_tracker.cc b/cartographer/kalman_filter/pose_tracker.cc index 27c8fe7..be3154e 100644 --- a/cartographer/kalman_filter/pose_tracker.cc +++ b/cartographer/kalman_filter/pose_tracker.cc @@ -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 = diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h index c27542a..84db576 100644 --- a/cartographer/kalman_filter/pose_tracker.h +++ b/cartographer/kalman_filter/pose_tracker.h @@ -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; using State = KalmanFilter::StateType; using StateCovariance = Eigen::Matrix; @@ -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_; diff --git a/cartographer/kalman_filter/pose_tracker_test.cc b/cartographer/kalman_filter/pose_tracker_test.cc index c43145a..dcf47d3 100644 --- a/cartographer/kalman_filter/pose_tracker_test.cc +++ b/cartographer/kalman_filter/pose_tracker_test.cc @@ -50,30 +50,13 @@ class PoseTrackerTest : public ::testing::Test { )text"); const proto::PoseTrackerOptions options = CreatePoseTrackerOptions(parameter_dictionary.get()); - pose_tracker_ = common::make_unique( - options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000)); + pose_tracker_ = + common::make_unique(options, common::FromUniversal(1000)); } std::unique_ptr pose_tracker_; }; -TEST(CovarianceTest, EmbedAndProjectCovariance) { - std::mt19937 prng(42); - std::uniform_real_distribution 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 poses(3); std::vector covariances(3); diff --git a/cartographer/mapping/proto/sparse_pose_graph.proto b/cartographer/mapping/proto/sparse_pose_graph.proto index edde43e..64f043f 100644 --- a/cartographer/mapping/proto/sparse_pose_graph.proto +++ b/cartographer/mapping/proto/sparse_pose_graph.proto @@ -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; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 0b46a3a..828d0ae 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -58,20 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( return options; } -Eigen::Matrix FromTranslationRotationWeights( - const double translation_weight, const double rotation_weight) { - Eigen::Matrix 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>( - 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( diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 83915d6..a1a43ed 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -35,10 +35,6 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); -// TODO(whess): Change to two doubles for performance. -Eigen::Matrix 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 sqrt_Lambda_ij; + double translation_weight; + double rotation_weight; }; mapping::SubmapId submap_id; // 'i' in the paper. diff --git a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto index f10e7b2..b1fec7c 100644 --- a/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto +++ b/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto @@ -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; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 1da0f24..c320215 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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(); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index aa5547f..3f9a4c9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -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; diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 24d845c..4bd0d87 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -155,17 +155,10 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } // Add penalties for changes between consecutive scans. - const Eigen::DiagonalMatrix 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( new SpaCostFunction(Constraint::Pose{ @@ -173,9 +166,8 @@ void OptimizationProblem::Solve(const std::vector& 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()); diff --git a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h index 5d07956..3976925 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h @@ -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 static void ComputeScaledError(const Constraint::Pose& pose, const T* const c_i, const T* const c_j, T* const e) { - std::array e_ij = + const std::array e_ij = ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j); - (Eigen::Map>(e)) = - kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast() * - Eigen::Map>(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 diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 15526d6..23e40c4 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -57,7 +57,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData( pose_tracker_ = common::make_unique( 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, diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 5ae5d36..c216715 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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(); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index dfabaae..0ce6050 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -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()) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 3bc8ad6..336cc99 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -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::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::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::Identity()}}); + 1e-9, 1e-9}}); } double translation_error_before = 0.; diff --git a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h index ae08806..d1d2c7f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -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 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 e_ij = + const std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation, c_j_translation); - (Eigen::Map>(e)) = - pose.sqrt_Lambda_ij.cast() * - Eigen::Map>(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 diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 68e2ace..7d34240 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -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"