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 a33b3ce..2e10fa9 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -24,6 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" +#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" #include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" @@ -126,6 +127,7 @@ struct Candidate { FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, + const HybridGrid* const low_resolution_hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options) : options_(options), @@ -133,49 +135,53 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( width_in_voxels_(hybrid_grid.grid_size()), precomputation_grid_stack_( common::make_unique(hybrid_grid, options)), + low_resolution_hybrid_grid_(low_resolution_hybrid_grid), rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, const float min_score, - const MatchingFunction& low_resolution_matcher, float* const score, - transform::Rigid3d* const pose_estimate, float* const rotational_score, - float* const low_resolution_score) const { + 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 auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( + low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); const SearchParameters search_parameters{ common::RoundToInt(options_.linear_xy_search_window() / resolution_), common::RoundToInt(options_.linear_z_search_window() / resolution_), options_.angular_search_window(), &low_resolution_matcher}; - return MatchWithSearchParameters(search_parameters, initial_pose_estimate, - coarse_point_cloud, fine_point_cloud, - min_score, score, pose_estimate, - rotational_score, low_resolution_score); + return MatchWithSearchParameters( + search_parameters, initial_pose_estimate, + constant_data.high_resolution_point_cloud, + constant_data.range_data.returns.Decompress(), min_score, score, + pose_estimate, rotational_score, low_resolution_score); } bool FastCorrelativeScanMatcher::MatchFullSubmap( const Eigen::Quaterniond& gravity_alignment, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, const float min_score, - const MatchingFunction& low_resolution_matcher, float* const score, - transform::Rigid3d* const pose_estimate, float* const rotational_score, - float* const low_resolution_score) const { + 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 : coarse_point_cloud) { + for (const Eigen::Vector3f& point : + constant_data.high_resolution_point_cloud) { max_point_distance = std::max(max_point_distance, point.norm()); } const int linear_window_size = (width_in_voxels_ + 1) / 2 + common::RoundToInt(max_point_distance / resolution_ + 0.5f); + const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( + low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud); const SearchParameters search_parameters{ linear_window_size, linear_window_size, M_PI, &low_resolution_matcher}; - return MatchWithSearchParameters(search_parameters, initial_pose_estimate, - coarse_point_cloud, fine_point_cloud, - min_score, score, pose_estimate, - rotational_score, low_resolution_score); + return MatchWithSearchParameters( + search_parameters, initial_pose_estimate, + constant_data.high_resolution_point_cloud, + constant_data.range_data.returns.Decompress(), min_score, score, + pose_estimate, rotational_score, low_resolution_score); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( @@ -269,8 +275,9 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( } const float kSafetyMargin = 1.f - 1e-2f; const float angular_step_size = - kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) / - (2.f * common::Pow2(max_scan_range))); + kSafetyMargin * std::acos(1.f - + common::Pow2(resolution_) / + (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 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 1fcce5f..bc069b4 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -51,6 +51,7 @@ class FastCorrelativeScanMatcher { public: FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, + const HybridGrid* low_resolution_hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options); ~FastCorrelativeScanMatcher(); @@ -65,11 +66,9 @@ class FastCorrelativeScanMatcher { // 'rotational_score', and 'low_resolution_score' are updated with the result. // 'fine_point_cloud' is used to compute the rotational scan matcher score. bool Match(const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, float min_score, - const MatchingFunction& low_resolution_matcher, float* score, - transform::Rigid3d* pose_estimate, float* rotational_score, - float* low_resolution_score) const; + const mapping::TrajectoryNode::Data& constant_data, + float min_score, float* score, transform::Rigid3d* pose_estimate, + float* rotational_score, float* low_resolution_score) const; // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which // is expected to be approximately gravity aligned. If a score above @@ -78,11 +77,9 @@ class FastCorrelativeScanMatcher { // are updated with the result. 'fine_point_cloud' is used to compute the // rotational scan matcher score. bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, - float min_score, - const MatchingFunction& low_resolution_matcher, - float* score, transform::Rigid3d* pose_estimate, + const mapping::TrajectoryNode::Data& constant_data, + float min_score, float* score, + transform::Rigid3d* pose_estimate, float* rotational_score, float* low_resolution_score) const; @@ -130,6 +127,7 @@ class FastCorrelativeScanMatcher { const float resolution_; const int width_in_voxels_; std::unique_ptr precomputation_grid_stack_; + const HybridGrid* const low_resolution_hybrid_grid_; RotationalScanMatcher rotational_scan_matcher_; }; 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 8e51934..5994925 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 @@ -72,7 +72,9 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { ", " "rotational_histogram_size = 30, " "min_rotational_score = 0.1, " - "min_low_resolution_score = 0.5, " + // Unknown space has kMinProbability = 0.1, so we need to make sure here + // to pick a larger number otherwise we always find matches. + "min_low_resolution_score = 0.15, " "linear_xy_search_window = 0.8, " "linear_z_search_window = 0.8, " "angular_search_window = 0.3, " @@ -94,16 +96,28 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { std::unique_ptr GetFastCorrelativeScanMatcher( const proto::FastCorrelativeScanMatcherOptions& options, const transform::Rigid3f& pose) { - HybridGrid hybrid_grid(0.05f); + hybrid_grid_ = common::make_unique(0.05f); range_data_inserter_.Insert( sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), {}}, - &hybrid_grid); - hybrid_grid.FinishUpdate(); + hybrid_grid_.get()); + hybrid_grid_->FinishUpdate(); return common::make_unique( - hybrid_grid, std::vector(), options); + *hybrid_grid_, hybrid_grid_.get(), + std::vector(), options); + } + + mapping::TrajectoryNode::Data CreateConstantData( + const sensor::PointCloud& low_resolution_point_cloud) { + return mapping::TrajectoryNode::Data{ + common::FromUniversal(0), + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}), + {}, + point_cloud_, + low_resolution_point_cloud, + transform::Rigid3d::Identity()}; } std::mt19937 prng_ = std::mt19937(42); @@ -112,11 +126,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { RangeDataInserter range_data_inserter_; const proto::FastCorrelativeScanMatcherOptions options_; sensor::PointCloud point_cloud_; + std::unique_ptr hybrid_grid_; }; constexpr float kMinScore = 0.1f; -constexpr float kPassingLowResolutionScore = 1.f; -constexpr float kFailingLowResolutionScore = 0.f; TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) { for (int i = 0; i != 20; ++i) { @@ -130,20 +143,21 @@ 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(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score, &low_resolution_score)); + 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.99f, low_resolution_score); + EXPECT_LT(0.14f, low_resolution_score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->Match( - transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score, &low_resolution_score)); + transform::Rigid3d::Identity(), + CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, + &score, &pose_estimate, &rotational_score, &low_resolution_score)) + << low_resolution_score; } } @@ -158,20 +172,21 @@ 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(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return kPassingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score, &low_resolution_score)); + 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.99f, low_resolution_score); + EXPECT_LT(0.14f, low_resolution_score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap( - Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore, - [](const transform::Rigid3f&) { return kFailingLowResolutionScore; }, - &score, &pose_estimate, &rotational_score, &low_resolution_score)); + Eigen::Quaterniond::Identity(), + CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore, + &score, &pose_estimate, &rotational_score, &low_resolution_score)) + << low_resolution_score; } } // namespace diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index d165f24..2043724 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -29,7 +29,6 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" @@ -142,7 +141,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher( const Submap* const submap) { auto submap_scan_matcher = common::make_unique( - submap->high_resolution_hybrid_grid(), submap_nodes, + submap->high_resolution_hybrid_grid(), + &submap->low_resolution_hybrid_grid(), submap_nodes, options_.fast_correlative_scan_matcher_options_3d()); common::MutexLocker locker(&mutex_); submap_scan_matchers_[submap_id] = {&submap->high_resolution_hybrid_grid(), @@ -173,8 +173,6 @@ void ConstraintBuilder::ComputeConstraint( std::unique_ptr* constraint) { const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); - const sensor::PointCloud point_cloud = - constant_data->range_data.returns.Decompress(); // The 'constraint_transform' (submap i <- scan j) is computed from: // - a 'high_resolution_point_cloud' in scan j and @@ -184,20 +182,15 @@ void ConstraintBuilder::ComputeConstraint( float rotational_score = 0.f; float low_resolution_score = 0.f; - const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( - submap_scan_matcher->low_resolution_hybrid_grid, - &constant_data->low_resolution_point_cloud); - // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. // 2. Prune if the score is too low. // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - initial_pose.rotation(), constant_data->high_resolution_point_cloud, - point_cloud, options_.global_localization_min_score(), - low_resolution_matcher, &score, &pose_estimate, &rotational_score, - &low_resolution_score)) { + initial_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); @@ -208,8 +201,7 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, constant_data->high_resolution_point_cloud, - point_cloud, options_.min_score(), low_resolution_matcher, &score, + initial_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()); @@ -274,7 +266,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { for (const std::unique_ptr& - constraint : constraints_) { + constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); }