From 9e5903fa5af2ce98eabf74c82f7583eac15fb19d Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 12 Nov 2019 19:11:26 -0500 Subject: [PATCH 01/13] add out param to CeresScanMatcher to return seedless_covariance for use in creating asymetric constraints --- .../2d/scan_matching/ceres_scan_matcher_2d.cc | 49 ++++++++++++------- .../2d/scan_matching/ceres_scan_matcher_2d.h | 3 +- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 3590d31d8d..7493439123 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -65,30 +65,28 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* const pose_estimate, - ceres::Solver::Summary* const summary) const { + ceres::Solver::Summary* summary, + Eigen::Matrix3d * seedless_covariance) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()}; ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); - switch (grid.GetGridType()) { - case GridType::PROBABILITY_GRID: - problem.AddResidualBlock( - CreateOccupiedSpaceCostFunction2D( - options_.occupied_space_weight() / - std::sqrt(static_cast(point_cloud.size())), - point_cloud, grid), - nullptr /* loss function */, ceres_pose_estimate); - break; - case GridType::TSDF: - problem.AddResidualBlock( - CreateTSDFMatchCostFunction2D( - options_.occupied_space_weight() / - std::sqrt(static_cast(point_cloud.size())), - point_cloud, static_cast(grid)), - nullptr /* loss function */, ceres_pose_estimate); - break; - } + + auto scalingWeight = options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())); + + auto costFunction = [&]() { + switch (grid.GetGridType()) { + case GridType::PROBABILITY_GRID: + return CreateOccupiedSpaceCostFunction2D(scalingWeight, point_cloud, grid); + case GridType::TSDF: + return CreateTSDFMatchCostFunction2D(scalingWeight, point_cloud, static_cast(grid)); + } + throw std::logic_error("unknown GridType"); + }(); + + problem.AddResidualBlock(costFunction, nullptr /* loss function */, ceres_pose_estimate); + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( @@ -102,6 +100,19 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, ceres::Solve(ceres_solver_options_, &problem, summary); + if(seedless_covariance) + { + auto parameter_blocks = std::array{{ceres_pose_estimate}}; + auto residuals = std::vector(point_cloud.size(), 0.0); + auto row_major_jacobian = Eigen::Matrix(point_cloud.size(), 4); + auto jacobianPtr = row_major_jacobian.data(); // for use to create fake array of jacobians + + costFunction->Evaluate(parameter_blocks.data(), residuals.data(), &jacobianPtr); + + /// FIXME there might be a more efficient inverse for covariance matrices + *seedless_covariance = (row_major_jacobian.transpose() * row_major_jacobian).inverse(); + } + *pose_estimate = transform::Rigid2d( {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); } diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h index 48108d5dee..44d0d902a3 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h @@ -50,7 +50,8 @@ class CeresScanMatcher2D { const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* pose_estimate, - ceres::Solver::Summary* summary) const; + ceres::Solver::Summary* summary, + Eigen::Matrix3d * seedless_covariance = nullptr) const; private: const proto::CeresScanMatcherOptions2D options_; From 1e4e84f01d47b8702b2a29a042149c13095740e7 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 12 Nov 2019 19:37:01 -0500 Subject: [PATCH 02/13] Add in precision matrix to Constraint Pose to allow for asymetric constraints --- .../2d/overlapping_submaps_trimmer_2d_test.cc | 2 +- cartographer/mapping/pose_graph_interface.h | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index 1f2b985b65..cbf6f10f47 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -86,7 +86,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { fake_pose_graph_.mutable_constraints()->push_back( {SubmapId{0 /* trajectory_id */, submap_index}, NodeId{0 /* trajectory_id */, node_index}, - {} /* pose */, + {{},{},{}} /* pose */, is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP : PoseGraphInterface::Constraint::INTER_SUBMAP}); } diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 2701e93d46..f7b397ae4b 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -35,9 +35,26 @@ class PoseGraphInterface { // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. struct Constraint { struct Pose { + Pose(const transform::Rigid3d & zbar_ij_arg, + double translation_weight_arg, + double rotation_weight_arg) + : zbar_ij(zbar_ij_arg), + translation_weight(translation_weight_arg), + rotation_weight(rotation_weight_arg) {}; + + Pose(const transform::Rigid3d & zbar_ij_arg, + double translation_weight_arg, + double rotation_weight_arg, + const Eigen::Matrix3d & precision_arg) + : zbar_ij(zbar_ij_arg), + translation_weight(translation_weight_arg), + rotation_weight(rotation_weight_arg), + precision(precision_arg) {}; + transform::Rigid3d zbar_ij; double translation_weight; double rotation_weight; + Eigen::Matrix3d precision = Eigen::Matrix3d::Identity(); /// FIXME: is this the right size? }; SubmapId submap_id; // 'i' in the paper. From 7a81fe2e7b86c924422a322f5bf2b2de1ff052e2 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Wed, 13 Nov 2019 15:03:26 -0500 Subject: [PATCH 03/13] Add in de/serialization for constraint precision --- cartographer/mapping/pose_graph.cc | 54 ++++++++++++++++++--- cartographer/mapping/proto/pose_graph.proto | 15 ++++++ 2 files changed, 63 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 097e8db515..1cb34d22d2 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -32,7 +32,9 @@ proto::PoseGraph::Constraint::Tag ToProto( case PoseGraph::Constraint::Tag::INTER_SUBMAP: return proto::PoseGraph::Constraint::INTER_SUBMAP; } - LOG(FATAL) << "Unsupported tag."; + auto error = "Unsupported tag."; + LOG(FATAL) << error; + throw std::runtime_error(error); } PoseGraph::Constraint::Tag FromProto( @@ -46,7 +48,38 @@ PoseGraph::Constraint::Tag FromProto( case ::google::protobuf::kint32min: { } } - LOG(FATAL) << "Unsupported tag."; + auto error = "Unsupported tag."; + LOG(FATAL) << error; + throw std::runtime_error(error); +} + +Eigen::Matrix3d FromProto(const proto::Matrix3d &matrix3d_proto) { + auto matrix = Eigen::Matrix3d{}; + matrix + << matrix3d_proto.r0_c1() + , matrix3d_proto.r0_c2() + , matrix3d_proto.r0_c0() + , matrix3d_proto.r1_c1() + , matrix3d_proto.r1_c2() + , matrix3d_proto.r1_c0() + , matrix3d_proto.r2_c1() + , matrix3d_proto.r2_c2() + , matrix3d_proto.r2_c0(); + return matrix; +} + +proto::Matrix3d ToProto(const Eigen::Matrix3d &matrix3d) { + auto matrix3d_proto = proto::Matrix3d{}; + matrix3d_proto.set_r0_c0(matrix3d(0,0)); + matrix3d_proto.set_r1_c0(matrix3d(1,0)); + matrix3d_proto.set_r2_c0(matrix3d(2,0)); + matrix3d_proto.set_r0_c1(matrix3d(0,1)); + matrix3d_proto.set_r1_c1(matrix3d(1,1)); + matrix3d_proto.set_r2_c1(matrix3d(2,1)); + matrix3d_proto.set_r0_c2(matrix3d(0,2)); + matrix3d_proto.set_r1_c2(matrix3d(1,2)); + matrix3d_proto.set_r2_c2(matrix3d(2,2)); + return matrix3d_proto; } std::vector FromProto( @@ -59,10 +92,17 @@ std::vector FromProto( constraint_proto.submap_id().submap_index()}; const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(), constraint_proto.node_id().node_index()}; - const PoseGraph::Constraint::Pose pose{ - transform::ToRigid3(constraint_proto.relative_pose()), - constraint_proto.translation_weight(), - constraint_proto.rotation_weight()}; + const auto pose = [&]() -> PoseGraph::Constraint::Pose { + auto relative_pose = transform::ToRigid3(constraint_proto.relative_pose()); + auto translation_weight = constraint_proto.translation_weight(); + auto rotation_weight = constraint_proto.rotation_weight(); + + if(constraint_proto.has_precision()) + return { relative_pose, translation_weight, rotation_weight}; + + auto precision = FromProto(constraint_proto.precision()); + return { relative_pose, translation_weight, rotation_weight, precision }; + }(); const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag()); constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag}); } @@ -120,6 +160,8 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) { transform::ToProto(constraint.pose.zbar_ij); constraint_proto.set_translation_weight(constraint.pose.translation_weight); constraint_proto.set_rotation_weight(constraint.pose.rotation_weight); + if(not constraint.pose.precision.isIdentity()) + *constraint_proto.mutable_precision() = ToProto(constraint.pose.precision); 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/proto/pose_graph.proto b/cartographer/mapping/proto/pose_graph.proto index 76c10b8ea7..41bd83438f 100644 --- a/cartographer/mapping/proto/pose_graph.proto +++ b/cartographer/mapping/proto/pose_graph.proto @@ -29,6 +29,18 @@ message NodeId { int32 node_index = 2; // Node index in the given trajectory. } +message Matrix3d { // col major order + double r0_c0 = 1; + double r1_c0 = 2; + double r2_c0 = 3; + double r0_c1 = 4; + double r1_c1 = 5; + double r2_c1 = 6; + double r0_c2 = 7; + double r1_c2 = 8; + double r2_c2 = 9; +} + message PoseGraph { message Constraint { // Differentiates between intra-submap (where the range data was inserted @@ -48,6 +60,9 @@ message PoseGraph { double translation_weight = 6; // Weight of the rotational part of the constraint. double rotation_weight = 7; + // precision with which the relative_pose is known we will assume identity if not set + oneof maybe_precision { Matrix3d precision = 8; }; + Tag tag = 5; } From d9ba9a912adfffa1a2f11bad1f3e3a34c38dd92f Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 12 Nov 2019 19:44:50 -0500 Subject: [PATCH 04/13] Fill in constraint precision by calculating from ceres scan matcher seedless covariance --- .../internal/constraints/constraint_builder_2d.cc | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index b05cad2ed5..61811b738d 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -258,21 +258,31 @@ void ConstraintBuilder2D::ComputeConstraint( // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original // CSM estimate. + ceres::Solver::Summary unused_summary; + Eigen::Matrix3d seedless_covariance; ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate, constant_data->filtered_gravity_aligned_point_cloud, *submap_scan_matcher.grid, &pose_estimate, - &unused_summary); + &unused_summary, &seedless_covariance); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; + + auto seedless_precision = Eigen::Matrix3d(seedless_covariance.inverse().eval()); + auto scaled_seedless_precision = Eigen::Matrix3d((seedless_precision * 0.0005).eval()); // FIXME: has fudge factor + scaled_seedless_precision.row(2) = Eigen::Vector3d{0,0,1}; // FIXME not sure what to do with theta parts + scaled_seedless_precision.col(2) = Eigen::Vector3d{0,0,1}; // FIXME not sure what to do with theta parts + constraint->reset(new Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), options_.loop_closure_translation_weight(), - options_.loop_closure_rotation_weight()}, + options_.loop_closure_rotation_weight(), + scaled_seedless_precision}, Constraint::INTER_SUBMAP}); + if (options_.log_matches()) { std::ostringstream info; info << "Node " << node_id << " with " From af3711aaa1e1c9c58f1e694f33d8474458797ea7 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 12 Nov 2019 19:51:44 -0500 Subject: [PATCH 05/13] Use stored constraint precision to solve the SpaCostFunction with more weight on low variance directions, and vice versa --- .../cost_functions/spa_cost_function_2d.cc | 34 +++++++++++++++---- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc index 4c3aee9cb6..4dfef2d0b0 100644 --- a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc @@ -37,15 +37,32 @@ class SpaCostFunction2D { const PoseGraphInterface::Constraint::Pose& observed_relative_pose) : observed_relative_pose_(observed_relative_pose) {} + template static Eigen::Matrix ConvertCovariance(const Eigen::Matrix3d & covariance) + { + auto returnValue = Eigen::Matrix{}; + returnValue << + T{covariance(0,0)}, T{covariance(0,1)}, T{covariance(0,2)}, + T{covariance(1,0)}, T{covariance(1,1)}, T{covariance(1,2)}, + T{covariance(2,0)}, T{covariance(2,1)}, T{covariance(2,2)}; + return returnValue; + } + template - bool operator()(const T* const start_pose, const T* const end_pose, - T* e) const { - const std::array error = - ScaleError(ComputeUnscaledError( - transform::Project2D(observed_relative_pose_.zbar_ij), - start_pose, end_pose), + bool operator()(const T* const start_pose, const T* const end_pose, T* e) const { + auto raw_error = ComputeUnscaledError( + transform::Project2D(observed_relative_pose_.zbar_ij), start_pose, end_pose); + + auto precision = ConvertCovariance(observed_relative_pose_.precision); + + using ErrorMatrixMap = Eigen::Map>; + std::array transformed_error; + ErrorMatrixMap(transformed_error.data()) = precision * ErrorMatrixMap(raw_error.data()); + + auto error = + ScaleError(transformed_error, observed_relative_pose_.translation_weight, observed_relative_pose_.rotation_weight); + std::copy(std::begin(error), std::end(error), e); return true; } @@ -54,6 +71,11 @@ class SpaCostFunction2D { const PoseGraphInterface::Constraint::Pose observed_relative_pose_; }; +template <> Eigen::Matrix SpaCostFunction2D::ConvertCovariance(const Eigen::Matrix3d & covariance) +{ + return covariance; +} + class AnalyticalSpaCostFunction2D : public ceres::SizedCostFunction<3 /* number of residuals */, 3 /* size of start pose */, From fabc014f9284863b5cd455f43d2925d877c7353a Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 26 Nov 2019 15:25:21 -0500 Subject: [PATCH 06/13] fix missing copy construction --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 5519090bcc..62bf23527c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -789,7 +789,10 @@ void PoseGraph2D::AddSerializedConstraints( transform::Rigid3d::Rotation( data_.trajectory_nodes.at(constraint.node_id) .constant_data->gravity_alignment.inverse()), - constraint.pose.translation_weight, constraint.pose.rotation_weight}; + constraint.pose.translation_weight, + constraint.pose.rotation_weight, + constraint.pose.precision + }; data_.constraints.push_back(Constraint{ constraint.submap_id, constraint.node_id, pose, constraint.tag}); } @@ -1003,7 +1006,9 @@ std::vector PoseGraph2D::constraints() const { data_.trajectory_nodes.at(constraint.node_id) .constant_data->gravity_alignment), constraint.pose.translation_weight, - constraint.pose.rotation_weight}, + constraint.pose.rotation_weight, + constraint.pose.precision + }, constraint.tag}); } return result; From d4d857d15d97b9cd4dba5d5173db27a59b46d7b1 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 26 Nov 2019 15:30:11 -0500 Subject: [PATCH 07/13] rename precision to weight_matrix in constraint --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 4 ++-- .../cost_functions/spa_cost_function_2d.cc | 6 +++--- cartographer/mapping/pose_graph.cc | 4 ++-- cartographer/mapping/pose_graph_interface.h | 10 ++++++---- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 62bf23527c..a518a737cf 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -791,7 +791,7 @@ void PoseGraph2D::AddSerializedConstraints( .constant_data->gravity_alignment.inverse()), constraint.pose.translation_weight, constraint.pose.rotation_weight, - constraint.pose.precision + constraint.pose.weight_matrix }; data_.constraints.push_back(Constraint{ constraint.submap_id, constraint.node_id, pose, constraint.tag}); @@ -1007,7 +1007,7 @@ std::vector PoseGraph2D::constraints() const { .constant_data->gravity_alignment), constraint.pose.translation_weight, constraint.pose.rotation_weight, - constraint.pose.precision + constraint.pose.weight_matrix }, constraint.tag}); } diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc index 4dfef2d0b0..fc07a02c98 100644 --- a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc @@ -52,11 +52,11 @@ class SpaCostFunction2D { auto raw_error = ComputeUnscaledError( transform::Project2D(observed_relative_pose_.zbar_ij), start_pose, end_pose); - auto precision = ConvertCovariance(observed_relative_pose_.precision); + auto error_weight_mat = ConvertCovariance(observed_relative_pose_.weight_matrix); using ErrorMatrixMap = Eigen::Map>; - std::array transformed_error; - ErrorMatrixMap(transformed_error.data()) = precision * ErrorMatrixMap(raw_error.data()); + auto transformed_error = std::array{}; + ErrorMatrixMap(transformed_error.data()) = error_weight_mat * ErrorMatrixMap(raw_error.data()); auto error = ScaleError(transformed_error, diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 1cb34d22d2..a750859977 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -160,8 +160,8 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) { transform::ToProto(constraint.pose.zbar_ij); constraint_proto.set_translation_weight(constraint.pose.translation_weight); constraint_proto.set_rotation_weight(constraint.pose.rotation_weight); - if(not constraint.pose.precision.isIdentity()) - *constraint_proto.mutable_precision() = ToProto(constraint.pose.precision); + if(not constraint.pose.weight_matrix.isIdentity()) + *constraint_proto.mutable_precision() = ToProto(constraint.pose.weight_matrix); 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/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index f7b397ae4b..c333ca30f1 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -49,12 +49,14 @@ class PoseGraphInterface { : zbar_ij(zbar_ij_arg), translation_weight(translation_weight_arg), rotation_weight(rotation_weight_arg), - precision(precision_arg) {}; + weight_matrix(precision_arg) {}; + + Pose(const Pose & pose) = default; transform::Rigid3d zbar_ij; - double translation_weight; - double rotation_weight; - Eigen::Matrix3d precision = Eigen::Matrix3d::Identity(); /// FIXME: is this the right size? + double translation_weight; /// FIXME replace with weight matrix? + double rotation_weight; /// FIXME replace with weight matrix? + Eigen::Matrix3d weight_matrix = Eigen::Matrix3d::Identity(); /// FIXME: is this the right size? }; SubmapId submap_id; // 'i' in the paper. From 14bf6948b3145add47c1dd983b9778e4512ef037 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 26 Nov 2019 15:34:27 -0500 Subject: [PATCH 08/13] add precision option to constraint_build_options, and fix precision pass through to use choleskey instead of full precision --- .../internal/constraints/constraint_builder_2d.cc | 14 ++++++++------ .../pose_graph/constraint_builder_options.proto | 2 ++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index 61811b738d..14a44398c7 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -269,20 +269,21 @@ void ConstraintBuilder2D::ComputeConstraint( const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - auto seedless_precision = Eigen::Matrix3d(seedless_covariance.inverse().eval()); - auto scaled_seedless_precision = Eigen::Matrix3d((seedless_precision * 0.0005).eval()); // FIXME: has fudge factor - scaled_seedless_precision.row(2) = Eigen::Vector3d{0,0,1}; // FIXME not sure what to do with theta parts - scaled_seedless_precision.col(2) = Eigen::Vector3d{0,0,1}; // FIXME not sure what to do with theta parts + auto seedless_precision = options_.use_precision_matrix() + ? Eigen::Matrix3d(seedless_covariance.inverse().eval()) + : Eigen::Matrix3d::Identity(); + + auto seedless_precision_chol_up = Eigen::Matrix3d(seedless_precision.llt().matrixU()); + seedless_precision_chol_up = seedless_precision_chol_up * Eigen::Vector3d(0.02, 0.02, 0.01).asDiagonal(); // FIXME fudge factor constraint->reset(new Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), options_.loop_closure_translation_weight(), options_.loop_closure_rotation_weight(), - scaled_seedless_precision}, + seedless_precision_chol_up}, Constraint::INTER_SUBMAP}); - if (options_.log_matches()) { std::ostringstream info; info << "Node " << node_id << " with " @@ -298,6 +299,7 @@ void ConstraintBuilder2D::ComputeConstraint( << std::setprecision(3) << std::abs(difference.normalized_angle()); } info << " with score " << std::setprecision(1) << 100. * score << "%."; + info << "\nPrecision was: \n" << seedless_precision; LOG(INFO) << info.str(); } } diff --git a/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto b/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto index 8447cbaeaf..5abbdd10dd 100644 --- a/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto +++ b/cartographer/mapping/proto/pose_graph/constraint_builder_options.proto @@ -56,4 +56,6 @@ message ConstraintBuilderOptions { fast_correlative_scan_matcher_options_3d = 10; mapping.scan_matching.proto.CeresScanMatcherOptions3D ceres_scan_matcher_options_3d = 12; + + bool use_precision_matrix = 15; } From efe90522fe2b40af0b45f650bcf54dd41699c138 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Tue, 26 Nov 2019 16:04:21 -0500 Subject: [PATCH 09/13] switch from covariance to precision to avoid unused matrix inversion --- .../2d/scan_matching/ceres_scan_matcher_2d.cc | 7 +++---- .../constraints/constraint_builder_2d.cc | 17 ++++++++++------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 7493439123..caa019b642 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -66,7 +66,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, const Grid2D& grid, transform::Rigid2d* const pose_estimate, ceres::Solver::Summary* summary, - Eigen::Matrix3d * seedless_covariance) const { + Eigen::Matrix3d * seedless_precision) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()}; @@ -100,7 +100,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, ceres::Solve(ceres_solver_options_, &problem, summary); - if(seedless_covariance) + if(seedless_precision) { auto parameter_blocks = std::array{{ceres_pose_estimate}}; auto residuals = std::vector(point_cloud.size(), 0.0); @@ -109,8 +109,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, costFunction->Evaluate(parameter_blocks.data(), residuals.data(), &jacobianPtr); - /// FIXME there might be a more efficient inverse for covariance matrices - *seedless_covariance = (row_major_jacobian.transpose() * row_major_jacobian).inverse(); + *seedless_precision = (row_major_jacobian.transpose() * row_major_jacobian); } *pose_estimate = transform::Rigid2d( diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index 14a44398c7..443a126006 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -260,21 +260,24 @@ void ConstraintBuilder2D::ComputeConstraint( // CSM estimate. ceres::Solver::Summary unused_summary; - Eigen::Matrix3d seedless_covariance; + Eigen::Matrix3d seedless_precision; ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate, constant_data->filtered_gravity_aligned_point_cloud, *submap_scan_matcher.grid, &pose_estimate, - &unused_summary, &seedless_covariance); + &unused_summary, &seedless_precision); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - auto seedless_precision = options_.use_precision_matrix() - ? Eigen::Matrix3d(seedless_covariance.inverse().eval()) - : Eigen::Matrix3d::Identity(); + auto seedless_precision_chol_up = [&](){ + if (not options_.use_precision_matrix()) + return Eigen::Matrix3d::Identity().eval(); - auto seedless_precision_chol_up = Eigen::Matrix3d(seedless_precision.llt().matrixU()); - seedless_precision_chol_up = seedless_precision_chol_up * Eigen::Vector3d(0.02, 0.02, 0.01).asDiagonal(); // FIXME fudge factor + return Eigen::Matrix3d( + Eigen::Matrix3d(seedless_precision.llt().matrixU()) // Choleskey + * Eigen::Vector3d(0.02, 0.02, 0.01).asDiagonal() // FIXME fudge factor to match normal constraint SPA error + ); + }(); constraint->reset(new Constraint{submap_id, node_id, From 9f8fdc7dc66b2c48ba0e8ff02be9e10cbb97c43d Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Wed, 27 Nov 2019 15:13:04 -0500 Subject: [PATCH 10/13] fix covariance->precision naming --- .../mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h index 44d0d902a3..a5ce03a3e8 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h @@ -51,7 +51,7 @@ class CeresScanMatcher2D { const sensor::PointCloud& point_cloud, const Grid2D& grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary, - Eigen::Matrix3d * seedless_covariance = nullptr) const; + Eigen::Matrix3d * seedless_precision = nullptr) const; private: const proto::CeresScanMatcherOptions2D options_; From b19fed8450271d9c3beec0e6b18cd34f9ce75fe5 Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Wed, 27 Nov 2019 15:13:40 -0500 Subject: [PATCH 11/13] fix naming covariance-> mat or 3x3 in spa cost function --- .../cost_functions/spa_cost_function_2d.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc index fc07a02c98..44f0c21d81 100644 --- a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc @@ -37,13 +37,13 @@ class SpaCostFunction2D { const PoseGraphInterface::Constraint::Pose& observed_relative_pose) : observed_relative_pose_(observed_relative_pose) {} - template static Eigen::Matrix ConvertCovariance(const Eigen::Matrix3d & covariance) + template static Eigen::Matrix Convert3x3(const Eigen::Matrix3d & mat) { auto returnValue = Eigen::Matrix{}; returnValue << - T{covariance(0,0)}, T{covariance(0,1)}, T{covariance(0,2)}, - T{covariance(1,0)}, T{covariance(1,1)}, T{covariance(1,2)}, - T{covariance(2,0)}, T{covariance(2,1)}, T{covariance(2,2)}; + T{mat(0,0)}, T{mat(0,1)}, T{mat(0,2)}, + T{mat(1,0)}, T{mat(1,1)}, T{mat(1,2)}, + T{mat(2,0)}, T{mat(2,1)}, T{mat(2,2)}; return returnValue; } @@ -52,7 +52,7 @@ class SpaCostFunction2D { auto raw_error = ComputeUnscaledError( transform::Project2D(observed_relative_pose_.zbar_ij), start_pose, end_pose); - auto error_weight_mat = ConvertCovariance(observed_relative_pose_.weight_matrix); + auto error_weight_mat = Convert3x3(observed_relative_pose_.weight_matrix); using ErrorMatrixMap = Eigen::Map>; auto transformed_error = std::array{}; @@ -71,9 +71,9 @@ class SpaCostFunction2D { const PoseGraphInterface::Constraint::Pose observed_relative_pose_; }; -template <> Eigen::Matrix SpaCostFunction2D::ConvertCovariance(const Eigen::Matrix3d & covariance) +template <> Eigen::Matrix SpaCostFunction2D::Convert3x3(const Eigen::Matrix3d & mat) { - return covariance; + return mat; } class AnalyticalSpaCostFunction2D From ef0f3357a160b12bfd948d97e2a7f1bd38a841ff Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Wed, 27 Nov 2019 15:17:18 -0500 Subject: [PATCH 12/13] change precision to weight matrix --- cartographer/mapping/pose_graph.cc | 8 ++++---- cartographer/mapping/proto/pose_graph.proto | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index a750859977..805f0ec5ac 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -97,11 +97,11 @@ std::vector FromProto( auto translation_weight = constraint_proto.translation_weight(); auto rotation_weight = constraint_proto.rotation_weight(); - if(constraint_proto.has_precision()) + if(constraint_proto.has_weight_matrix()) return { relative_pose, translation_weight, rotation_weight}; - auto precision = FromProto(constraint_proto.precision()); - return { relative_pose, translation_weight, rotation_weight, precision }; + auto weight_matrix = FromProto(constraint_proto.weight_matrix()); + return { relative_pose, translation_weight, rotation_weight, weight_matrix }; }(); const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag()); constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag}); @@ -161,7 +161,7 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) { constraint_proto.set_translation_weight(constraint.pose.translation_weight); constraint_proto.set_rotation_weight(constraint.pose.rotation_weight); if(not constraint.pose.weight_matrix.isIdentity()) - *constraint_proto.mutable_precision() = ToProto(constraint.pose.weight_matrix); + *constraint_proto.mutable_weight_matrix() = ToProto(constraint.pose.weight_matrix); 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/proto/pose_graph.proto b/cartographer/mapping/proto/pose_graph.proto index 41bd83438f..f0d207f136 100644 --- a/cartographer/mapping/proto/pose_graph.proto +++ b/cartographer/mapping/proto/pose_graph.proto @@ -61,7 +61,7 @@ message PoseGraph { // Weight of the rotational part of the constraint. double rotation_weight = 7; // precision with which the relative_pose is known we will assume identity if not set - oneof maybe_precision { Matrix3d precision = 8; }; + oneof maybe_weight_matrix { Matrix3d weight_matrix = 8; }; Tag tag = 5; } From 97abc83f92b61cdf7fcd29659de4da8d8edcaded Mon Sep 17 00:00:00 2001 From: Andrew Somerville Date: Wed, 27 Nov 2019 16:49:10 -0500 Subject: [PATCH 13/13] fix serialization --- cartographer/mapping/pose_graph.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 805f0ec5ac..87932e5c60 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -98,10 +98,12 @@ std::vector FromProto( auto rotation_weight = constraint_proto.rotation_weight(); if(constraint_proto.has_weight_matrix()) - return { relative_pose, translation_weight, rotation_weight}; + { + auto weight_matrix = FromProto(constraint_proto.weight_matrix()); + return { relative_pose, translation_weight, rotation_weight, weight_matrix }; + } - auto weight_matrix = FromProto(constraint_proto.weight_matrix()); - return { relative_pose, translation_weight, rotation_weight, weight_matrix }; + return { relative_pose, translation_weight, rotation_weight}; }(); const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag()); constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag});