diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index ce5788e..e2db4f0 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -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 -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 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); diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 5686376..44dadc2 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -31,8 +31,7 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set& expected_sensor_ids, - std::unique_ptr - wrapped_trajectory_builder) + std::unique_ptr 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 data) { - sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data)); + std::unique_ptr 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 diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index 7802a09..a04e29d 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -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& expected_sensor_ids, - std::unique_ptr - wrapped_trajectory_builder); + std::unique_ptr 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( - 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 data); + void AddSensorData(std::unique_ptr data); void HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr data); sensor::Collator* const sensor_collator_; const int trajectory_id_; - std::unique_ptr wrapped_trajectory_builder_; + std::unique_ptr wrapped_trajectory_builder_; // Time at which we last logged the rates of incoming sensor data. std::chrono::steady_clock::time_point last_logging_time_; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 4e756de..0fb02b4 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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(); } diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index b47ce81..c8343c4 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -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> trajectory_builders_; + std::vector> + trajectory_builders_; }; } // namespace mapping diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 93396b9..90de96e 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -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, diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 2d5ed33..9c3c994 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -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 GenerateFakeRangeMeasurements() { - std::vector measurements; +std::vector GenerateFakeRangeMeasurements() { + std::vector 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 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(); diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h deleted file mode 100644 index 05833c1..0000000 --- a/cartographer/mapping/trajectory_builder.h +++ /dev/null @@ -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 -#include -#include - -#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_ diff --git a/cartographer/mapping/trajectory_builder.cc b/cartographer/mapping/trajectory_builder_interface.cc similarity index 95% rename from cartographer/mapping/trajectory_builder.cc rename to cartographer/mapping/trajectory_builder_interface.cc index a223804..b412402 100644 --- a/cartographer/mapping/trajectory_builder.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -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" diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h similarity index 60% rename from cartographer/mapping/global_trajectory_builder_interface.h rename to cartographer/mapping/trajectory_builder_interface.h index 2aa7ea1..af009a7 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -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 #include #include -#include +#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)>; + + 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_ diff --git a/cartographer/sensor/collator.cc b/cartographer/sensor/collator.cc index cadc6e2..3b18e33 100644 --- a/cartographer/sensor/collator.cc +++ b/cartographer/sensor/collator.cc @@ -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) { - 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(); } diff --git a/cartographer/sensor/collator.h b/cartographer/sensor/collator.h index f1ef511..600ed32 100644 --- a/cartographer/sensor/collator.h +++ b/cartographer/sensor/collator.h @@ -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); + // sensor data. Sensor packets with matching 'data.sensor_id_' must be added + // in time order. + void AddSensorData(int trajectory_id, std::unique_ptr data); // Dispatches all queued sensor packets. May only be called once. // AddSensorData may not be called after Flush. diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 59f1c03..cbd311e 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -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 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(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, 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(first)); - collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(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], 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)); diff --git a/cartographer/sensor/data.cc b/cartographer/sensor/data.cc deleted file mode 100644 index c6e3626..0000000 --- a/cartographer/sensor/data.cc +++ /dev/null @@ -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::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::AddToTrajectoryBuilder( - mapping::TrajectoryBuilder* const trajectory_builder, - const std::string& sensor_id) { - trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose); -} - -template <> -void Dispatchable::AddToTrajectoryBuilder( - mapping::TrajectoryBuilder* const trajectory_builder, - const std::string& sensor_id) { - trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose); -} - -} // namespace sensor -} // namespace cartographer diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 098ce94..064545d 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -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 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 -std::unique_ptr> MakeDispatchable(const DataType& data) { - return common::make_unique>(data); +std::unique_ptr> MakeDispatchable( + const std::string &sensor_id, const DataType &data) { + return common::make_unique>(sensor_id, data); } } // namespace sensor diff --git a/cartographer/sensor/ordered_multi_queue_test.cc b/cartographer/sensor/ordered_multi_queue_test.cc index efa53e3..578c8a7 100644 --- a/cartographer/sensor/ordered_multi_queue_test.cc +++ b/cartographer/sensor/ordered_multi_queue_test.cc @@ -44,9 +44,10 @@ class OrderedMultiQueueTest : public ::testing::Test { } std::unique_ptr 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> values_; diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h new file mode 100644 index 0000000..dc34fda --- /dev/null +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -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_