From a905036a007a822d884c73e3775e72ae6e701cd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 11 Jul 2018 19:44:37 +0200 Subject: [PATCH] Create first submap with first scan (#1253) The first submap is now created with first call to `ActiveSubmaps:InsertRangeData`. Originally the first submap was created at the origin. Creating the first submap with the first range data insertion allows to align the first submap with the gravity estimate of the first scan. This change makes the interface of ActiveSubmaps most consistent, as `ActiveSubmaps::submaps()` will return the correct active submaps after insertion. The change affects the result of cartographer. The results were verified and provide the same quality as current master. --- cartographer/mapping/2d/submap_2d.cc | 36 +++++++++---------- cartographer/mapping/2d/submap_2d.h | 14 ++++---- cartographer/mapping/2d/submap_2d_test.cc | 18 ++++++---- cartographer/mapping/3d/submap_3d.cc | 35 +++++++++--------- cartographer/mapping/3d/submap_3d.h | 16 +++++---- .../2d/local_trajectory_builder_2d.cc | 14 +++----- .../mapping/internal/2d/pose_graph_2d_test.cc | 9 +++-- .../3d/local_trajectory_builder_3d.cc | 16 ++++----- 8 files changed, 79 insertions(+), 79 deletions(-) diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 63aff81..53d1770 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -127,23 +127,26 @@ void Submap2D::Finish() { } ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) - : options_(options), range_data_inserter_(CreateRangeDataInserter()) { - // We always want to have at least one likelihood field which we can return, - // and will create it at the origin in absence of a better choice. - AddSubmap(Eigen::Vector2f::Zero()); + : options_(options), range_data_inserter_(CreateRangeDataInserter()) {} + +std::vector> ActiveSubmaps2D::submaps() const { + return std::vector>(submaps_.begin(), + submaps_.end()); } -std::vector> ActiveSubmaps2D::submaps() const { - return submaps_; -} - -void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) { +std::vector> ActiveSubmaps2D::InsertRangeData( + const sensor::RangeData& range_data) { + if (submaps_.empty() || + submaps_.back()->num_range_data() == options_.num_range_data()) { + AddSubmap(range_data.origin.head<2>()); + } for (auto& submap : submaps_) { submap->InsertRangeData(range_data, range_data_inserter_.get()); } - if (submaps_.back()->num_range_data() == options_.num_range_data()) { - AddSubmap(range_data.origin.head<2>()); + if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { + submaps_.front()->Finish(); } + return submaps(); } std::unique_ptr @@ -164,17 +167,12 @@ std::unique_ptr ActiveSubmaps2D::CreateGrid( CellLimits(kInitialSubmapSize, kInitialSubmapSize))); } -void ActiveSubmaps2D::FinishSubmap() { - Submap2D* submap = submaps_.front().get(); - submap->Finish(); - submaps_.erase(submaps_.begin()); -} - void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { - if (submaps_.size() > 1) { + if (submaps_.size() >= 2) { // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. - FinishSubmap(); + CHECK(submaps_.front()->finished()); + submaps_.erase(submaps_.begin()); } submaps_.push_back(common::make_unique( diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index a02b3ef..b2cd8f7 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -63,10 +63,11 @@ class Submap2D : public Submap { std::unique_ptr grid_; }; -// Except during initialization when only a single submap exists, there are -// always two submaps into which range data is inserted: an old submap that is -// used for matching, and a new one, which will be used for matching next, that -// is being initialized. +// The first active submap will be created on the insertion of the first range +// data. Except during this initialization when no or only one single submap +// exists, there are always two submaps into which range data is inserted: an +// old submap that is used for matching, and a new one, which will be used for +// matching next, that is being initialized. // // Once a certain number of range data have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap @@ -80,9 +81,10 @@ class ActiveSubmaps2D { ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete; // Inserts 'range_data' into the Submap collection. - void InsertRangeData(const sensor::RangeData& range_data); + std::vector> InsertRangeData( + const sensor::RangeData& range_data); - std::vector> submaps() const; + std::vector> submaps() const; private: std::unique_ptr CreateRangeDataInserter(); diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index d410560..855ff16 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -66,12 +66,13 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { "}," "}"); ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())}; - std::set> all_submaps; + std::set> all_submaps; for (int i = 0; i != 1000; ++i) { - submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); + auto insertion_submaps = + submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); // Except for the first, maps should only be returned after enough range // data. - for (const auto& submap : submaps.submaps()) { + for (const auto& submap : insertion_submaps) { all_submaps.insert(submap); } if (submaps.submaps().size() > 1) { @@ -79,14 +80,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { } } EXPECT_EQ(2, submaps.submaps().size()); - int correct_num_range_data = 0; + int correct_num_finished_submaps = 0; + int num_unfinished_submaps = 0; for (const auto& submap : all_submaps) { if (submap->num_range_data() == kNumRangeData * 2) { - ++correct_num_range_data; + ++correct_num_finished_submaps; + } else { + EXPECT_EQ(kNumRangeData, submap->num_range_data()); + ++num_unfinished_submaps; } } // Submaps should not be left without the right number of range data in them. - EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2); + EXPECT_EQ(correct_num_finished_submaps, all_submaps.size() - 1); + EXPECT_EQ(1, num_unfinished_submaps); } TEST(Submap2DTest, ToFromProto) { diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index b2c4b6e..7f46b48 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -281,35 +281,36 @@ void Submap3D::Finish() { ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options) : options_(options), - range_data_inserter_(options.range_data_inserter_options()) { - // We always want to have at least one submap which we can return and will - // create it at the origin in absence of a better choice. - // - // TODO(whess): Start with no submaps, so that all of them can be - // approximately gravity aligned. - AddSubmap(transform::Rigid3d::Identity()); + range_data_inserter_(options.range_data_inserter_options()) {} + +std::vector> ActiveSubmaps3D::submaps() const { + return std::vector>(submaps_.begin(), + submaps_.end()); } -std::vector> ActiveSubmaps3D::submaps() const { - return submaps_; -} - -void ActiveSubmaps3D::InsertRangeData( +std::vector> ActiveSubmaps3D::InsertRangeData( const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment) { + if (submaps_.empty() || + submaps_.back()->num_range_data() == options_.num_range_data()) { + AddSubmap(transform::Rigid3d(range_data.origin.cast(), + gravity_alignment)); + } for (auto& submap : submaps_) { submap->InsertRangeData(range_data, range_data_inserter_, options_.high_resolution_max_range()); } - if (submaps_.back()->num_range_data() == options_.num_range_data()) { - AddSubmap(transform::Rigid3d(range_data.origin.cast(), - gravity_alignment)); + if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { + submaps_.front()->Finish(); } + return submaps(); } void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) { - if (submaps_.size() > 1) { - submaps_.front()->Finish(); + if (submaps_.size() >= 2) { + // This will crop the finished Submap before inserting a new Submap to + // reduce peak memory usage a bit. + CHECK(submaps_.front()->finished()); submaps_.erase(submaps_.begin()); } submaps_.emplace_back(new Submap3D(options_.high_resolution(), diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index 3d18236..02dd999 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -72,10 +72,11 @@ class Submap3D : public Submap { std::unique_ptr low_resolution_hybrid_grid_; }; -// Except during initialization when only a single submap exists, there are -// always two submaps into which range data is inserted: an old submap that is -// used for matching, and a new one, which will be used for matching next, that -// is being initialized. +// The first active submap will be created on the insertion of the first range +// data. Except during this initialization when no or only one single submap +// exists, there are always two submaps into which range data is inserted: an +// old submap that is used for matching, and a new one, which will be used for +// matching next, that is being initialized. // // Once a certain number of range data have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap @@ -91,10 +92,11 @@ class ActiveSubmaps3D { // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is // used for the orientation of new submaps so that the z axis approximately // aligns with gravity. - void InsertRangeData(const sensor::RangeData& range_data, - const Eigen::Quaterniond& gravity_alignment); + std::vector> InsertRangeData( + const sensor::RangeData& range_data, + const Eigen::Quaterniond& gravity_alignment); - std::vector> submaps() const; + std::vector> submaps() const; private: void AddSubmap(const transform::Rigid3d& local_submap_pose); diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 228ec53..91a8cb9 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -63,6 +63,9 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( const common::Time time, const transform::Rigid2d& pose_prediction, const sensor::PointCloud& filtered_gravity_aligned_point_cloud) { + if (active_submaps_.submaps().empty()) { + return common::make_unique(pose_prediction); + } std::shared_ptr matching_submap = active_submaps_.submaps().front(); // The online correlative scan matcher will refine the initial estimate for @@ -258,15 +261,8 @@ LocalTrajectoryBuilder2D::InsertIntoSubmap( if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; } - - // Querying the active submaps must be done here before calling - // InsertRangeData() since the queried values are valid for next insertion. - std::vector> insertion_submaps; - for (const std::shared_ptr& submap : active_submaps_.submaps()) { - insertion_submaps.push_back(submap); - } - active_submaps_.InsertRangeData(range_data_in_local); - + std::vector> insertion_submaps = + active_submaps_.InsertRangeData(range_data_in_local); return common::make_unique(InsertionResult{ std::make_shared(TrajectoryNode::Data{ time, diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 547742b..73ba849 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -171,17 +171,16 @@ class PoseGraph2DTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - std::vector> insertion_submaps; - for (const auto& submap : active_submaps_->submaps()) { - insertion_submaps.push_back(submap); - } const sensor::RangeData range_data{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; constexpr int kTrajectoryId = 0; active_submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - + std::vector> insertion_submaps; + for (const auto& submap : active_submaps_->submaps()) { + insertion_submaps.push_back(submap); + } pose_graph_->AddNode( std::make_shared( TrajectoryNode::Data{common::FromUniversal(0), diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 28452f9..8c979d1 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -61,6 +61,9 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( const transform::Rigid3d& pose_prediction, const sensor::PointCloud& low_resolution_point_cloud_in_tracking, const sensor::PointCloud& high_resolution_point_cloud_in_tracking) { + if (active_submaps_.submaps().empty()) { + return common::make_unique(pose_prediction); + } std::shared_ptr matching_submap = active_submaps_.submaps().front(); transform::Rigid3d initial_ceres_pose = @@ -248,7 +251,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( LOG(WARNING) << "Dropped empty high resolution point cloud data."; return nullptr; } - sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = @@ -332,15 +334,9 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap( if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; } - // Querying the active submaps must be done here before calling - // InsertRangeData() since the queried values are valid for next insertion. - std::vector> insertion_submaps; - for (const std::shared_ptr& submap : - active_submaps_.submaps()) { - insertion_submaps.push_back(submap); - } - active_submaps_.InsertRangeData(filtered_range_data_in_local, - gravity_alignment); + std::vector> insertion_submaps = + active_submaps_.InsertRangeData(filtered_range_data_in_local, + gravity_alignment); const Eigen::VectorXf rotational_scan_matcher_histogram = scan_matching::RotationalScanMatcher::ComputeHistogram( sensor::TransformPointCloud(