Adds API to add trimmers. (#314)

This is related to #283. First to be implemented for
pure localization and 2D SLAM.

PAIR=SirVer
master
Wolfgang Hess 2017-06-06 17:07:30 +02:00 committed by GitHub
parent 56529e5968
commit c90b887bfb
5 changed files with 112 additions and 1 deletions

View File

@ -0,0 +1,53 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_
#include "cartographer/mapping/id.h"
namespace cartographer {
namespace mapping {
// Implemented by the pose graph to provide thread-safe access to functions for
// trimming the graph.
class TrimmingInterface {
public:
virtual ~TrimmingInterface() {}
// TODO(whess): This is all the functionality necessary for pure localization.
// To be expanded as needed for lifelong mapping.
virtual int num_submaps(int trajectory_id) const = 0;
// Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They
// will no longer take part in scan matching, loop closure, visualization.
// Submaps and nodes are only marked, the numbering remains unchanged.
virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0;
};
// An interface to implement algorithms that choose how to trim the pose graph.
class PoseGraphTrimmer {
public:
virtual ~PoseGraphTrimmer() {}
// Called once after each pose graph optimization.
virtual void Trim(TrimmingInterface* trimming) = 0;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_

View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
#include <memory>
#include <set> #include <set>
#include <unordered_map> #include <unordered_map>
#include <utility> #include <utility>
@ -24,6 +25,7 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
@ -65,10 +67,14 @@ class SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a 'trimmer'. It will be used after all data added before it has been
// included in the pose graph.
virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;
// Computes optimized poses. // Computes optimized poses.
virtual void RunFinalOptimization() = 0; virtual void RunFinalOptimization() = 0;
// Get the current trajectory clusters. // Gets the current trajectory clusters.
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0; virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory_id'. // Returns the current optimized transforms for the given 'trajectory_id'.

View File

@ -335,6 +335,15 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);
// C++11 does not allow us to move a unique_ptr into a lambda.
mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]()
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
}
void SparsePoseGraph::RunFinalOptimization() { void SparsePoseGraph::RunFinalOptimization() {
WaitForAllComputations(); WaitForAllComputations();
optimization_problem_.SetMaxNumIterations( optimization_problem_.SetMaxNumIterations(
@ -386,6 +395,11 @@ void SparsePoseGraph::RunOptimization() {
reverse_connected_components_.emplace(trajectory_id, i); reverse_connected_components_.emplace(trajectory_id, i);
} }
} }
TrimmingImplementation trimming(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming);
}
} }
std::vector<std::vector<mapping::TrajectoryNode>> std::vector<std::vector<mapping::TrajectoryNode>>
@ -468,5 +482,19 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return result; return result;
} }
SparsePoseGraph::TrimmingImplementation::TrimmingImplementation(
SparsePoseGraph* const parent)
: parent_(parent) {}
int SparsePoseGraph::TrimmingImplementation::num_submaps(
const int trajectory_id) const {
return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
}
void SparsePoseGraph::TrimmingImplementation::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
LOG(FATAL) << "Not yet implemented.";
}
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

View File

@ -21,6 +21,7 @@
#include <functional> #include <functional>
#include <limits> #include <limits>
#include <map> #include <map>
#include <memory>
#include <unordered_map> #include <unordered_map>
#include <vector> #include <vector>
@ -30,6 +31,7 @@
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
@ -77,6 +79,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id) std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
@ -202,6 +205,24 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>> std::vector<std::vector<sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_); optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> 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 {
public:
TrimmingImplementation(SparsePoseGraph* parent);
~TrimmingImplementation() override {}
int num_submaps(int trajectory_id) const override;
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;
private:
SparsePoseGraph* const parent_;
};
}; };
} // namespace mapping_2d } // namespace mapping_2d

View File

@ -76,6 +76,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity); const Eigen::Vector3d& angular_velocity);
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override {
LOG(FATAL) << "Not yet implemented for 3D.";
}
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id) std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)