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

Introduce TrajectoryBuilderInterface. (#736) 

[RFC=0004](https://github.com/googlecartographer/rfcs/blob/master/text/0004-trajectory-builder-interface.md)
master
Christoph Schütte 2017-12-08 11:52:29 +01:00 committed by Wally B. Feed
parent 96cdbde5bf
commit 3ae78563c6
17 changed files with 170 additions and 299 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "glog/logging.h"
@ -26,8 +26,7 @@ namespace mapping {
template <typename LocalTrajectoryBuilder,
typename LocalTrajectoryBuilderOptions, typename PoseGraph>
class GlobalTrajectoryBuilder
: public mapping::GlobalTrajectoryBuilderInterface {
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
public:
GlobalTrajectoryBuilder(
const LocalTrajectoryBuilderOptions& options, const int trajectory_id,
@ -42,12 +41,15 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
void AddRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) override {
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_.AddRangeData(
time, sensor::TimedRangeData{origin, ranges, {}});
timed_point_cloud_data.time,
sensor::TimedRangeData{timed_point_cloud_data.origin,
timed_point_cloud_data.ranges,
{}});
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
@ -67,18 +69,21 @@ class GlobalTrajectoryBuilder
}
}
void AddSensorData(const sensor::ImuData& imu_data) override {
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
local_trajectory_builder_.AddImuData(imu_data);
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void AddSensorData(const sensor::OdometryData& odometry_data) override {
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);
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) override {
CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose;
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);

9
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -31,8 +31,7 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
sensor::Collator* const sensor_collator, const int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface>
wrapped_trajectory_builder)
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
@ -47,8 +46,8 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
void CollatedTrajectoryBuilder::AddSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
@ -72,7 +71,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
last_logging_time_ = std::chrono::steady_clock::now();
}
data->AddToGlobalTrajectoryBuilder(wrapped_trajectory_builder_.get());
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}
} // namespace mapping

50
cartographer/mapping/collated_trajectory_builder.h
View File

@ -25,9 +25,8 @@
#include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h"
@ -35,59 +34,50 @@ namespace cartographer {
namespace mapping {
// Handles collating sensor data using a sensor::Collator, then passing it on to
// a mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D.
class CollatedTrajectoryBuilder : public TrajectoryBuilder {
// a mapping::TrajectoryBuilderInterface which is common for 2D and 3D.
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
public:
CollatedTrajectoryBuilder(
sensor::Collator* sensor_collator, int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
std::unique_ptr<GlobalTrajectoryBuilderInterface>
wrapped_trajectory_builder);
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder);
~CollatedTrajectoryBuilder() override;
CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete;
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
delete;
void AddRangefinderData(const std::string& sensor_id, common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) override {
AddSensorData(sensor_id,
common::make_unique<sensor::DispatchableRangefinderData>(
time, origin, ranges));
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddSensorData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
void AddImuData(const std::string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override {
AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
time, linear_acceleration, angular_velocity}));
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddSensorData(sensor::MakeDispatchable(sensor_id, imu_data));
}
void AddOdometerData(const std::string& sensor_id, common::Time time,
const transform::Rigid3d& odometer_pose) override {
AddSensorData(sensor_id, sensor::MakeDispatchable(
sensor::OdometryData{time, odometer_pose}));
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
AddSensorData(sensor::MakeDispatchable(sensor_id, odometry_data));
}
void AddFixedFramePoseData(
const std::string& sensor_id, common::Time time,
const transform::Rigid3d& fixed_frame_pose) override {
AddSensorData(sensor_id,
sensor::MakeDispatchable(
sensor::FixedFramePoseData{time, fixed_frame_pose}));
void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
AddSensorData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
}
private:
void AddSensorData(const std::string& sensor_id,
std::unique_ptr<sensor::Data> data);
void AddSensorData(std::unique_ptr<sensor::Data> data);
void HandleCollatedSensorData(const std::string& sensor_id,
std::unique_ptr<sensor::Data> data);
sensor::Collator* const sensor_collator_;
const int trajectory_id_;
std::unique_ptr<GlobalTrajectoryBuilderInterface> wrapped_trajectory_builder_;
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_logging_time_;

2
cartographer/mapping/map_builder.cc
View File

@ -121,7 +121,7 @@ int MapBuilder::AddTrajectoryForDeserialization() {
return trajectory_id;
}
TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
TrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
const int trajectory_id) const {
return trajectory_builders_.at(trajectory_id).get();
}

