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

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);

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

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_;

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();
}

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

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,

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();

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_

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"

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_

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(); }

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.

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));

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

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

View File

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

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_