Adds a PureLocalizationTrimmer. (#316)

Related to #315.
master
Wolfgang Hess 2017-06-07 12:39:04 +02:00 committed by GitHub
parent c90b887bfb
commit a66147f131
7 changed files with 133 additions and 14 deletions

View File

@ -47,6 +47,11 @@ struct SubmapId {
int trajectory_id; int trajectory_id;
int submap_index; 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 { bool operator<(const SubmapId& other) const {
return std::forward_as_tuple(trajectory_id, submap_index) < return std::forward_as_tuple(trajectory_id, submap_index) <
std::forward_as_tuple(other.trajectory_id, other.submap_index); std::forward_as_tuple(other.trajectory_id, other.submap_index);

View File

@ -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

View File

@ -24,9 +24,9 @@ namespace mapping {
// Implemented by the pose graph to provide thread-safe access to functions for // Implemented by the pose graph to provide thread-safe access to functions for
// trimming the graph. // trimming the graph.
class TrimmingInterface { class Trimmable {
public: public:
virtual ~TrimmingInterface() {} virtual ~Trimmable() {}
// TODO(whess): This is all the functionality necessary for pure localization. // TODO(whess): This is all the functionality necessary for pure localization.
// To be expanded as needed for lifelong mapping. // To be expanded as needed for lifelong mapping.
@ -44,7 +44,22 @@ class PoseGraphTrimmer {
virtual ~PoseGraphTrimmer() {} virtual ~PoseGraphTrimmer() {}
// Called once after each pose graph optimization. // 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 } // namespace mapping

View File

@ -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 <vector>
#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<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
private:
std::vector<SubmapId> 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

View File

@ -15,6 +15,7 @@
*/ */
#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/probability_values.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {

View File

@ -396,9 +396,9 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
TrimmingImplementation trimming(this); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming); trimmer->Trim(&trimming_handle);
} }
} }
@ -482,16 +482,15 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return result; return result;
} }
SparsePoseGraph::TrimmingImplementation::TrimmingImplementation( SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
SparsePoseGraph* const parent)
: parent_(parent) {} : parent_(parent) {}
int SparsePoseGraph::TrimmingImplementation::num_submaps( int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const { const int trajectory_id) const {
return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
} }
void SparsePoseGraph::TrimmingImplementation::MarkSubmapAsTrimmed( void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
LOG(FATAL) << "Not yet implemented."; LOG(FATAL) << "Not yet implemented.";
} }

View File

@ -210,12 +210,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_ std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Used to decide which submaps to trim. The 'mutex_' of the pose graph is // Allows querying and manipulating the pose graph by the 'trimmers_'. The
// held while this class is used. // 'mutex_' of the pose graph is held while this class is used.
class TrimmingImplementation : public mapping::TrimmingInterface { class TrimmingHandle : public mapping::Trimmable {
public: public:
TrimmingImplementation(SparsePoseGraph* parent); TrimmingHandle(SparsePoseGraph* parent);
~TrimmingImplementation() override {} ~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override; void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;