diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index dbf9c00..c621075 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -88,8 +88,9 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData( data->imu.angular_velocity); return; - case sensor::Data::Type::kLaserFan: - wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan); + case sensor::Data::Type::kRangefinder: + wrapped_trajectory_builder_->AddRangefinderData( + data->time, data->rangefinder.origin, data->rangefinder.ranges); return; case sensor::Data::Type::kOdometer: diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index cbf7619..086bba2 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -50,8 +50,9 @@ class GlobalTrajectoryBuilderInterface { virtual const Submaps* submaps() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; - virtual void AddLaserFan(common::Time time, - const sensor::LaserFan& laser_fan) = 0; + virtual void AddRangefinderData(common::Time time, + const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) = 0; virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 867c743..92b2036 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -83,10 +83,12 @@ class TrajectoryBuilder { virtual void AddSensorData(const string& sensor_id, std::unique_ptr data) = 0; - void AddLaserFan(const string& sensor_id, common::Time time, - const sensor::LaserFan& laser_fan) { + void AddRangefinderData(const string& sensor_id, common::Time time, + const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) { AddSensorData(sensor_id, - common::make_unique(time, laser_fan)); + common::make_unique( + time, sensor::Data::Rangefinder{origin, ranges})); } void AddImuData(const string& sensor_id, common::Time time, diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index d8be710..f7d3b8d 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -32,10 +32,12 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const { return local_trajectory_builder_.submaps(); } -void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time, - const sensor::LaserFan& laser_fan) { +void GlobalTrajectoryBuilder::AddRangefinderData( + const common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) { std::unique_ptr insertion_result = - local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); + local_trajectory_builder_.AddHorizontalLaserFan( + time, sensor::LaserFan{origin, ranges, {}, {}}); if (insertion_result != nullptr) { sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index d6dadb2..cfdb3f5 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -38,10 +38,10 @@ class GlobalTrajectoryBuilder const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const override; - // Projects 'laser_fan' to 2D, and therefore should be approximately - // horizontal. - void AddLaserFan(common::Time time, - const sensor::LaserFan& laser_fan) override; + // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately + // parallel to the ground plane. + void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddOdometerData( diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index e296a0e..8d0154f 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -41,10 +41,11 @@ void GlobalTrajectoryBuilder::AddImuData( sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); } -void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time, - const sensor::LaserFan& laser_fan) { +void GlobalTrajectoryBuilder::AddRangefinderData( + const common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) { auto insertion_result = - local_trajectory_builder_->AddLaserFan(time, laser_fan); + local_trajectory_builder_->AddRangefinderData(time, origin, ranges); if (insertion_result == nullptr) { return; diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index b9fd76d..0b6b9b9 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -40,8 +40,8 @@ class GlobalTrajectoryBuilder const mapping_3d::Submaps* submaps() const override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - void AddLaserFan(common::Time time, - const sensor::LaserFan& laser_fan) override; + void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) override; void AddOdometerData( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 3e4bcd9..004918d 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -70,8 +70,9 @@ void KalmanLocalTrajectoryBuilder::AddImuData( } std::unique_ptr -KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time, - const sensor::LaserFan& laser_fan) { +KalmanLocalTrajectoryBuilder::AddRangefinderData( + const common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) { if (!pose_tracker_) { LOG(INFO) << "PoseTracker not yet initialized."; return nullptr; @@ -89,7 +90,8 @@ KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time, const transform::Rigid3f tracking_delta = first_pose_prediction_.inverse() * pose_prediction.cast(); const sensor::LaserFan laser_fan_in_first_tracking = - sensor::TransformLaserFan(laser_fan, tracking_delta); + sensor::TransformLaserFan(sensor::LaserFan{origin, ranges, {}, {}}, + tracking_delta); for (const Eigen::Vector3f& laser_return : laser_fan_in_first_tracking.returns) { const Eigen::Vector3f delta = diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 65ef646..3d1a922 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -48,8 +48,9 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - std::unique_ptr AddLaserFan( - common::Time time, const sensor::LaserFan& laser_fan) override; + std::unique_ptr AddRangefinderData( + common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) override; void AddOdometerData( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 41da9d7..1667442 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -256,8 +256,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - if (local_trajectory_builder_->AddLaserFan( - node.time, GenerateLaserFan(node.pose)) != nullptr) { + const auto laser_fan = GenerateLaserFan(node.pose); + if (local_trajectory_builder_->AddRangefinderData( + node.time, laser_fan.origin, laser_fan.returns) != nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); ++num_poses; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 01ece27..0bf14b6 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -53,8 +53,9 @@ class LocalTrajectoryBuilderInterface { virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; - virtual std::unique_ptr AddLaserFan( - common::Time time, const sensor::LaserFan& laser_fan) = 0; + virtual std::unique_ptr AddRangefinderData( + common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) = 0; virtual void AddOdometerData( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) = 0; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 5c08d37..e2ab29d 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -133,15 +133,16 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData( } std::unique_ptr -OptimizingLocalTrajectoryBuilder::AddLaserFan( - const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { - CHECK_GT(laser_fan_in_tracking.returns.size(), 0); +OptimizingLocalTrajectoryBuilder::AddRangefinderData( + const common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) { + CHECK_GT(ranges.size(), 0); // TODO(hrapp): Handle misses. // TODO(hrapp): Where are NaNs in laser_fan_in_tracking coming from? sensor::PointCloud point_cloud; - for (const Eigen::Vector3f& laser_return : laser_fan_in_tracking.returns) { - const Eigen::Vector3f delta = laser_return - laser_fan_in_tracking.origin; + for (const Eigen::Vector3f& laser_return : ranges) { + const Eigen::Vector3f delta = laser_return - origin; const float range = delta.norm(); if (range >= options_.laser_min_range()) { if (range <= options_.laser_max_range()) { diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 562a0af..422b53f 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -52,10 +52,9 @@ class OptimizingLocalTrajectoryBuilder void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - std::unique_ptr AddLaserFan( - common::Time time, - const sensor::LaserFan& laser_fan_in_tracking) override; - + std::unique_ptr AddRangefinderData( + common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges) override; void AddOdometerData( const common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 1a6fcd5..cea4223 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -31,12 +31,12 @@ namespace { TEST(Collator, Ordering) { const std::array kSensorId = { {"horizontal_laser", "vertical_laser", "imu", "odometry"}}; - Data zero(common::FromUniversal(0), sensor::LaserFan{}); - Data first(common::FromUniversal(100), sensor::LaserFan{}); - Data second(common::FromUniversal(200), sensor::LaserFan{}); + Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{}); + Data first(common::FromUniversal(100), sensor::Data::Rangefinder{}); + Data second(common::FromUniversal(200), sensor::Data::Rangefinder{}); Data third(common::FromUniversal(300), Data::Imu{}); - Data fourth(common::FromUniversal(400), sensor::LaserFan{}); - Data fifth(common::FromUniversal(500), sensor::LaserFan{}); + Data fourth(common::FromUniversal(400), sensor::Data::Rangefinder{}); + Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{}); Data sixth(common::FromUniversal(600), Data::Odometer{}); std::vector> received; diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 3cbc412..5cb9705 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -20,6 +20,7 @@ #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/sensor/laser.h" +#include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -29,7 +30,17 @@ namespace sensor { // filled in. It is only used for time ordering sensor data before passing it // on. struct Data { - enum class Type { kImu, kLaserFan, kOdometer }; + enum class Type { kImu, kRangefinder, kOdometer }; + + struct Imu { + Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; + }; + + struct Rangefinder { + Eigen::Vector3f origin; + PointCloud ranges; + }; struct Odometer { transform::Rigid3d pose; @@ -38,17 +49,11 @@ struct Data { kalman_filter::PoseCovariance covariance; }; - struct Imu { - Eigen::Vector3d linear_acceleration; - Eigen::Vector3d angular_velocity; - }; - Data(const common::Time time, const Imu& imu) : type(Type::kImu), time(time), imu(imu) {} - Data(const common::Time time, - const ::cartographer::sensor::LaserFan& laser_fan) - : type(Type::kLaserFan), time(time), laser_fan(laser_fan) {} + Data(const common::Time time, const Rangefinder& rangefinder) + : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} Data(const common::Time time, const Odometer& odometer) : type(Type::kOdometer), time(time), odometer(odometer) {} @@ -56,7 +61,7 @@ struct Data { Type type; common::Time time; Imu imu; - sensor::LaserFan laser_fan; + Rangefinder rangefinder; Odometer odometer; };