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

Collate sensor data in the MapBuilder. (#85)

master
Wolfgang Hess 2016-10-19 17:25:44 +02:00 committed by GitHub
parent 15b58766f5
commit 3cf59a0266
3 changed files with 40 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/CMakeLists.txt
View File

@ -68,17 +68,20 @@ google_library(mapping_map_builder
DEPENDS
common_lua_parameter_dictionary
common_make_unique
common_port
common_thread_pool
mapping_2d_global_trajectory_builder
mapping_2d_sparse_pose_graph
mapping_3d_global_trajectory_builder
mapping_3d_local_trajectory_builder_options
mapping_3d_sparse_pose_graph
mapping_global_trajectory_builder_interface
mapping_collated_trajectory_builder
mapping_proto_map_builder_options
mapping_sparse_pose_graph
mapping_submaps
mapping_trajectory_builder
mapping_trajectory_node
sensor_collator
sensor_laser
sensor_voxel_filter
transform_rigid_transform

28
cartographer/mapping/map_builder.cc
View File

@ -23,6 +23,7 @@
#include <utility>
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/global_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
@ -57,8 +58,9 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
return options;
}
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* constant_data)
MapBuilder::MapBuilder(
const proto::MapBuilderOptions& options,
std::deque<TrajectoryNode::ConstantData>* const constant_data)
: options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
@ -74,35 +76,39 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
MapBuilder::~MapBuilder() {}
int MapBuilder::AddTrajectoryBuilder() {
int MapBuilder::AddTrajectoryBuilder(
const std::unordered_set<string>& expected_sensor_ids) {
const int trajectory_id = trajectory_builders_.size();
if (options_.use_trajectory_builder_3d()) {
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
options_.trajectory_builder_3d_options(),
sparse_pose_graph_3d_.get()));
sparse_pose_graph_3d_.get())));
} else {
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
options_.trajectory_builder_2d_options(),
sparse_pose_graph_2d_.get()));
sparse_pose_graph_2d_.get())));
}
const int trajectory_id = trajectory_builders_.size() - 1;
trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),
trajectory_id);
return trajectory_id;
}
GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
const int trajectory_id) const {
return trajectory_builders_.at(trajectory_id).get();
}
GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
const Submaps* trajectory) const {
return trajectory_builders_.at(GetTrajectoryId(trajectory)).get();
void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_.FinishTrajectory(trajectory_id);
}
int MapBuilder::GetTrajectoryId(const Submaps* trajectory) const {
int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
const auto trajectory_id = trajectory_ids_.find(trajectory);
CHECK(trajectory_id != trajectory_ids_.end());
return trajectory_id->second;

24
cartographer/mapping/map_builder.h
View File

@ -19,19 +19,23 @@
#include <deque>
#include <memory>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#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"
namespace cartographer {
namespace mapping {
@ -51,14 +55,16 @@ class MapBuilder {
MapBuilder& operator=(const MapBuilder&) = delete;
// Create a new trajectory and return its index.
int AddTrajectoryBuilder();
int AddTrajectoryBuilder(
const std::unordered_set<string>& expected_sensor_ids);
// Returns the TrajectoryBuilder corresponding to the specified
// 'trajectory_id' or 'trajectory' pointer.
mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const;
mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
const mapping::Submaps* trajectory) const;
// 'trajectory_id'.
mapping::TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id) const;
// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,
// i.e. no further sensor data is expected.
void FinishTrajectory(int trajectory_id);
// Returns the trajectory ID for 'trajectory'.
int GetTrajectoryId(const mapping::Submaps* trajectory) const;
@ -78,8 +84,8 @@ class MapBuilder {
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
mapping::SparsePoseGraph* sparse_pose_graph_;
std::vector<std::unique_ptr<mapping::GlobalTrajectoryBuilderInterface>>
trajectory_builders_;
sensor::Collator sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
};