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

Template dispatching of sensor data. (#484) 

This reduces the amount of code that needs to be written when a
new type of sensor data is introduced by a tiny bit.

Rangefinder data is not yet following this pattern.
master
Wolfgang Hess 2017-08-28 15:43:26 +02:00 committed by GitHub
parent a7fe8bd2ab
commit 094b5a4d93
13 changed files with 68 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -27,6 +27,7 @@
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
@ -53,10 +54,9 @@ class GlobalTrajectoryBuilderInterface {
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0;
virtual void AddFixedFramePoseData(
virtual void AddSensorData(const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
};

14
cartographer/mapping/trajectory_builder.h
View File

@ -65,23 +65,21 @@ class TrajectoryBuilder {
void AddImuData(const string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
AddSensorData(sensor_id, common::make_unique<sensor::DispatchableImuData>(
sensor::ImuData{time, linear_acceleration,
angular_velocity}));
AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
time, linear_acceleration, angular_velocity}));
}
void AddOdometerData(const string& sensor_id, common::Time time,
const transform::Rigid3d& odometer_pose) {
AddSensorData(sensor_id,
common::make_unique<sensor::DispatchableOdometerData>(
time, odometer_pose));
AddSensorData(sensor_id, sensor::MakeDispatchable(
sensor::OdometryData{time, odometer_pose}));
}
void AddFixedFramePoseData(const string& sensor_id, common::Time time,
const transform::Rigid3d& fixed_frame_pose) {
AddSensorData(sensor_id,
common::make_unique<sensor::DispatchableFixedFramePoseData>(
time, fixed_frame_pose));
sensor::MakeDispatchable(
sensor::FixedFramePoseData{time, fixed_frame_pose}));
}
};

13
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -45,19 +45,18 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
insertion_result->insertion_submaps);
}
void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
local_trajectory_builder_.AddImuData(imu_data);
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
local_trajectory_builder_.AddOdometerData(time, pose);
sparse_pose_graph_->AddOdometerData(trajectory_id_,
sensor::OdometryData{time, pose});
void GlobalTrajectoryBuilder::AddSensorData(
const sensor::OdometryData& odometry_data) {
local_trajectory_builder_.AddOdometerData(odometry_data);
sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
}
void GlobalTrajectoryBuilder::AddFixedFramePoseData(
void GlobalTrajectoryBuilder::AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) {
sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
}

8
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -20,6 +20,7 @@
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
namespace cartographer {
namespace mapping_2d {
@ -41,10 +42,9 @@ class GlobalTrajectoryBuilder
// parallel to the ground plane.
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddImuData(const sensor::ImuData& imu_data) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
void AddFixedFramePoseData(
void AddSensorData(const sensor::ImuData& imu_data) override;
void AddSensorData(const sensor::OdometryData& odometry_data) override;
void AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) override;
private:

4
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -203,13 +203,13 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
}
void LocalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& odometer_pose) {
const sensor::OdometryData& odometry_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator we cannot add odometry data.
LOG(INFO) << "Extrapolator not yet initialized.";
return;
}
extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
extrapolator_->AddOdometryData(odometry_data);
}
void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {

8
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -20,13 +20,16 @@
#include <memory>
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/pose_estimate.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
@ -57,8 +60,7 @@ class LocalTrajectoryBuilder {
std::unique_ptr<InsertionResult> AddHorizontalRangeData(
common::Time, const sensor::RangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometerData(common::Time time,
const transform::Rigid3d& odometer_pose);
void AddOdometerData(const sensor::OdometryData& odometry_data);
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(

18
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -28,11 +28,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
local_trajectory_builder_.AddImuData(imu_data);
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
@ -49,12 +44,17 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
insertion_result->insertion_submaps);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
local_trajectory_builder_.AddOdometerData(time, pose);
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
local_trajectory_builder_.AddImuData(imu_data);
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void GlobalTrajectoryBuilder::AddFixedFramePoseData(
void GlobalTrajectoryBuilder::AddSensorData(
const sensor::OdometryData& odometry_data) {
local_trajectory_builder_.AddOdometerData(odometry_data);
}
void GlobalTrajectoryBuilder::AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) {
sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
}

15
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -30,25 +30,24 @@ class GlobalTrajectoryBuilder
public:
GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
int trajectory_id,
mapping_3d::SparsePoseGraph* sparse_pose_graph);
SparsePoseGraph* sparse_pose_graph);
~GlobalTrajectoryBuilder() override;
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
void AddImuData(const sensor::ImuData& imu_data) override;
const mapping::PoseEstimate& pose_estimate() const override;
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
void AddFixedFramePoseData(
void AddSensorData(const sensor::ImuData& imu_data) override;
void AddSensorData(const sensor::OdometryData& odometry_data) override;
void AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) override;
const mapping::PoseEstimate& pose_estimate() const override;
private:
const int trajectory_id_;
mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
SparsePoseGraph* const sparse_pose_graph_;
LocalTrajectoryBuilder local_trajectory_builder_;
};