5
cartographer/mapping/map_builder.h
View File

@ -52,7 +52,7 @@ class MapBuilder : public MapBuilderInterface {
int AddTrajectoryForDeserialization() override;
mapping::TrajectoryBuilder* GetTrajectoryBuilder(
mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const override;
void FinishTrajectory(int trajectory_id) override;
@ -77,7 +77,8 @@ class MapBuilder : public MapBuilderInterface {
mapping::PoseGraph* pose_graph_;
sensor::Collator sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_;
};
} // namespace mapping

9
cartographer/mapping/map_builder_interface.h
View File

@ -25,13 +25,12 @@
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
namespace cartographer {
namespace mapping {
@ -41,7 +40,7 @@ namespace mapping {
class MapBuilderInterface {
public:
using LocalSlamResultCallback =
GlobalTrajectoryBuilderInterface::LocalSlamResultCallback;
TrajectoryBuilderInterface::LocalSlamResultCallback;
MapBuilderInterface() {}
virtual ~MapBuilderInterface() {}
@ -59,10 +58,10 @@ class MapBuilderInterface {
// builder for it will return 'nullptr'.
virtual int AddTrajectoryForDeserialization() = 0;
// Returns the TrajectoryBuilder corresponding to the specified
// Returns the 'TrajectoryBuilderInterface' corresponding to the specified
// 'trajectory_id' or 'nullptr' if the trajectory has no corresponding
// builder.
virtual mapping::TrajectoryBuilder* GetTrajectoryBuilder(
virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const = 0;
// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,

33
cartographer/mapping/map_builder_test.cc
View File

@ -24,7 +24,7 @@
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "gtest/gtest.h"
namespace cartographer {
@ -37,13 +37,8 @@ constexpr double kDuration = 2.;
constexpr double kTimeStep = 0.1;
constexpr double kTravelDistance = 0.4;
struct FakeRangeMeasurement {
common::Time time;
sensor::TimedPointCloud ranges;
};
std::vector<FakeRangeMeasurement> GenerateFakeRangeMeasurements() {
std::vector<FakeRangeMeasurement> measurements;
std::vector<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {
std::vector<sensor::TimedPointCloudData> measurements;
sensor::TimedPointCloud point_cloud;
for (double angle = 0.; angle < M_PI; angle += 0.01) {
constexpr double kRadius = 5;
@ -60,7 +55,8 @@ std::vector<FakeRangeMeasurement> GenerateFakeRangeMeasurements() {
transform::Rigid3f::Translation(elapsed_time * kVelocity);
sensor::TimedPointCloud ranges =
sensor::TransformTimedPointCloud(point_cloud, pose.inverse());
measurements.emplace_back(FakeRangeMeasurement{time, ranges});
measurements.emplace_back(
sensor::TimedPointCloudData{time, Eigen::Vector3f::Zero(), ranges});
}
return measurements;
}
@ -160,13 +156,11 @@ TEST_F(MapBuilderTest, LocalSlam2D) {
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder =
TrajectoryBuilderInterface* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time,
Eigen::Vector3f::Zero(),
measurement.ranges);
trajectory_builder->AddSensorData(kRangeSensorId, measurement);
}
map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization();
@ -186,16 +180,15 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder =
TrajectoryBuilderInterface* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time,
Eigen::Vector3f::Zero(),
measurement.ranges);
trajectory_builder->AddImuData(kIMUSensorId, measurement.time,
Eigen::Vector3d(0., 0., 9.8),
Eigen::Vector3d::Zero());
trajectory_builder->AddSensorData(kRangeSensorId, measurement);
trajectory_builder->AddSensorData(
kIMUSensorId,
sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
Eigen::Vector3d::Zero()});
}
map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization();

67
cartographer/mapping/trajectory_builder.h
View File

@ -1,67 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
#include <functional>
#include <memory>
#include <string>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping {
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// This interface is used for both 2D and 3D SLAM.
class TrajectoryBuilder {
public:
TrajectoryBuilder() {}
virtual ~TrajectoryBuilder() {}
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
virtual void AddRangefinderData(const std::string& sensor_id,
common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) = 0;
virtual void AddImuData(const std::string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddOdometerData(const std::string& sensor_id, common::Time time,
const transform::Rigid3d& odometer_pose) = 0;
virtual void AddFixedFramePoseData(
const std::string& sensor_id, common::Time time,
const transform::Rigid3d& fixed_frame_pose) = 0;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_

2
cartographer/mapping/trajectory_builder.cc → cartographer/mapping/trajectory_builder_interface.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping_2d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"

54
cartographer/mapping/global_trajectory_builder_interface.h → cartographer/mapping/trajectory_builder_interface.h
View File

@ -14,48 +14,36 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#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"
#include "cartographer/sensor/timed_point_cloud_data.h"
namespace cartographer {
namespace mapping {
proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.
class GlobalTrajectoryBuilderInterface {
class TrajectoryBuilderInterface {
public:
GlobalTrajectoryBuilderInterface() {}
virtual ~GlobalTrajectoryBuilderInterface() {}
GlobalTrajectoryBuilderInterface(const GlobalTrajectoryBuilderInterface&) =
delete;
GlobalTrajectoryBuilderInterface& operator=(
const GlobalTrajectoryBuilderInterface&) = delete;
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) = 0;
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;
// A callback which is called after local SLAM processes an accumulated
// 'sensor::RangeData'. If the data was inserted into a submap, reports the
// assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
@ -64,9 +52,27 @@ class GlobalTrajectoryBuilderInterface {
transform::Rigid3d /* local pose estimate */,
sensor::RangeData /* in local frame */,
std::unique_ptr<const mapping::NodeId>)>;
TrajectoryBuilderInterface() {}
virtual ~TrajectoryBuilderInterface() {}
TrajectoryBuilderInterface(const TrajectoryBuilderInterface&) = delete;
TrajectoryBuilderInterface& operator=(const TrajectoryBuilderInterface&) =
delete;
virtual void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
virtual void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) = 0;
virtual void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) = 0;
virtual void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_INTERFACE_H_

4
cartographer/sensor/collator.cc
View File

@ -40,9 +40,9 @@ void Collator::FinishTrajectory(const int trajectory_id) {
}
void Collator::AddSensorData(const int trajectory_id,
const std::string& sensor_id,
std::unique_ptr<Data> data) {
queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data));
QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
}
void Collator::Flush() { queue_.Flush(); }

7
cartographer/sensor/collator.h
View File

@ -49,10 +49,9 @@ class Collator {
void FinishTrajectory(int trajectory_id);
// Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
// sensor data. Sensor packets with matching 'sensor_id' must be added in time
// order.
void AddSensorData(int trajectory_id, const std::string& sensor_id,
std::unique_ptr<Data> data);
// sensor data. Sensor packets with matching 'data.sensor_id_' must be added
// in time order.
void AddSensorData(int trajectory_id, std::unique_ptr<Data> data);
// Dispatches all queued sensor packets. May only be called once.
// AddSensorData may not be called after Flush.

57
cartographer/sensor/collator_test.cc
View File

@ -23,6 +23,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "gtest/gtest.h"
namespace cartographer {
@ -32,17 +33,17 @@ namespace {
TEST(Collator, Ordering) {
const std::array<std::string, 4> kSensorId = {
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
DispatchableRangefinderData zero(common::FromUniversal(0),
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData first(common::FromUniversal(100),
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData second(common::FromUniversal(200),
Eigen::Vector3f::Zero(), {});
TimedPointCloudData zero{
common::FromUniversal(0), Eigen::Vector3f::Zero(), {}};
TimedPointCloudData first{
common::FromUniversal(100), Eigen::Vector3f::Zero(), {}};
TimedPointCloudData second{
common::FromUniversal(200), Eigen::Vector3f::Zero(), {}};
ImuData third{common::FromUniversal(300)};
DispatchableRangefinderData fourth(common::FromUniversal(400),
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData fifth(common::FromUniversal(500),
Eigen::Vector3f::Zero(), {});
TimedPointCloudData fourth{
common::FromUniversal(400), Eigen::Vector3f::Zero(), {}};
TimedPointCloudData fifth{
common::FromUniversal(500), Eigen::Vector3f::Zero(), {}};
OdometryData sixth{common::FromUniversal(600),
transform::Rigid3d::Identity()};
@ -57,33 +58,17 @@ TEST(Collator, Ordering) {
constexpr int kTrajectoryId = 0;
// Establish a common start time.
collator.AddSensorData(
kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[2],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[3],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], zero));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], zero));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], zero));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(first));
collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(sixth));
collator.AddSensorData(
kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(fourth));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(second));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(fifth));
collator.AddSensorData(kTrajectoryId, kSensorId[2], MakeDispatchable(third));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], first));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], sixth));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], fourth));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], second));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], fifth));
collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], third));
ASSERT_EQ(7, received.size());
EXPECT_EQ(100, common::ToUniversal(received[4].second));

