diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 32a5808..c6efabd 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -66,7 +66,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; } - it->second.Pulse(data->time); + it->second.Pulse(data->GetTime()); if (std::chrono::steady_clock::now() - last_logging_time_ > common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { @@ -76,29 +76,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( last_logging_time_ = std::chrono::steady_clock::now(); } - switch (data->type) { - case sensor::Data::Type::kImu: - wrapped_trajectory_builder_->AddImuData( - sensor::ImuData{data->time, data->imu.linear_acceleration, - data->imu.angular_velocity}); - return; - - case sensor::Data::Type::kRangefinder: - wrapped_trajectory_builder_->AddRangefinderData( - data->time, data->rangefinder.origin, data->rangefinder.ranges); - return; - - case sensor::Data::Type::kOdometer: - wrapped_trajectory_builder_->AddOdometerData(data->time, - data->odometer_pose); - return; - - case sensor::Data::Type::kFixedFramePose: - wrapped_trajectory_builder_->AddFixedFramePoseData( - sensor::FixedFramePoseData{data->time, data->fixed_frame_pose.pose}); - return; - } - LOG(FATAL); + data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); } } // namespace mapping diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 4d48ed3..0b7fb79 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -58,28 +58,30 @@ class TrajectoryBuilder { const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { AddSensorData(sensor_id, - common::make_unique( - time, sensor::Data::Rangefinder{origin, ranges})); + common::make_unique( + time, origin, ranges)); } 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( - time, sensor::Data::Imu{linear_acceleration, - angular_velocity})); + AddSensorData(sensor_id, common::make_unique( + 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(time, odometer_pose)); + common::make_unique( + time, odometer_pose)); } - void AddFixedFramePose(const string& sensor_id, common::Time time, - const transform::Rigid3d& fixed_frame_pose) { + void AddFixedFramePoseData(const string& sensor_id, common::Time time, + const transform::Rigid3d& fixed_frame_pose) { AddSensorData(sensor_id, - common::make_unique(time, fixed_frame_pose)); + common::make_unique( + time, fixed_frame_pose)); } }; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 544c5c8..e1a996a 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -60,7 +60,7 @@ void GlobalTrajectoryBuilder::AddFixedFramePoseData( const sensor::FixedFramePoseData& fixed_frame_pose) { sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } - + const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 4597539..2592205 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -32,62 +32,76 @@ namespace { TEST(Collator, Ordering) { const std::array kSensorId = { {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; - Data zero(common::FromUniversal(0), Data::Rangefinder{}); - Data first(common::FromUniversal(100), Data::Rangefinder{}); - Data second(common::FromUniversal(200), Data::Rangefinder{}); - Data third(common::FromUniversal(300), Data::Imu{}); - Data fourth(common::FromUniversal(400), Data::Rangefinder{}); - Data fifth(common::FromUniversal(500), Data::Rangefinder{}); - Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity()); + DispatchableRangefinderData zero(common::FromUniversal(0), + Eigen::Vector3f::Zero(), {}); + DispatchableRangefinderData first(common::FromUniversal(100), + Eigen::Vector3f::Zero(), {}); + DispatchableRangefinderData second(common::FromUniversal(200), + Eigen::Vector3f::Zero(), {}); + DispatchableImuData third(ImuData{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()); - std::vector> received; + std::vector> received; Collator collator; collator.AddTrajectory( 0, std::unordered_set(kSensorId.begin(), kSensorId.end()), [&received](const string& sensor_id, std::unique_ptr data) { - received.push_back(std::make_pair(sensor_id, *data)); + received.push_back(std::make_pair(sensor_id, data->GetTime())); }); constexpr int kTrajectoryId = 0; // Establish a common start time. - collator.AddSensorData(kTrajectoryId, kSensorId[0], - common::make_unique(zero)); - collator.AddSensorData(kTrajectoryId, kSensorId[1], - common::make_unique(zero)); - collator.AddSensorData(kTrajectoryId, kSensorId[2], - common::make_unique(zero)); - collator.AddSensorData(kTrajectoryId, kSensorId[3], - common::make_unique(zero)); + collator.AddSensorData( + kTrajectoryId, kSensorId[0], + common::make_unique(zero)); + collator.AddSensorData( + kTrajectoryId, kSensorId[1], + common::make_unique(zero)); + collator.AddSensorData( + kTrajectoryId, kSensorId[2], + common::make_unique(zero)); + collator.AddSensorData( + kTrajectoryId, kSensorId[3], + common::make_unique(zero)); - collator.AddSensorData(kTrajectoryId, kSensorId[0], - common::make_unique(first)); + collator.AddSensorData( + kTrajectoryId, kSensorId[0], + common::make_unique(first)); collator.AddSensorData(kTrajectoryId, kSensorId[3], - common::make_unique(sixth)); - collator.AddSensorData(kTrajectoryId, kSensorId[0], - common::make_unique(fourth)); - collator.AddSensorData(kTrajectoryId, kSensorId[1], - common::make_unique(second)); - collator.AddSensorData(kTrajectoryId, kSensorId[1], - common::make_unique(fifth)); + common::make_unique(sixth)); + collator.AddSensorData( + kTrajectoryId, kSensorId[0], + common::make_unique(fourth)); + collator.AddSensorData( + kTrajectoryId, kSensorId[1], + common::make_unique(second)); + collator.AddSensorData( + kTrajectoryId, kSensorId[1], + common::make_unique(fifth)); collator.AddSensorData(kTrajectoryId, kSensorId[2], - common::make_unique(third)); + common::make_unique(third)); ASSERT_EQ(7, received.size()); - EXPECT_EQ(100, common::ToUniversal(received[4].second.time)); + EXPECT_EQ(100, common::ToUniversal(received[4].second)); EXPECT_EQ(kSensorId[0], received[4].first); - EXPECT_EQ(200, common::ToUniversal(received[5].second.time)); + EXPECT_EQ(200, common::ToUniversal(received[5].second)); EXPECT_EQ(kSensorId[1], received[5].first); - EXPECT_EQ(300, common::ToUniversal(received[6].second.time)); + EXPECT_EQ(300, common::ToUniversal(received[6].second)); EXPECT_EQ(kSensorId[2], received[6].first); collator.Flush(); ASSERT_EQ(10, received.size()); EXPECT_EQ(kSensorId[0], received[7].first); - EXPECT_EQ(500, common::ToUniversal(received[8].second.time)); + EXPECT_EQ(500, common::ToUniversal(received[8].second)); EXPECT_EQ(kSensorId[1], received[8].first); - EXPECT_EQ(600, common::ToUniversal(received[9].second.time)); + EXPECT_EQ(600, common::ToUniversal(received[9].second)); EXPECT_EQ(kSensorId[3], received[9].first); } diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index b29665e..16f54d5 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -18,6 +18,9 @@ #define CARTOGRAPHER_MAPPING_DATA_H_ #include "cartographer/common/time.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -25,46 +28,79 @@ namespace cartographer { namespace sensor { -// This type is a logical union, i.e. only one type of sensor data is actually -// filled in. It is only used for time ordering sensor data before passing it -// on. -struct Data { - enum class Type { kImu, kRangefinder, kOdometer, kFixedFramePose }; +class Data { + public: + virtual ~Data() {} - struct Imu { - Eigen::Vector3d linear_acceleration; - Eigen::Vector3d angular_velocity; - }; + virtual common::Time GetTime() const = 0; + virtual void AddToTrajectoryBuilder( + mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0; +}; - struct Rangefinder { - Eigen::Vector3f origin; - PointCloud ranges; - }; +class DispatchableImuData : public Data { + public: + DispatchableImuData(const ImuData& imu_data) : imu_data_(imu_data) {} - struct FixedFramePose { - transform::Rigid3d pose; - }; + common::Time GetTime() const override { return imu_data_.time; } + void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const + trajectory_builder) override { + trajectory_builder->AddImuData(imu_data_); + } - Data(const common::Time time, const Imu& imu) - : type(Type::kImu), time(time), imu(imu) {} + private: + const ImuData imu_data_; +}; - Data(const common::Time time, const Rangefinder& rangefinder) - : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} +class DispatchableRangefinderData : public Data { + public: + DispatchableRangefinderData(const common::Time time, + const Eigen::Vector3f& origin, + const PointCloud& ranges) + : time_(time), origin_(origin), ranges_(ranges) {} - Data(const common::Time time, const transform::Rigid3d& odometer_pose) - : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} + common::Time GetTime() const override { return time_; } + void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const + trajectory_builder) override { + trajectory_builder->AddRangefinderData(time_, origin_, ranges_); + } - Data(const common::Time time, const FixedFramePose& fixed_frame_pose) - : type(Type::kFixedFramePose), - time(time), - fixed_frame_pose(fixed_frame_pose) {} + private: + const common::Time time_; + const Eigen::Vector3f origin_; + const PointCloud ranges_; +}; - Type type; - common::Time time; - Imu imu; - Rangefinder rangefinder; - transform::Rigid3d odometer_pose; - FixedFramePose fixed_frame_pose; +class DispatchableOdometerData : public Data { + public: + DispatchableOdometerData(const common::Time time, + const transform::Rigid3d& odometer_pose) + : time_(time), odometer_pose_(odometer_pose) {} + + common::Time GetTime() const override { return time_; } + void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const + trajectory_builder) override { + trajectory_builder->AddOdometerData(time_, odometer_pose_); + } + + private: + const common::Time time_; + const transform::Rigid3d odometer_pose_; +}; + +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_; }; } // namespace sensor diff --git a/cartographer/sensor/ordered_multi_queue.cc b/cartographer/sensor/ordered_multi_queue.cc index 84258ce..e159ae3 100644 --- a/cartographer/sensor/ordered_multi_queue.cc +++ b/cartographer/sensor/ordered_multi_queue.cc @@ -104,12 +104,12 @@ void OrderedMultiQueue::Dispatch() { CannotMakeProgress(it->first); return; } - if (next_data == nullptr || data->time < next_data->time) { + if (next_data == nullptr || data->GetTime() < next_data->GetTime()) { next_data = data; next_queue = &it->second; next_queue_key = it->first; } - CHECK_LE(last_dispatched_time_, next_data->time) + CHECK_LE(last_dispatched_time_, next_data->GetTime()) << "Non-sorted data added to queue: '" << it->first << "'"; ++it; } @@ -123,9 +123,9 @@ void OrderedMultiQueue::Dispatch() { const common::Time common_start_time = GetCommonStartTime(next_queue_key.trajectory_id); - if (next_data->time >= common_start_time) { + if (next_data->GetTime() >= common_start_time) { // Happy case, we are beyond the 'common_start_time' already. - last_dispatched_time_ = next_data->time; + last_dispatched_time_ = next_data->GetTime(); next_queue->callback(next_queue->queue.Pop()); } else if (next_queue->queue.Size() < 2) { if (!next_queue->finished) { @@ -133,15 +133,15 @@ void OrderedMultiQueue::Dispatch() { CannotMakeProgress(next_queue_key); return; } - last_dispatched_time_ = next_data->time; + last_dispatched_time_ = next_data->GetTime(); next_queue->callback(next_queue->queue.Pop()); } else { // We take a peek at the time after next data. If it also is not beyond // 'common_start_time' we drop 'next_data', otherwise we just found the // first packet to dispatch from this queue. std::unique_ptr next_data_owner = next_queue->queue.Pop(); - if (next_queue->queue.Peek()->time > common_start_time) { - last_dispatched_time_ = next_data->time; + if (next_queue->queue.Peek()->GetTime() > common_start_time) { + last_dispatched_time_ = next_data->GetTime(); next_queue->callback(std::move(next_data_owner)); } } @@ -165,8 +165,8 @@ common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) { if (emplace_result.second) { for (auto& entry : queues_) { if (entry.first.trajectory_id == trajectory_id) { - common_start_time = - std::max(common_start_time, entry.second.queue.Peek()->time); + common_start_time = std::max( + common_start_time, entry.second.queue.Peek()->GetTime()); } } LOG(INFO) << "All sensor data for trajectory " << trajectory_id diff --git a/cartographer/sensor/ordered_multi_queue_test.cc b/cartographer/sensor/ordered_multi_queue_test.cc index b906fb5..d78efe7 100644 --- a/cartographer/sensor/ordered_multi_queue_test.cc +++ b/cartographer/sensor/ordered_multi_queue_test.cc @@ -36,20 +36,20 @@ class OrderedMultiQueueTest : public ::testing::Test { for (const auto& queue_key : {kFirst, kSecond, kThird}) { queue_.AddQueue(queue_key, [this](std::unique_ptr data) { if (!values_.empty()) { - EXPECT_GE(data->time, values_.back().time); + EXPECT_GE(data->GetTime(), values_.back()->GetTime()); } - values_.push_back(*data); + values_.push_back(std::move(data)); }); } } std::unique_ptr MakeImu(const int ordinal) { - return common::make_unique( - common::FromUniversal(ordinal), - Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); + return common::make_unique( + sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()}); } - std::vector values_; + std::vector> values_; OrderedMultiQueue queue_; }; @@ -73,7 +73,7 @@ TEST_F(OrderedMultiQueueTest, Ordering) { EXPECT_EQ(11, values_.size()); for (size_t i = 0; i < values_.size() - 1; ++i) { - EXPECT_LE(values_[i].time, values_[i + 1].time); + EXPECT_LE(values_[i]->GetTime(), values_[i + 1]->GetTime()); } } @@ -90,7 +90,7 @@ TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { EXPECT_EQ(3, values_.size()); for (size_t i = 0; i < values_.size(); ++i) { - EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); + EXPECT_EQ(i + 1, common::ToUniversal(values_[i]->GetTime())); } }