4
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -168,13 +168,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
}
void LocalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& odometer_pose) {
const sensor::OdometryData& odometry_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator we cannot add odometry data.
LOG(INFO) << "Extrapolator not yet initialized.";
return;
}
extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
extrapolator_->AddOdometryData(odometry_data);
}
const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

7
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -20,13 +20,15 @@
#include <memory>
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/pose_estimate.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
@ -61,8 +63,7 @@ class LocalTrajectoryBuilder {
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges);
void AddOdometerData(common::Time time,
const transform::Rigid3d& odometer_pose);
void AddOdometerData(const sensor::OdometryData& odometry_data);
const mapping::PoseEstimate& pose_estimate() const;
private:

12
cartographer/sensor/collator_test.cc
View File

@ -38,13 +38,13 @@ TEST(Collator, Ordering) {
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData second(common::FromUniversal(200),
Eigen::Vector3f::Zero(), {});
DispatchableImuData third(ImuData{common::FromUniversal(300)});
ImuData third{common::FromUniversal(300)};
DispatchableRangefinderData fourth(common::FromUniversal(400),
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData fifth(common::FromUniversal(500),
Eigen::Vector3f::Zero(), {});
DispatchableOdometerData sixth(common::FromUniversal(600),
transform::Rigid3d::Identity());
OdometryData sixth{common::FromUniversal(600),
transform::Rigid3d::Identity()};
std::vector<std::pair<string, common::Time>> received;
Collator collator;
@ -73,8 +73,7 @@ TEST(Collator, Ordering) {
collator.AddSensorData(
kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(first));
collator.AddSensorData(kTrajectoryId, kSensorId[3],
common::make_unique<DispatchableOdometerData>(sixth));
collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(sixth));
collator.AddSensorData(
kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(fourth));
@ -84,8 +83,7 @@ TEST(Collator, Ordering) {
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(fifth));
collator.AddSensorData(kTrajectoryId, kSensorId[2],
common::make_unique<DispatchableImuData>(third));
collator.AddSensorData(kTrajectoryId, kSensorId[2], MakeDispatchable(third));
ASSERT_EQ(7, received.size());
EXPECT_EQ(100, common::ToUniversal(received[4].second));

48
cartographer/sensor/data.h
View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_DATA_H_
#define CARTOGRAPHER_MAPPING_DATA_H_
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
@ -37,20 +38,6 @@ class Data {
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
};
class DispatchableImuData : public Data {
public:
DispatchableImuData(const ImuData& imu_data) : imu_data_(imu_data) {}
common::Time GetTime() const override { return imu_data_.time; }
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddImuData(imu_data_);
}
private:
const ImuData imu_data_;
};
class DispatchableRangefinderData : public Data {
public:
DispatchableRangefinderData(const common::Time time,
@ -70,38 +57,25 @@ class DispatchableRangefinderData : public Data {
const PointCloud ranges_;
};
class DispatchableOdometerData : public Data {
template <typename DataType>
class Dispatchable : public Data {
public:
DispatchableOdometerData(const common::Time time,
const transform::Rigid3d& odometer_pose)
: time_(time), odometer_pose_(odometer_pose) {}
Dispatchable(const DataType& data) : data_(data) {}
common::Time GetTime() const override { return time_; }
common::Time GetTime() const override { return data_.time; }
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddOdometerData(time_, odometer_pose_);
trajectory_builder->AddSensorData(data_);
}
private:
const common::Time time_;
const transform::Rigid3d odometer_pose_;
const DataType data_;
};
class DispatchableFixedFramePoseData : public Data {
public:
DispatchableFixedFramePoseData(const common::Time time,
const transform::Rigid3d& fixed_frame_pose)
: fixed_frame_pose_data_{time, fixed_frame_pose} {}
common::Time GetTime() const override { return fixed_frame_pose_data_.time; }
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddFixedFramePoseData(fixed_frame_pose_data_);
}
private:
const FixedFramePoseData fixed_frame_pose_data_;
};
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(const DataType& data) {
return common::make_unique<Dispatchable<DataType>>(data);
}
} // namespace sensor
} // namespace cartographer

6
cartographer/sensor/ordered_multi_queue_test.cc
View File

@ -44,9 +44,9 @@ class OrderedMultiQueueTest : public ::testing::Test {
}
std::unique_ptr<Data> MakeImu(const int ordinal) {
return common::make_unique<DispatchableImuData>(
sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero()});
return MakeDispatchable(sensor::ImuData{common::FromUniversal(ordinal),
Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero()});
}
std::vector<std::unique_ptr<Data>> values_;