45
cartographer/sensor/data.cc
View File

@ -1,45 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/sensor/data.h"
namespace cartographer {
namespace sensor {
template <>
void Dispatchable<sensor::ImuData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddImuData(
sensor_id, data_.time, data_.linear_acceleration, data_.angular_velocity);
}
template <>
void Dispatchable<sensor::OdometryData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose);
}
template <>
void Dispatchable<sensor::FixedFramePoseData>::AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) {
trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose);
}
} // namespace sensor
} // namespace cartographer

59
cartographer/sensor/data.h
View File

@ -19,13 +19,7 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/trajectory_builder.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/mapping/trajectory_builder_interface.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
@ -33,63 +27,38 @@ namespace sensor {
class Data {
public:
explicit Data(const std::string &sensor_id) : sensor_id_(sensor_id) {}
virtual ~Data() {}
virtual common::Time GetTime() const = 0;
virtual void AddToGlobalTrajectoryBuilder(
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
const std::string &GetSensorId() const { return sensor_id_; }
virtual void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) = 0;
};
mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;
class DispatchableRangefinderData : public Data {
public:
DispatchableRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const TimedPointCloud& ranges)
: time_(time), origin_(origin), ranges_(ranges) {}
common::Time GetTime() const override { return time_; }
void AddToGlobalTrajectoryBuilder(
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
override {
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
}
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) override {
trajectory_builder->AddRangefinderData(sensor_id, time_, origin_, ranges_);
}
private:
const common::Time time_;
const Eigen::Vector3f origin_;
const TimedPointCloud ranges_;
protected:
const std::string sensor_id_;
};
template <typename DataType>
class Dispatchable : public Data {
public:
Dispatchable(const DataType& data) : data_(data) {}
Dispatchable(const std::string &sensor_id, const DataType &data)
: Data(sensor_id), data_(data) {}
common::Time GetTime() const override { return data_.time; }
void AddToGlobalTrajectoryBuilder(
mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
override {
trajectory_builder->AddSensorData(data_);
}
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilder* const trajectory_builder,
const std::string& sensor_id) override;
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
private:
const DataType data_;
};
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(const DataType& data) {
return common::make_unique<Dispatchable<DataType>>(data);
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
const std::string &sensor_id, const DataType &data) {
return common::make_unique<Dispatchable<DataType>>(sensor_id, data);
}
} // namespace sensor

7
cartographer/sensor/ordered_multi_queue_test.cc
View File

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

36
cartographer/sensor/timed_point_cloud_data.h Normal file
View File

@ -0,0 +1,36 @@
/*
* Copyright 2017 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_
#define CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_
#include "Eigen/Core"
#include "cartographer/common/time.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace sensor {
struct TimedPointCloudData {
common::Time time;
Eigen::Vector3f origin;
sensor::TimedPointCloud ranges;
};
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_TIMED_POINT_CLOUD_DATA_H_