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(