diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 22ec9c9..381fde2 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -197,7 +197,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) { + sensor::TimedPointCloudData GeneratePointCloudData( + const transform::Rigid3d& pose, const common::Time time) { // 360 degree rays at 16 angles. sensor::TimedPointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { @@ -237,10 +238,9 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { 0.}; returns_in_world_frame.push_back(return_point); } - return {Eigen::Vector3f::Zero(), + return {time, Eigen::Vector3f::Zero(), sensor::TransformTimedPointCloud(returns_in_world_frame, - pose.inverse().cast()), - {}}; + pose.inverse().cast())}; } void AddLinearOnlyImuObservation(const common::Time time, @@ -278,11 +278,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - const auto range_data = GenerateRangeData(node.pose); + const auto point_cloud = GeneratePointCloudData(node.pose, node.time); const std::unique_ptr - matching_result = local_trajectory_builder_->AddRangeData( - kSensorId, sensor::TimedPointCloudData{ - node.time, range_data.origin, range_data.returns}); + matching_result = + local_trajectory_builder_->AddRangeData(kSensorId, point_cloud); if (matching_result != nullptr) { EXPECT_THAT(matching_result->local_pose, transform::IsNearly(node.pose, 1e-1)); diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 2eea185..1151545 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -31,15 +31,6 @@ 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, @@ -47,13 +38,6 @@ RangeData CropRangeData(const RangeData& range_data, const float min_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)}; -} - proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 7439bc7..e7cd658 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -35,26 +35,12 @@ 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); - // Converts 'range_data' to a proto::RangeData. proto::RangeData ToProto(const RangeData& range_data);