From 565e9c3eff2b3e78167ab779bca5ed62697f47fb Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 1 Jun 2017 17:03:49 +0200 Subject: [PATCH] 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. --- cartographer/kalman_filter/pose_tracker.cc | 52 ++----------------- cartographer/kalman_filter/pose_tracker.h | 13 +---- .../kalman_filter/pose_tracker_test.cc | 21 +------- .../mapping/proto/sparse_pose_graph.proto | 7 +-- cartographer/mapping/sparse_pose_graph.cc | 25 ++------- cartographer/mapping/sparse_pose_graph.h | 7 +-- .../proto/ceres_scan_matcher_options.proto | 3 -- cartographer/mapping_2d/sparse_pose_graph.cc | 14 +++-- .../sparse_pose_graph/constraint_builder.cc | 14 +++-- .../sparse_pose_graph/optimization_problem.cc | 12 +---- .../sparse_pose_graph/spa_cost_function.h | 12 ++--- .../kalman_local_trajectory_builder.cc | 4 +- cartographer/mapping_3d/sparse_pose_graph.cc | 13 +++-- .../sparse_pose_graph/constraint_builder.cc | 5 +- .../optimization_problem_test.cc | 11 ++-- .../sparse_pose_graph/spa_cost_function.h | 14 +++-- cartographer/sensor/data.h | 1 - 17 files changed, 59 insertions(+), 169 deletions(-) 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"