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

Make LocalTrajectoryBuilder optional. (#799) 

This is to prepare for the cloud-based mapping case where the robot is running cartographer in pure-localization mode and the cloud instance only solves the global problem. In that case a `LocalTrajectoryBuilder` need to be instantiated in for the cloud instance.
master
Christoph Schütte 2018-01-09 16:54:30 +01:00 committed by Wally B. Feed
parent 8165da873f
commit 8c7c4e3d2a
2 changed files with 35 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cartographer/internal/mapping/global_trajectory_builder.h
View File

@ -28,13 +28,15 @@ template <typename LocalTrajectoryBuilder,
typename LocalTrajectoryBuilderOptions, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
public:
// Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no
// 'TimedPointCloudData' may be added in that case.
GlobalTrajectoryBuilder(
const LocalTrajectoryBuilderOptions& options, const int trajectory_id,
PoseGraph* const pose_graph,
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
const int trajectory_id, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback)
: trajectory_id_(trajectory_id),
pose_graph_(pose_graph),
local_trajectory_builder_(options),
local_trajectory_builder_(std::move(local_trajectory_builder)),
local_slam_result_callback_(local_slam_result_callback) {}
~GlobalTrajectoryBuilder() override {}
@ -44,8 +46,10 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
CHECK(local_trajectory_builder_)
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_.AddRangeData(
matching_result = local_trajectory_builder_->AddRangeData(
timed_point_cloud_data.time,
sensor::TimedRangeData{timed_point_cloud_data.origin,
timed_point_cloud_data.ranges,
@ -71,14 +75,18 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
local_trajectory_builder_.AddImuData(imu_data);
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
local_trajectory_builder_.AddOdometryData(odometry_data);
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
@ -95,7 +103,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
private:
const int trajectory_id_;
PoseGraph* const pose_graph_;
LocalTrajectoryBuilder local_trajectory_builder_;
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
LocalSlamResultCallback local_slam_result_callback_;
};

30
cartographer/mapping/map_builder.cc
View File

@ -75,29 +75,39 @@ int MapBuilder::AddTrajectoryBuilder(
LocalSlamResultCallback local_slam_result_callback) {
const int trajectory_id = trajectory_builders_.size();
if (options_.use_trajectory_builder_3d()) {
CHECK(trajectory_options.has_trajectory_builder_3d_options());
std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_3d_options()) {
local_trajectory_builder =
common::make_unique<mapping_3d::LocalTrajectoryBuilder>(
trajectory_options.trajectory_builder_3d_options());
}
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_3d::LocalTrajectoryBuilder,
mapping_3d::proto::LocalTrajectoryBuilderOptions,
mapping_3d::PoseGraph>>(
trajectory_options.trajectory_builder_3d_options(),
trajectory_id, pose_graph_3d_.get(),
local_slam_result_callback)));
mapping_3d::PoseGraph>>(std::move(local_trajectory_builder),
trajectory_id, pose_graph_3d_.get(),
local_slam_result_callback)));
} else {
CHECK(trajectory_options.has_trajectory_builder_2d_options());
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder =
common::make_unique<mapping_2d::LocalTrajectoryBuilder>(
trajectory_options.trajectory_builder_2d_options());
}
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder,
mapping_2d::proto::LocalTrajectoryBuilderOptions,
mapping_2d::PoseGraph>>(
trajectory_options.trajectory_builder_2d_options(),
trajectory_id, pose_graph_2d_.get(),
local_slam_result_callback)));
mapping_2d::PoseGraph>>(std::move(local_trajectory_builder),
trajectory_id, pose_graph_2d_.get(),
local_slam_result_callback)));
}
if (trajectory_options.pure_localization()) {
constexpr int kSubmapsToKeep = 3;