From d4db1e79a630a3a27fcd9a270062ff6ded550fcd Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 24 Oct 2017 11:47:35 +0200 Subject: [PATCH] Add TimedPointCloud and TimedRangeData. (#601) Adds a type TimedPointCloud that holds 4-dimensional vectors where measurement time of individual points are stored in the fourth entry. Uses TimedRangeData to pass TimedPointCloud of incoming measurements to LocalTrajectoryBuilder. Fixes #573. --- .../mapping/global_trajectory_builder.h | 4 +- .../global_trajectory_builder_interface.h | 2 +- cartographer/mapping/trajectory_builder.h | 2 +- .../mapping_2d/local_trajectory_builder.cc | 15 +++--- .../mapping_2d/local_trajectory_builder.h | 3 +- .../mapping_3d/local_trajectory_builder.cc | 14 ++--- .../mapping_3d/local_trajectory_builder.h | 2 +- .../local_trajectory_builder_test.cc | 53 +++++++++++-------- cartographer/sensor/compressed_point_cloud.h | 2 +- cartographer/sensor/data.h | 4 +- cartographer/sensor/point_cloud.cc | 28 +++++++++- cartographer/sensor/point_cloud.h | 29 +++++++--- cartographer/sensor/point_cloud_test.cc | 12 +++++ cartographer/sensor/range_data.cc | 21 +++++++- cartographer/sensor/range_data.h | 14 +++++ 15 files changed, 151 insertions(+), 54 deletions(-) diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h index b091fbe..9da6c07 100644 --- a/cartographer/mapping/global_trajectory_builder.h +++ b/cartographer/mapping/global_trajectory_builder.h @@ -46,10 +46,10 @@ class GlobalTrajectoryBuilder void AddRangefinderData(const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override { + const sensor::TimedPointCloud& ranges) override { std::unique_ptr insertion_result = local_trajectory_builder_.AddRangeData( - time, sensor::RangeData{origin, ranges, {}}); + time, sensor::TimedRangeData{origin, ranges, {}}); if (insertion_result == nullptr) { return; } diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 9cc1ff6..6c4cd26 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -53,7 +53,7 @@ class GlobalTrajectoryBuilderInterface { virtual void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) = 0; + 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( diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index c5c5055..79f0155 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -56,7 +56,7 @@ class TrajectoryBuilder { void AddRangefinderData(const string& sensor_id, common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { + const sensor::TimedPointCloud& ranges) { AddSensorData(sensor_id, common::make_unique( time, origin, ranges)); diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index a70d37e..e0760ed 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -75,7 +75,7 @@ void LocalTrajectoryBuilder::ScanMatch( std::unique_ptr LocalTrajectoryBuilder::AddRangeData(const common::Time time, - const sensor::RangeData& range_data) { + const sensor::TimedRangeData& range_data) { // Initialize extrapolator now if we do not ever use an IMU. if (!options_.use_imu_data()) { InitializeExtrapolator(time); @@ -93,19 +93,22 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; } + // TODO(gaschler): Take time delta of individual points into account. const transform::Rigid3f tracking_delta = first_pose_estimate_.inverse() * extrapolator_->ExtrapolatePose(time).cast(); - const sensor::RangeData range_data_in_first_tracking = - sensor::TransformRangeData(range_data, tracking_delta); + // TODO(gaschler): Try to move this common behavior with 3D into helper. + const sensor::TimedRangeData range_data_in_first_tracking = + sensor::TransformTimedRangeData(range_data, tracking_delta); // Drop any returns below the minimum range and convert returns beyond the // maximum range into misses. - for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { - const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; + for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) { + const Eigen::Vector3f delta = + hit.head<3>() - range_data_in_first_tracking.origin; const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { - accumulated_range_data_.returns.push_back(hit); + accumulated_range_data_.returns.push_back(hit.head<3>()); } else { accumulated_range_data_.misses.push_back( range_data_in_first_tracking.origin + diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 0a6fef6..36a2040 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -38,6 +38,7 @@ namespace mapping_2d { // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) // without loop closure. +// TODO(gaschler): Add test for this class similar to the 3D test. class LocalTrajectoryBuilder { public: struct InsertionResult { @@ -56,7 +57,7 @@ class LocalTrajectoryBuilder { // Range data must be approximately horizontal for 2D SLAM. std::unique_ptr AddRangeData( - common::Time, const sensor::RangeData& range_data); + common::Time, const sensor::TimedRangeData& range_data); void AddImuData(const sensor::ImuData& imu_data); void AddOdometerData(const sensor::OdometryData& odometry_data); diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index a3c3215..c35f07b 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -60,7 +60,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { std::unique_ptr LocalTrajectoryBuilder::AddRangeData(const common::Time time, - const sensor::RangeData& range_data) { + const sensor::TimedRangeData& range_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -73,17 +73,19 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; } + // TODO(gaschler): Take time delta of individual points into account. const transform::Rigid3f tracking_delta = first_pose_estimate_.inverse() * extrapolator_->ExtrapolatePose(time).cast(); - const sensor::RangeData range_data_in_first_tracking = - sensor::TransformRangeData(range_data, tracking_delta); - for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { - const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; + const sensor::TimedRangeData range_data_in_first_tracking = + sensor::TransformTimedRangeData(range_data, tracking_delta); + for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) { + const Eigen::Vector3f delta = + hit.head<3>() - range_data_in_first_tracking.origin; const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { - accumulated_range_data_.returns.push_back(hit); + accumulated_range_data_.returns.push_back(hit.head<3>()); } else { // We insert a ray cropped to 'max_range' as a miss for hits beyond the // maximum range. This way the free space up to the maximum range will diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 81398a6..7ec9d8e 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -54,7 +54,7 @@ class LocalTrajectoryBuilder { void AddImuData(const sensor::ImuData& imu_data); std::unique_ptr AddRangeData( - common::Time time, const sensor::RangeData& range_data); + common::Time time, const sensor::TimedRangeData& range_data); void AddOdometerData(const sensor::OdometryData& odometry_data); const mapping::PoseEstimate& pose_estimate() const; diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index 461cda8..e5dbc81 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -170,37 +170,48 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) { + sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) { // 360 degree rays at 16 angles. - sensor::PointCloud directions_in_rangefinder_frame; + sensor::TimedPointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { for (int s = -250; s != 250; ++s) { - directions_in_rangefinder_frame.push_back( - Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * - Eigen::Vector3f::UnitX()); + Eigen::Vector4f first_point; + first_point << Eigen::AngleAxisf(M_PI * s / 250., + Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX(), + 0.; + directions_in_rangefinder_frame.push_back(first_point); // Second orthogonal rangefinder. - directions_in_rangefinder_frame.push_back( - Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * - Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * - Eigen::Vector3f::UnitX()); + Eigen::Vector4f second_point; + second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(M_PI * s / 250., + Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX(), + 0.; + directions_in_rangefinder_frame.push_back(second_point); } } // We simulate a 30 m edge length box around the origin, also containing // 50 cm radius spheres. - sensor::PointCloud returns_in_world_frame; - for (const Eigen::Vector3f& direction_in_world_frame : - sensor::TransformPointCloud(directions_in_rangefinder_frame, - pose.cast())) { + sensor::TimedPointCloud returns_in_world_frame; + for (const Eigen::Vector4f& direction_in_world_frame : + sensor::TransformTimedPointCloud(directions_in_rangefinder_frame, + pose.cast())) { const Eigen::Vector3f origin = pose.cast() * Eigen::Vector3f::Zero(); - returns_in_world_frame.push_back(CollideWithBubbles( - origin, CollideWithBox(origin, direction_in_world_frame))); + Eigen::Vector4f return_point; + return_point << CollideWithBubbles( + origin, CollideWithBox(origin, direction_in_world_frame.head<3>())), + 0.; + returns_in_world_frame.push_back(return_point); } return {Eigen::Vector3f::Zero(), - sensor::TransformPointCloud(returns_in_world_frame, - pose.inverse().cast()), + sensor::TransformTimedPointCloud(returns_in_world_frame, + pose.inverse().cast()), {}}; } @@ -242,8 +253,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { const auto range_data = GenerateRangeData(node.pose); if (local_trajectory_builder_->AddRangeData( node.time, - sensor::RangeData{range_data.origin, range_data.returns, {}}) != - nullptr) { + sensor::TimedRangeData{ + range_data.origin, range_data.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/sensor/compressed_point_cloud.h b/cartographer/sensor/compressed_point_cloud.h index 2fbc798..de2b083 100644 --- a/cartographer/sensor/compressed_point_cloud.h +++ b/cartographer/sensor/compressed_point_cloud.h @@ -29,7 +29,7 @@ namespace cartographer { namespace sensor { // A compressed representation of a point cloud consisting of a collection of -// points (Vector3f). +// points (Vector3f) without time information. // Internally, points are grouped by blocks. Each block encodes a bit of meta // data (number of points in block, coordinates of the block) and encodes each // point with a fixed bit rate in relation to the block. diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 985d190..8cc016a 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -42,7 +42,7 @@ class DispatchableRangefinderData : public Data { public: DispatchableRangefinderData(const common::Time time, const Eigen::Vector3f& origin, - const PointCloud& ranges) + const TimedPointCloud& ranges) : time_(time), origin_(origin), ranges_(ranges) {} common::Time GetTime() const override { return time_; } @@ -54,7 +54,7 @@ class DispatchableRangefinderData : public Data { private: const common::Time time_; const Eigen::Vector3f origin_; - const PointCloud ranges_; + const TimedPointCloud ranges_; }; template diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index bddd85b..452f3de 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -32,8 +32,21 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud, return result; } -PointCloud Crop(const PointCloud& point_cloud, const float min_z, - const float max_z) { +TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, + const transform::Rigid3f& transform) { + TimedPointCloud result; + result.reserve(point_cloud.size()); + for (const Eigen::Vector4f& point : point_cloud) { + Eigen::Vector4f result_point; + result_point.head<3>() = transform * point.head<3>(); + result_point[3] = point[3]; + result.emplace_back(result_point); + } + return result; +} + +PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, + const float max_z) { PointCloud cropped_point_cloud; for (const auto& point : point_cloud) { if (min_z <= point.z() && point.z() <= max_z) { @@ -43,5 +56,16 @@ PointCloud Crop(const PointCloud& point_cloud, const float min_z, return cropped_point_cloud; } +TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, + const float min_z, const float max_z) { + TimedPointCloud cropped_point_cloud; + for (const auto& point : point_cloud) { + if (min_z <= point.z() && point.z() <= max_z) { + cropped_point_cloud.push_back(point); + } + } + return cropped_point_cloud; +} + } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index 1824782..d2dd5e6 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -27,25 +27,38 @@ namespace cartographer { namespace sensor { +// Stores 3D positions of points. +// For 2D points, the third entry is 0.f. typedef std::vector PointCloud; -struct PointCloudWithIntensities { - PointCloud points; - std::vector intensities; +// Stores 3D positions of points with their measurement time in the fourth +// entry. Time is in seconds, increasing and relative to the moment when +// 'points[0]' was acquired. If timing is not available, all fourth entries +// are 0.f. For 2D points, the third entry is 0.f and the fourth entry is time. +typedef std::vector TimedPointCloud; - // For each item in 'points', contains the time delta of when it was acquired - // after points[0], i.e. the first entry is always 0.f. If timing - // information is not available all entries will be 0.f. - std::vector offset_seconds; +struct PointCloudWithIntensities { + TimedPointCloud points; + std::vector intensities; }; // Transforms 'point_cloud' according to 'transform'. PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform); +// Transforms 'point_cloud' according to 'transform'. +TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, + const transform::Rigid3f& transform); + // Returns a new point cloud without points that fall outside the region defined // by 'min_z' and 'max_z'. -PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z); +PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z, + float max_z); + +// Returns a new point cloud without points that fall outside the region defined +// by 'min_z' and 'max_z'. +TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, + float min_z, float max_z); } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 394a46b..d7394a7 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -37,6 +37,18 @@ TEST(PointCloudTest, TransformPointCloud) { EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); } +TEST(PointCloudTest, TransformTimedPointCloud) { + TimedPointCloud point_cloud; + point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f); + point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f); + const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud( + point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); +} + } // namespace } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 3bb58b1..439fe36 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -31,10 +31,27 @@ RangeData TransformRangeData(const RangeData& range_data, }; } +TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, + const transform::Rigid3f& transform) { + return TimedRangeData{ + transform * range_data.origin, + TransformTimedPointCloud(range_data.returns, transform), + TransformTimedPointCloud(range_data.misses, transform), + }; +} + RangeData CropRangeData(const RangeData& range_data, const float min_z, const float max_z) { - return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z), - Crop(range_data.misses, min_z, max_z)}; + return RangeData{range_data.origin, + CropPointCloud(range_data.returns, min_z, max_z), + CropPointCloud(range_data.misses, min_z, max_z)}; +} + +TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, + const float min_z, const float max_z) { + return TimedRangeData{range_data.origin, + CropTimedPointCloud(range_data.returns, min_z, max_z), + CropTimedPointCloud(range_data.misses, min_z, max_z)}; } } // namespace sensor diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 26c4b25..05873db 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -35,12 +35,26 @@ struct RangeData { PointCloud misses; }; +// Like 'RangeData', but with 'TimedPointClouds'. +struct TimedRangeData { + Eigen::Vector3f origin; + TimedPointCloud returns; + TimedPointCloud misses; +}; + RangeData TransformRangeData(const RangeData& range_data, const transform::Rigid3f& transform); +TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, + const transform::Rigid3f& transform); + // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); +// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. +TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z, + float max_z); + } // namespace sensor } // namespace cartographer