Collate_by_trajectory option (#828)

Adds an option to create a MapBuilder using TrajectoryCollator
(instead of Collator).

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
master
gaschler 2018-01-17 21:15:15 +01:00 committed by Wally B. Feed
parent d92040d0e6
commit de5937856d
4 changed files with 19 additions and 6 deletions

View File

@ -28,7 +28,9 @@
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h" #include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h" #include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/trajectory_collator.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -65,6 +67,11 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
options_.pose_graph_options(), &thread_pool_); options_.pose_graph_options(), &thread_pool_);
pose_graph_ = pose_graph_3d_.get(); pose_graph_ = pose_graph_3d_.get();
} }
if (options.collate_by_trajectory()) {
sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>();
} else {
sensor_collator_ = common::make_unique<sensor::Collator>();
}
} }
MapBuilder::~MapBuilder() {} MapBuilder::~MapBuilder() {}
@ -84,7 +91,7 @@ int MapBuilder::AddTrajectoryBuilder(
} }
trajectory_builders_.push_back( trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>( common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids, sensor_collator_.get(), trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder< common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_3d::LocalTrajectoryBuilder, mapping_3d::LocalTrajectoryBuilder,
mapping_3d::proto::LocalTrajectoryBuilderOptions, mapping_3d::proto::LocalTrajectoryBuilderOptions,
@ -101,7 +108,7 @@ int MapBuilder::AddTrajectoryBuilder(
} }
trajectory_builders_.push_back( trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>( common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids, sensor_collator_.get(), trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder< common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder, mapping_2d::LocalTrajectoryBuilder,
mapping_2d::proto::LocalTrajectoryBuilderOptions, mapping_2d::proto::LocalTrajectoryBuilderOptions,
@ -137,7 +144,7 @@ TrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
} }
void MapBuilder::FinishTrajectory(const int trajectory_id) { void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_.FinishTrajectory(trajectory_id); sensor_collator_->FinishTrajectory(trajectory_id);
pose_graph_->FinishTrajectory(trajectory_id); pose_graph_->FinishTrajectory(trajectory_id);
} }

View File

@ -27,7 +27,7 @@
#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_2d/pose_graph.h"
#include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h"
#include "cartographer/sensor/collator.h" #include "cartographer/sensor/collator_interface.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
@ -39,7 +39,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
// and a PoseGraph for loop closure. // and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface { class MapBuilder : public MapBuilderInterface {
public: public:
MapBuilder(const proto::MapBuilderOptions& options); explicit MapBuilder(const proto::MapBuilderOptions& options);
~MapBuilder() override; ~MapBuilder() override;
MapBuilder(const MapBuilder&) = delete; MapBuilder(const MapBuilder&) = delete;
@ -76,7 +76,7 @@ class MapBuilder : public MapBuilderInterface {
std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_; std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_;
mapping::PoseGraph* pose_graph_; mapping::PoseGraph* pose_graph_;
sensor::Collator sensor_collator_; std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_; trajectory_builders_;
}; };

View File

@ -25,4 +25,6 @@ message MapBuilderOptions {
// Number of threads to use for background computations. // Number of threads to use for background computations.
int32 num_background_threads = 3; int32 num_background_threads = 3;
PoseGraphOptions pose_graph_options = 4; PoseGraphOptions pose_graph_options = 4;
// Sort sensor input independently for each trajectory.
bool collate_by_trajectory = 5;
} }

View File

@ -34,6 +34,10 @@ void Run(const std::string& configuration_directory,
proto::MapBuilderServerOptions map_builder_server_options = proto::MapBuilderServerOptions map_builder_server_options =
LoadMapBuilderServerOptions(configuration_directory, LoadMapBuilderServerOptions(configuration_directory,
configuration_basename); configuration_basename);
// TODO(gaschler): Remove this override when parameter is imported from lua
// config.
map_builder_server_options.mutable_map_builder_options()
->set_collate_by_trajectory(true);
auto map_builder = auto map_builder =
cartographer::common::make_unique<cartographer::mapping::MapBuilder>( cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
map_builder_server_options.map_builder_options()); map_builder_server_options.map_builder_options());