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;
}
MapBuilder::MapBuilder(
const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* const constant_data)
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) {
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();
}
if (options.use_trajectory_builder_3d()) {
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();
}
}

5
cartographer/mapping/map_builder.h
View File

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

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -40,12 +40,10 @@ namespace mapping_2d {
SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options()),
constraint_builder_(options_.constraint_builder_options(), thread_pool),
constant_node_data_(constant_node_data) {}
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
SparsePoseGraph::~SparsePoseGraph() {
WaitForAllComputations();
@ -96,12 +94,12 @@ void SparsePoseGraph::AddScan(
const int j = trajectory_nodes_.size();
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,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps,
tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose,
&constant_node_data_.back(), optimized_pose,
});
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.
class SparsePoseGraph : public mapping::SparsePoseGraph {
public:
SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data);
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool);
~SparsePoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete;
@ -185,7 +183,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
//
// Deque to keep references valid for the background computation when adding
// 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_);
// 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");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),
&thread_pool_, &constant_node_data_);
&thread_pool_);
}
current_pose_ = transform::Rigid2d::Identity();
@ -172,7 +172,6 @@ class SparsePoseGraphTest : public ::testing::Test {
sensor::PointCloud point_cloud_;
std::unique_ptr<Submaps> submaps_;
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
common::ThreadPool thread_pool_;
std::unique_ptr<SparsePoseGraph> sparse_pose_graph_;
transform::Rigid2d current_pose_;

10
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -40,13 +40,11 @@ namespace mapping_3d {
SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data)
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options(),
sparse_pose_graph::OptimizationProblem::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool),
constant_node_data_(constant_node_data) {}
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
SparsePoseGraph::~SparsePoseGraph() {
WaitForAllComputations();
@ -95,12 +93,12 @@ void SparsePoseGraph::AddScan(
const int j = trajectory_nodes_.size();
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(), {}, {}},
sensor::Compress(range_data_in_tracking), submaps,
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose});
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
trajectory_connectivity_.Add(submaps);
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.
class SparsePoseGraph : public mapping::SparsePoseGraph {
public:
SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool,
std::deque<mapping::TrajectoryNode::ConstantData>* constant_node_data);
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool);
~SparsePoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete;
@ -205,7 +203,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
//
// Deque to keep references valid for the background computation when adding
// 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_);
// Current submap transforms used for displaying data.