From c80c2eaa40151f8afa69f3e1630fdf526178f6cc Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 8 Nov 2016 14:47:03 +0100 Subject: [PATCH] Unify 2D/3D constraints. (#119) This removes Constraint2D and instead embeds 2D constraints in a Constraint3D which is renamed to Constraint. --- cartographer/mapping/sparse_pose_graph.cc | 38 ++----------------- cartographer/mapping/sparse_pose_graph.h | 30 ++------------- cartographer/mapping_2d/sparse_pose_graph.cc | 20 +++++----- cartographer/mapping_2d/sparse_pose_graph.h | 5 +-- .../sparse_pose_graph/CMakeLists.txt | 4 +- .../sparse_pose_graph/constraint_builder.cc | 17 +++++---- .../sparse_pose_graph/constraint_builder.h | 4 +- .../sparse_pose_graph/optimization_problem.cc | 11 ++++-- .../sparse_pose_graph/optimization_problem.h | 2 +- .../sparse_pose_graph/spa_cost_function.h | 9 +++-- cartographer/mapping_3d/sparse_pose_graph.cc | 22 +++++------ cartographer/mapping_3d/sparse_pose_graph.h | 5 +-- .../sparse_pose_graph/constraint_builder.h | 2 +- .../sparse_pose_graph/optimization_problem.h | 2 +- .../sparse_pose_graph/spa_cost_function.h | 2 +- 15 files changed, 63 insertions(+), 110 deletions(-) diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index ea3b021..91476aa 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -28,22 +28,11 @@ namespace cartographer { namespace mapping { proto::SparsePoseGraph::Constraint::Tag ToProto( - const SparsePoseGraph::Constraint2D::Tag& tag) { + const SparsePoseGraph::Constraint::Tag& tag) { switch (tag) { - case SparsePoseGraph::Constraint2D::Tag::INTRA_SUBMAP: + case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP: return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; - case SparsePoseGraph::Constraint2D::Tag::INTER_SUBMAP: - return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; - } - LOG(FATAL) << "Unsupported tag."; -} - -proto::SparsePoseGraph::Constraint::Tag ToProto( - const SparsePoseGraph::Constraint3D::Tag& tag) { - switch (tag) { - case SparsePoseGraph::Constraint3D::Tag::INTRA_SUBMAP: - return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; - case SparsePoseGraph::Constraint3D::Tag::INTER_SUBMAP: + case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP: return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; } LOG(FATAL) << "Unsupported tag."; @@ -85,26 +74,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; - for (const auto& constraint : constraints_2d()) { - auto* const constraint_proto = proto.add_constraint(); - *constraint_proto->mutable_relative_pose() = - transform::ToProto(transform::Embed3D(constraint.pose.zbar_ij)); - for (int i = 0; i != 36; ++i) { - constraint_proto->mutable_sqrt_lambda()->Add(0.); - } - constexpr double kFakePositionCovariance = 1.; - constexpr double kFakeOrientationCovariance = 1.; - Eigen::Map>( - constraint_proto->mutable_sqrt_lambda()->mutable_data()) = - kalman_filter::Embed3D(constraint.pose.sqrt_Lambda_ij, - kFakePositionCovariance, - kFakeOrientationCovariance); - // TODO(whess): Support multi-trajectory. - constraint_proto->mutable_submap_id()->set_submap_index(constraint.i); - constraint_proto->mutable_scan_id()->set_scan_index(constraint.j); - constraint_proto->set_tag(mapping::ToProto(constraint.tag)); - } - for (const auto& constraint : constraints_3d()) { + for (const auto& constraint : constraints()) { auto* const constraint_proto = proto.add_constraint(); *constraint_proto->mutable_relative_pose() = transform::ToProto(constraint.pose.zbar_ij); diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index bf001a6..22d08f8 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -42,28 +42,7 @@ class SparsePoseGraph { // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. - struct Constraint2D { - struct Pose { - transform::Rigid2d zbar_ij; - Eigen::Matrix sqrt_Lambda_ij; - }; - - // Submap index. - int i; - - // Scan index. - int j; - - // Pose of the scan 'j' relative to submap 'i'. - Pose pose; - - // Differentiates between intra-submap (where scan 'j' was inserted into - // submap 'i') and inter-submap constraints (where scan 'j' was not inserted - // into submap 'i'). - enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; - }; - - struct Constraint3D { + struct Constraint { struct Pose { transform::Rigid3d zbar_ij; Eigen::Matrix sqrt_Lambda_ij; @@ -116,11 +95,8 @@ class SparsePoseGraph { // Returns the current optimized trajectory. virtual std::vector GetTrajectoryNodes() = 0; - // Returns the collection of 2D constraints. - virtual std::vector constraints_2d() = 0; - - // Returns the collection of 3D constraints. - virtual std::vector constraints_3d() = 0; + // Returns the collection of constraints. + virtual std::vector constraints() = 0; // Serializes the constraints and the computed trajectory. // diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 2ecf4d8..dad8d7a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -172,14 +172,18 @@ void SparsePoseGraph::ComputeConstraintsForScan( // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; - constraints_.push_back(Constraint2D{ + constexpr double kFakePositionCovariance = 1.; + constexpr double kFakeOrientationCovariance = 1.; + constraints_.push_back(Constraint{ submap_index, scan_index, - {constraint_transform, + {transform::Embed3D(constraint_transform), common::ComputeSpdMatrixSqrtInverse( - covariance, options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint2D::INTRA_SUBMAP}); + kalman_filter::Embed3D(covariance, kFakePositionCovariance, + kFakeOrientationCovariance), + options_.constraint_builder_options() + .lower_covariance_eigenvalue_bound())}, + Constraint::INTRA_SUBMAP}); } // Determine if this scan should be globally localized. @@ -389,14 +393,10 @@ std::vector SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } -std::vector SparsePoseGraph::constraints_2d() { +std::vector SparsePoseGraph::constraints() { return constraints_; } -std::vector SparsePoseGraph::constraints_3d() { - return {}; -} - transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps& submaps) { return GetSubmapTransforms(submaps).back() * diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 84a1c16..990d7c7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -90,8 +90,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); - std::vector constraints_2d() override; - std::vector constraints_3d() override; + std::vector constraints() override; private: struct SubmapState { @@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); - std::vector constraints_; + std::vector constraints_; std::vector initial_point_cloud_poses_; std::vector point_cloud_poses_; // (map <- point cloud) std::vector submap_transforms_; // (map <- submap) diff --git a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt index 3d7784e..ffe4ac9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt +++ b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt @@ -31,10 +31,10 @@ google_library(mapping_2d_sparse_pose_graph_constraint_builder mapping_2d_scan_matching_fast_correlative_scan_matcher mapping_2d_scan_matching_proto_ceres_scan_matcher_options mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options - mapping_2d_sparse_pose_graph_optimization_problem mapping_2d_submaps mapping_3d_scan_matching_ceres_scan_matcher mapping_3d_scan_matching_fast_correlative_scan_matcher + mapping_sparse_pose_graph mapping_sparse_pose_graph_proto_constraint_builder_options mapping_trajectory_connectivity sensor_point_cloud @@ -70,6 +70,8 @@ google_library(mapping_2d_sparse_pose_graph_spa_cost_function spa_cost_function.h DEPENDS common_math + kalman_filter_pose_tracker mapping_sparse_pose_graph transform_rigid_transform + transform_transform ) diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 4a96406..6af6513 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -176,7 +176,7 @@ void ConstraintBuilder::ComputeConstraint( mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose, - std::unique_ptr* constraint) { + std::unique_ptr* constraint) { const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; const SubmapScanMatcher* const submap_scan_matcher = @@ -226,13 +226,17 @@ void ConstraintBuilder::ComputeConstraint( const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; - constraint->reset(new OptimizationProblem::Constraint{ + constexpr double kFakePositionCovariance = 1.; + constexpr double kFakeOrientationCovariance = 1.; + constraint->reset(new Constraint{ submap_index, scan_index, - {constraint_transform, + {transform::Embed3D(constraint_transform), common::ComputeSpdMatrixSqrtInverse( - covariance, options_.lower_covariance_eigenvalue_bound())}, - OptimizationProblem::Constraint::INTER_SUBMAP}); + kalman_filter::Embed3D(covariance, kFakePositionCovariance, + kFakeOrientationCovariance), + options_.lower_covariance_eigenvalue_bound())}, + Constraint::INTER_SUBMAP}); if (options_.log_matches()) { const transform::Rigid2d difference = @@ -261,8 +265,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { if (pending_computations_.empty()) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { - for (const std::unique_ptr& - constraint : constraints_) { + for (const std::unique_ptr& constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index b485b76..ff8359e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -30,11 +30,11 @@ #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" -#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" @@ -59,7 +59,7 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap); // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::SparsePoseGraph::Constraint2D; + using Constraint = mapping::SparsePoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index e5bf470..3a8db8b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -134,12 +134,17 @@ void OptimizationProblem::Solve( // This pose has a predecessor. if (last_pose_indices.count(trajectory) != 0) { const int last_pose_index = last_pose_indices[trajectory]; + constexpr double kUnusedPositionPenalty = 1.; + constexpr double kUnusedOrientationPenalty = 1.; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ - initial_point_cloud_poses[last_pose_index].inverse() * - initial_point_cloud_poses[j], - consecutive_pose_change_penalty_matrix})), + transform::Embed3D( + initial_point_cloud_poses[last_pose_index].inverse() * + initial_point_cloud_poses[j]), + kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, + kUnusedPositionPenalty, + kUnusedOrientationPenalty)})), nullptr /* loss function */, C_point_clouds[last_pose_index].data(), C_point_clouds[j].data()); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index f928fb8..be98365 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -36,7 +36,7 @@ namespace sparse_pose_graph { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::SparsePoseGraph::Constraint2D; + using Constraint = mapping::SparsePoseGraph::Constraint; explicit OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& 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 5a5ddd0..5d07956 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h @@ -22,8 +22,10 @@ #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" #include "ceres/ceres.h" #include "ceres/jet.h" @@ -33,7 +35,7 @@ namespace sparse_pose_graph { class SpaCostFunction { public: - using Constraint = mapping::SparsePoseGraph::Constraint2D; + using Constraint = mapping::SparsePoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} @@ -62,9 +64,10 @@ class SpaCostFunction { static void ComputeScaledError(const Constraint::Pose& pose, const T* const c_i, const T* const c_j, T* const e) { - std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j); + std::array e_ij = + ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j); (Eigen::Map>(e)) = - pose.sqrt_Lambda_ij.cast() * + kalman_filter::Project2D(pose.sqrt_Lambda_ij).cast() * Eigen::Map>(e_ij.data()); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index fc60bd8..19d409c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -184,14 +184,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = submap->local_pose().inverse() * pose; - constraints_.push_back(Constraint3D{ - submap_index, - scan_index, - {constraint_transform, - common::ComputeSpdMatrixSqrtInverse( - covariance, options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint3D::INTRA_SUBMAP}); + constraints_.push_back( + Constraint{submap_index, + scan_index, + {constraint_transform, + common::ComputeSpdMatrixSqrtInverse( + covariance, options_.constraint_builder_options() + .lower_covariance_eigenvalue_bound())}, + Constraint::INTRA_SUBMAP}); } // Determine if this scan should be globally localized. @@ -393,11 +393,7 @@ std::vector SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } -std::vector SparsePoseGraph::constraints_2d() { - return {}; -} - -std::vector SparsePoseGraph::constraints_3d() { +std::vector SparsePoseGraph::constraints() { return constraints_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index def9e0c..ef7b912 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -92,8 +92,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); - std::vector constraints_2d() override; - std::vector constraints_3d() override; + std::vector constraints() override; private: struct SubmapState { @@ -179,7 +178,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); - std::vector constraints_; + std::vector constraints_; std::vector submap_transforms_; // (map <- submap) // Submaps get assigned an index and state as soon as they are seen, even diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 0ba3542..0f513a7 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -55,7 +55,7 @@ namespace sparse_pose_graph { // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::SparsePoseGraph::Constraint3D; + using Constraint = mapping::SparsePoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 6d4eb34..b28776b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -44,7 +44,7 @@ struct NodeData { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::SparsePoseGraph::Constraint3D; + using Constraint = mapping::SparsePoseGraph::Constraint; explicit OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& 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 26bd20e..14b9fb3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -33,7 +33,7 @@ namespace sparse_pose_graph { class SpaCostFunction { public: - using Constraint = mapping::SparsePoseGraph::Constraint3D; + using Constraint = mapping::SparsePoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}