diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 9c7f8db..f33fe50 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -47,6 +47,11 @@ struct SubmapId { int trajectory_id; int submap_index; + bool operator==(const SubmapId& other) const { + return std::forward_as_tuple(trajectory_id, submap_index) == + std::forward_as_tuple(other.trajectory_id, other.submap_index); + } + bool operator<(const SubmapId& other) const { return std::forward_as_tuple(trajectory_id, submap_index) < std::forward_as_tuple(other.trajectory_id, other.submap_index); diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc new file mode 100644 index 0000000..60ac470 --- /dev/null +++ b/cartographer/mapping/pose_graph_trimmer.cc @@ -0,0 +1,41 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_graph_trimmer.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id, + const int num_submaps_to_keep) + : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) { + CHECK_GE(num_submaps_to_keep, 3); +} + +void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { + const int total_num_submaps = pose_graph->num_submaps(trajectory_id_); + while (total_num_submaps > num_submaps_trimmed_ + num_submaps_to_keep_) { + const int submap_index_to_trim_next = num_submaps_trimmed_; + pose_graph->MarkSubmapAsTrimmed( + SubmapId{trajectory_id_, submap_index_to_trim_next}); + ++num_submaps_trimmed_; + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h index f3f1d98..5e87a4d 100644 --- a/cartographer/mapping/pose_graph_trimmer.h +++ b/cartographer/mapping/pose_graph_trimmer.h @@ -24,9 +24,9 @@ namespace mapping { // Implemented by the pose graph to provide thread-safe access to functions for // trimming the graph. -class TrimmingInterface { +class Trimmable { public: - virtual ~TrimmingInterface() {} + virtual ~Trimmable() {} // TODO(whess): This is all the functionality necessary for pure localization. // To be expanded as needed for lifelong mapping. @@ -44,7 +44,22 @@ class PoseGraphTrimmer { virtual ~PoseGraphTrimmer() {} // Called once after each pose graph optimization. - virtual void Trim(TrimmingInterface* trimming) = 0; + virtual void Trim(Trimmable* pose_graph) = 0; +}; + +// Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id' +// to implement localization without mapping. +class PureLocalizationTrimmer : public PoseGraphTrimmer { + public: + PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep); + ~PureLocalizationTrimmer() override {} + + void Trim(Trimmable* pose_graph) override; + + private: + const int trajectory_id_; + const int num_submaps_to_keep_; + int num_submaps_trimmed_ = 0; }; } // namespace mapping diff --git a/cartographer/mapping/pose_graph_trimmer_test.cc b/cartographer/mapping/pose_graph_trimmer_test.cc new file mode 100644 index 0000000..0bbf787 --- /dev/null +++ b/cartographer/mapping/pose_graph_trimmer_test.cc @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_graph_trimmer.h" + +#include + +#include "cartographer/mapping/id.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +class FakePoseGraph : public Trimmable { + public: + ~FakePoseGraph() override {} + + int num_submaps(int trajectory_id) const override { return 17; } + + void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { + trimmed_submaps_.push_back(submap_id); + } + + std::vector trimmed_submaps() { return trimmed_submaps_; } + + private: + std::vector trimmed_submaps_; +}; + +TEST(PureLocalizationTrimmerTest, MarksSubmapsAsExpected) { + const int kTrajectoryId = 42; + PureLocalizationTrimmer trimmer(kTrajectoryId, 15); + FakePoseGraph fake_pose_graph; + trimmer.Trim(&fake_pose_graph); + + const auto trimmed_submaps = fake_pose_graph.trimmed_submaps(); + ASSERT_EQ(2, trimmed_submaps.size()); + EXPECT_EQ((SubmapId{kTrajectoryId, 0}), trimmed_submaps[0]); + EXPECT_EQ((SubmapId{kTrajectoryId, 1}), trimmed_submaps[1]); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/probability_values_test.cc b/cartographer/mapping/probability_values_test.cc index 10aa2cf..39c7286 100644 --- a/cartographer/mapping/probability_values_test.cc +++ b/cartographer/mapping/probability_values_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/probability_values.h" + #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index b3d1b05..805b304 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -396,9 +396,9 @@ void SparsePoseGraph::RunOptimization() { } } - TrimmingImplementation trimming(this); + TrimmingHandle trimming_handle(this); for (auto& trimmer : trimmers_) { - trimmer->Trim(&trimming); + trimmer->Trim(&trimming_handle); } } @@ -482,16 +482,15 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( return result; } -SparsePoseGraph::TrimmingImplementation::TrimmingImplementation( - SparsePoseGraph* const parent) +SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) : parent_(parent) {} -int SparsePoseGraph::TrimmingImplementation::num_submaps( +int SparsePoseGraph::TrimmingHandle::num_submaps( const int trajectory_id) const { return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); } -void SparsePoseGraph::TrimmingImplementation::MarkSubmapAsTrimmed( +void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( const mapping::SubmapId& submap_id) { LOG(FATAL) << "Not yet implemented."; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 84bf2ef..ede88db 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -210,12 +210,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> trimmers_ GUARDED_BY(mutex_); - // Used to decide which submaps to trim. The 'mutex_' of the pose graph is - // held while this class is used. - class TrimmingImplementation : public mapping::TrimmingInterface { + // Allows querying and manipulating the pose graph by the 'trimmers_'. The + // 'mutex_' of the pose graph is held while this class is used. + class TrimmingHandle : public mapping::Trimmable { public: - TrimmingImplementation(SparsePoseGraph* parent); - ~TrimmingImplementation() override {} + TrimmingHandle(SparsePoseGraph* parent); + ~TrimmingHandle() override {} int num_submaps(int trajectory_id) const override; void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;