From 386ee328d870237cfce40c7cd35e31dcd82febc9 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 8 Nov 2017 17:37:36 +0100 Subject: [PATCH] Fix 3D yaw rotation in GenerateDiscreteScans. (#640) Correctly generates discrete scans rotating around yaw in the gravity-aligned global map frame. Fixes #639. PAIR=wohe --- .../fast_correlative_scan_matcher.cc | 36 ++++++++++------- .../fast_correlative_scan_matcher.h | 21 ++++++---- .../fast_correlative_scan_matcher_test.cc | 16 ++++---- cartographer/mapping_3d/sparse_pose_graph.cc | 35 +++++++--------- .../sparse_pose_graph/constraint_builder.cc | 40 +++++++++++-------- .../sparse_pose_graph/constraint_builder.h | 17 +++++--- 6 files changed, 92 insertions(+), 73 deletions(-) diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 2e6326a..3f5d4fa 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -157,7 +157,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( - const transform::Rigid3d& initial_pose_estimate, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, const mapping::TrajectoryNode::Data& constant_data, const float min_score, float* const score, transform::Rigid3d* const pose_estimate, float* const rotational_score, float* const low_resolution_score) const { @@ -168,7 +169,8 @@ bool FastCorrelativeScanMatcher::Match( common::RoundToInt(options_.linear_z_search_window() / resolution_), options_.angular_search_window(), &low_resolution_matcher}; return MatchWithSearchParameters( - search_parameters, initial_pose_estimate, + search_parameters, global_node_pose.cast(), + global_submap_pose.cast(), constant_data.high_resolution_point_cloud, constant_data.rotational_scan_matcher_histogram, constant_data.gravity_alignment, min_score, score, pose_estimate, @@ -176,12 +178,11 @@ bool FastCorrelativeScanMatcher::Match( } bool FastCorrelativeScanMatcher::MatchFullSubmap( - const Eigen::Quaterniond& gravity_alignment, + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation, const mapping::TrajectoryNode::Data& constant_data, const float min_score, float* const score, transform::Rigid3d* const pose_estimate, float* const rotational_score, float* const low_resolution_score) const { - const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(), - gravity_alignment); float max_point_distance = 0.f; for (const Eigen::Vector3f& point : constant_data.high_resolution_point_cloud) { @@ -195,7 +196,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( const SearchParameters search_parameters{ linear_window_size, linear_window_size, M_PI, &low_resolution_matcher}; return MatchWithSearchParameters( - search_parameters, initial_pose_estimate, + search_parameters, + transform::Rigid3f::Rotation(global_node_rotation.cast()), + transform::Rigid3f::Rotation(global_submap_rotation.cast()), constant_data.high_resolution_point_cloud, constant_data.rotational_scan_matcher_histogram, constant_data.gravity_alignment, min_score, score, pose_estimate, @@ -204,7 +207,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, - const transform::Rigid3d& initial_pose_estimate, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, const float min_score, @@ -215,7 +219,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const std::vector discrete_scans = GenerateDiscreteScans( search_parameters, point_cloud, rotational_scan_matcher_histogram, - gravity_alignment, initial_pose_estimate.cast()); + gravity_alignment, global_node_pose, global_submap_pose); const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(search_parameters, discrete_scans); @@ -284,7 +288,8 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, - const transform::Rigid3f& initial_pose) const { + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose) const { std::vector result; // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. @@ -299,15 +304,15 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt( search_parameters.angular_search_window / angular_step_size); - // TODO(whess): Should there be a small search window for rotations around - // x and y? std::vector angles; for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { angles.push_back(rz * angular_step_size); } + const transform::Rigid3f node_to_submap = + global_submap_pose.inverse() * global_node_pose; const std::vector scores = rotational_scan_matcher_.Match( rotational_scan_matcher_histogram, - transform::GetYaw(initial_pose.rotation() * + transform::GetYaw(node_to_submap.rotation() * gravity_alignment.inverse().cast()), angles); for (size_t i = 0; i != angles.size(); ++i) { @@ -319,9 +324,10 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( // and rotation of the 'initial_pose', so that the rotation is around the // origin of the range data, and yaw is in map frame. const transform::Rigid3f pose( - initial_pose.translation(), - transform::AngleAxisVectorToRotationQuaternion(angle_axis) * - initial_pose.rotation()); + node_to_submap.translation(), + global_submap_pose.rotation().inverse() * + transform::AngleAxisVectorToRotationQuaternion(angle_axis) * + global_node_pose.rotation()); result.push_back( DiscretizeScan(search_parameters, point_cloud, pose, scores[i])); } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h index 1c57418..8b5903f 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -61,20 +61,23 @@ class FastCorrelativeScanMatcher { delete; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' - // given an 'initial_pose_estimate'. If a score above 'min_score' (excluding - // equality) is possible, true is returned, and 'score', 'pose_estimate', - // 'rotational_score', and 'low_resolution_score' are updated with the result. - bool Match(const transform::Rigid3d& initial_pose_estimate, + // given 'global_node_pose' and 'global_submap_pose'. If a score above + // 'min_score' (excluding equality) is possible, true is returned, and + // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score' + // are updated with the result. + bool Match(const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, const mapping::TrajectoryNode::Data& constant_data, float min_score, float* score, transform::Rigid3d* pose_estimate, float* rotational_score, float* low_resolution_score) const; // Aligns the node with the given 'constant_data' within the 'hybrid_grid' - // given a rotation which is expected to be approximately gravity aligned. + // given rotations which are expected to be approximately gravity aligned. // If a score above 'min_score' (excluding equality) is possible, true is // returned, and 'score', 'pose_estimate', 'rotational_score', and // 'low_resolution_score' are updated with the result. - bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, + bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation, const mapping::TrajectoryNode::Data& constant_data, float min_score, float* score, transform::Rigid3d* pose_estimate, @@ -91,7 +94,8 @@ class FastCorrelativeScanMatcher { bool MatchWithSearchParameters( const SearchParameters& search_parameters, - const transform::Rigid3d& initial_pose_estimate, + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose, const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, float min_score, @@ -106,7 +110,8 @@ class FastCorrelativeScanMatcher { const sensor::PointCloud& point_cloud, const Eigen::VectorXf& rotational_scan_matcher_histogram, const Eigen::Quaterniond& gravity_alignment, - const transform::Rigid3f& initial_pose) const; + const transform::Rigid3f& global_node_pose, + const transform::Rigid3f& global_submap_pose) const; std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters, int num_discrete_scans) const; void ScoreCandidates(int depth, diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index 0b3654f..1a8a8a6 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -145,9 +145,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { float rotational_score = 0.f; float low_resolution_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->Match( - transform::Rigid3d::Identity(), CreateConstantData(point_cloud_), - kMinScore, &score, &pose_estimate, &rotational_score, - &low_resolution_score)); + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), + CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate, + &rotational_score, &low_resolution_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); EXPECT_LT(0.14f, low_resolution_score); @@ -156,7 +156,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->Match( - transform::Rigid3d::Identity(), + transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, &score, &pose_estimate, &rotational_score, &low_resolution_score)) << low_resolution_score; @@ -174,9 +174,9 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { float rotational_score = 0.f; float low_resolution_score = 0.f; EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap( - Eigen::Quaterniond::Identity(), CreateConstantData(point_cloud_), - kMinScore, &score, &pose_estimate, &rotational_score, - &low_resolution_score)); + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), + CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate, + &rotational_score, &low_resolution_score)); EXPECT_LT(kMinScore, score); EXPECT_LT(0.09f, rotational_score); EXPECT_LT(0.14f, low_resolution_score); @@ -185,7 +185,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) { << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap( - Eigen::Quaterniond::Identity(), + Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, &score, &pose_estimate, &rotational_score, &low_resolution_score)) << low_resolution_score; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 76e9916..cfee968 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -179,19 +179,21 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); - const transform::Rigid3d inverse_submap_pose = - optimization_problem_.submap_data().at(submap_id).pose.inverse(); - - const transform::Rigid3d initial_relative_pose = - inverse_submap_pose * + const transform::Rigid3d global_node_pose = optimization_problem_.node_data().at(node_id).global_pose; + const transform::Rigid3d global_submap_pose = + optimization_problem_.submap_data().at(submap_id).pose; + + const transform::Rigid3d global_submap_pose_inverse = + global_submap_pose.inverse(); + std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : submap_data_.at(submap_id).node_ids) { submap_nodes.push_back(mapping::TrajectoryNode{ trajectory_nodes_.at(submap_node_id).constant_data, - inverse_submap_pose * + global_submap_pose_inverse * trajectory_nodes_.at(submap_node_id).global_pose}); } @@ -211,24 +213,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose); + global_node_pose, global_submap_pose); } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { - // In this situation, 'initial_relative_pose' is: - // - // submap <- global map 2 <- global map 1 <- tracking - // (agreeing on gravity) - // - // Since they possibly came from two disconnected trajectories, the only - // guaranteed connection between the tracking and the submap frames is - // an agreement on the direction of gravity. Therefore, excluding yaw, - // 'initial_relative_pose.rotation()' is a good estimate of the relative - // orientation of the point cloud in the submap frame. Finding the correct - // yaw component will be handled by the matching procedure in the - // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. + // In this situation, 'global_node_pose' and 'global_submap_pose' have + // orientations agreeing on gravity. Their relationship regarding yaw is + // arbitrary. Finding the correct yaw component will be handled by the + // matching procedure in the FastCorrelativeScanMatcher, and the given yaw + // is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose.rotation()); + global_node_pose.rotation(), global_submap_pose.rotation()); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index b479a6e..88beb7f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -59,8 +59,10 @@ void ConstraintBuilder::MaybeAddConstraint( const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const transform::Rigid3d& initial_pose) { - if (initial_pose.translation().norm() > options_.max_constraint_distance()) { + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose) { + if ((global_node_pose.translation() - global_submap_pose.translation()) + .norm() > options_.max_constraint_distance()) { return; } if (sampler_.Pulse()) { @@ -72,7 +74,8 @@ void ConstraintBuilder::MaybeAddConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ - constant_data, initial_pose, constraint); + constant_data, global_node_pose, global_submap_pose, + constraint); FinishComputation(current_computation); }); } @@ -83,7 +86,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const Eigen::Quaterniond& gravity_alignment) { + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); @@ -91,10 +95,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ - constant_data, - transform::Rigid3d::Rotation(gravity_alignment), - constraint); + ComputeConstraint( + submap_id, node_id, true, /* match_full_submap */ + constant_data, transform::Rigid3d::Rotation(global_node_rotation), + transform::Rigid3d::Rotation(global_submap_rotation), constraint); FinishComputation(current_computation); }); } @@ -166,7 +170,8 @@ void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, const mapping::TrajectoryNode::Data* const constant_data, - const transform::Rigid3d& initial_pose, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, std::unique_ptr* constraint) { const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); @@ -178,6 +183,7 @@ void ConstraintBuilder::ComputeConstraint( transform::Rigid3d pose_estimate; float rotational_score = 0.f; float low_resolution_score = 0.f; + // TODO(gaschler): Match methods should return unique_ptr. // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. @@ -185,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint( // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - initial_pose.rotation(), *constant_data, - options_.global_localization_min_score(), &score, &pose_estimate, - &rotational_score, &low_resolution_score)) { + global_node_pose.rotation(), global_submap_pose.rotation(), + *constant_data, options_.global_localization_min_score(), &score, + &pose_estimate, &rotational_score, &low_resolution_score)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); @@ -196,8 +202,9 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, *constant_data, options_.min_score(), &score, - &pose_estimate, &rotational_score, &low_resolution_score)) { + global_node_pose, global_submap_pose, *constant_data, + options_.min_score(), &score, &pose_estimate, &rotational_score, + &low_resolution_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { @@ -238,8 +245,9 @@ void ConstraintBuilder::ComputeConstraint( if (match_full_submap) { info << " matches"; } else { - const transform::Rigid3d difference = - initial_pose.inverse() * constraint_transform; + const transform::Rigid3d difference = global_submap_pose.inverse() * + global_node_pose * + constraint_transform; info << " differs by translation " << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << transform::GetAngle(difference); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 80ae20d..649a386 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -68,7 +68,9 @@ class ConstraintBuilder { // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. - // The 'initial_pose' is relative to the 'submap'. + // + // 'global_node_pose' and 'global_submap_pose' are initial estimates of poses + // in the global map frame, i.e. both are gravity aligned. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. @@ -77,14 +79,15 @@ class ConstraintBuilder { const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const transform::Rigid3d& initial_pose); + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. // This performs full-submap matching. // - // The 'gravity_alignment' is the rotation to apply to the point cloud data - // to make it approximately gravity aligned. + // 'global_node_rotation' and 'global_submap_rotation' are initial estimates + // of roll and pitch, i.e. their yaw is essentially ignored. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. @@ -93,7 +96,8 @@ class ConstraintBuilder { const mapping::NodeId& node_id, const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, - const Eigen::Quaterniond& gravity_alignment); + const Eigen::Quaterniond& global_node_rotation, + const Eigen::Quaterniond& global_submap_rotation); // Must be called after all computations related to one node have been added. void NotifyEndOfScan(); @@ -141,7 +145,8 @@ class ConstraintBuilder { const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, const mapping::TrajectoryNode::Data* const constant_data, - const transform::Rigid3d& initial_pose, + const transform::Rigid3d& global_node_pose, + const transform::Rigid3d& global_submap_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); // Decrements the 'pending_computations_' count. If all computations are done,