12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move ConstantData into the SPG (#263)

master
Holger Rapp 2017-05-08 17:10:07 +02:00 committed by Wolfgang Hess
parent 9194c8679b
commit 772d02c00e
7 changed files with 19 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/map_builder.cc
View File

@ -51,18 +51,16 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
return options; return options;
} }
MapBuilder::MapBuilder( MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* const constant_data)
: options_(options), thread_pool_(options.num_background_threads()) { : options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>( sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_, constant_data); options_.sparse_pose_graph_options(), &thread_pool_);
sparse_pose_graph_ = sparse_pose_graph_2d_.get(); sparse_pose_graph_ = sparse_pose_graph_2d_.get();
} }
if (options.use_trajectory_builder_3d()) { if (options.use_trajectory_builder_3d()) {
sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>( sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_, constant_data); options_.sparse_pose_graph_options(), &thread_pool_);
sparse_pose_graph_ = sparse_pose_graph_3d_.get(); sparse_pose_graph_ = sparse_pose_graph_3d_.get();
} }
} }

5
cartographer/mapping/map_builder.h
View File

@ -17,7 +17,6 @@
#ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
#include <deque>
#include <memory> #include <memory>
#include <string> #include <string>
#include <unordered_map> #include <unordered_map>
@ -34,7 +33,6 @@
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/sensor/collator.h" #include "cartographer/sensor/collator.h"
@ -49,8 +47,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
// and a SparsePoseGraph for loop closure. // and a SparsePoseGraph for loop closure.
class MapBuilder { class MapBuilder {
public: public:
MapBuilder(const proto::MapBuilderOptions& options, MapBuilder(const proto::MapBuilderOptions& options);
std::deque<mapping::TrajectoryNode::ConstantData>* constant_data);
~MapBuilder(); ~MapBuilder();
MapBuilder(const MapBuilder&) = delete; MapBuilder(const MapBuilder&) = delete;

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -40,12 +40,10 @@ namespace mapping_2d {
SparsePoseGraph::SparsePoseGraph( SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options, const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool, common::ThreadPool* thread_pool)
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options()), optimization_problem_(options_.optimization_problem_options()),
constraint_builder_(options_.constraint_builder_options(), thread_pool), constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
constant_node_data_(constant_node_data) {}
SparsePoseGraph::~SparsePoseGraph() { SparsePoseGraph::~SparsePoseGraph() {
WaitForAllComputations(); WaitForAllComputations();
@ -96,12 +94,12 @@ void SparsePoseGraph::AddScan(
const int j = trajectory_nodes_.size(); const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose, time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps,
tracking_to_pose}); tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{ trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose, &constant_node_data_.back(), optimized_pose,
}); });
trajectory_connectivity_.Add(submaps); trajectory_connectivity_.Add(submaps);

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -56,10 +56,8 @@ namespace mapping_2d {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::SparsePoseGraph {
public: public:
SparsePoseGraph( SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool);
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data);
~SparsePoseGraph() override; ~SparsePoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
@ -185,7 +183,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// //
// Deque to keep references valid for the background computation when adding // Deque to keep references valid for the background computation when adding
// new data. // new data.
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data_; std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.

3
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -135,7 +135,7 @@ class SparsePoseGraphTest : public ::testing::Test {
})text"); })text");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>( sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),
&thread_pool_, &constant_node_data_); &thread_pool_);
} }
current_pose_ = transform::Rigid2d::Identity(); current_pose_ = transform::Rigid2d::Identity();
@ -172,7 +172,6 @@ class SparsePoseGraphTest : public ::testing::Test {
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<Submaps> submaps_; std::unique_ptr<Submaps> submaps_;
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
common::ThreadPool thread_pool_; common::ThreadPool thread_pool_;
std::unique_ptr<SparsePoseGraph> sparse_pose_graph_; std::unique_ptr<SparsePoseGraph> sparse_pose_graph_;
transform::Rigid2d current_pose_; transform::Rigid2d current_pose_;

10
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -40,13 +40,11 @@ namespace mapping_3d {
SparsePoseGraph::SparsePoseGraph( SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options, const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool, common::ThreadPool* thread_pool)
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options(), optimization_problem_(options_.optimization_problem_options(),
sparse_pose_graph::OptimizationProblem::FixZ::kNo), sparse_pose_graph::OptimizationProblem::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool), constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
constant_node_data_(constant_node_data) {}
SparsePoseGraph::~SparsePoseGraph() { SparsePoseGraph::~SparsePoseGraph() {
WaitForAllComputations(); WaitForAllComputations();
@ -95,12 +93,12 @@ void SparsePoseGraph::AddScan(
const int j = trajectory_nodes_.size(); const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max()); CHECK_LT(j, std::numeric_limits<int>::max());
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), submaps, sensor::Compress(range_data_in_tracking), submaps,
transform::Rigid3d::Identity()}); transform::Rigid3d::Identity()});
trajectory_nodes_.push_back( trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose}); mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
trajectory_connectivity_.Add(submaps); trajectory_connectivity_.Add(submaps);
if (submap_indices_.count(insertion_submaps.back()) == 0) { if (submap_indices_.count(insertion_submaps.back()) == 0) {

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -56,10 +56,8 @@ namespace mapping_3d {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::SparsePoseGraph {
public: public:
SparsePoseGraph( SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool);
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data);
~SparsePoseGraph() override; ~SparsePoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
@ -205,7 +203,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// //
// Deque to keep references valid for the background computation when adding // Deque to keep references valid for the background computation when adding
// new data. // new data.
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data_; std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.