From 4064c227111eb6dbfc1141c19c6c44571b38937e Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Wed, 9 Mar 2022 15:58:20 +0100 Subject: [PATCH] Fix RangefinderPoint comparison Before the origin was not taken into account when comparing RangefinderPoint. This commit add the comparison of the origin when comparing two RangefinderPoint. --- cartographer/sensor/compressed_point_cloud.cc | 9 ++++++++- cartographer/sensor/compressed_point_cloud.h | 1 + cartographer/sensor/rangefinder_point.h | 2 +- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/cartographer/sensor/compressed_point_cloud.cc b/cartographer/sensor/compressed_point_cloud.cc index f5337e57d2..de5f97b6a1 100644 --- a/cartographer/sensor/compressed_point_cloud.cc +++ b/cartographer/sensor/compressed_point_cloud.cc @@ -58,7 +58,7 @@ CompressedPointCloud::ConstIterator::EndIterator( RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const { CHECK_GT(remaining_points_, 0); - return {current_point_}; + return {current_point_, current_point_origin_}; } CompressedPointCloud::ConstIterator& @@ -94,6 +94,10 @@ void CompressedPointCloud::ConstIterator::ReadNextPoint() { current_point_[2] = (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) * kPrecision; + // TODO(Magnago): at the moment serialization of the origin is not supported. + // Here we assume set the origin to (0,0,0). Fix this when the origin + // will be stored inside the CompressedPointCloud. + current_point_origin_ = Eigen::Vector3f::Zero(); } CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) @@ -113,6 +117,9 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision, 1 << kMaxBitsPerDirection) << "Point out of bounds: " << point.position; + // TODO(Magnago): point.origin is not compressed. Proper compression of the + // origin should be implemented enabling the recovery of the correct value + // when uncompressing. Eigen::Array3i raster_point; Eigen::Array3i block_coordinate; for (int i = 0; i < 3; ++i) { diff --git a/cartographer/sensor/compressed_point_cloud.h b/cartographer/sensor/compressed_point_cloud.h index 6cb01cc089..14a9e5b110 100644 --- a/cartographer/sensor/compressed_point_cloud.h +++ b/cartographer/sensor/compressed_point_cloud.h @@ -86,6 +86,7 @@ class CompressedPointCloud::ConstIterator { size_t remaining_points_; int32 remaining_points_in_current_block_; Eigen::Vector3f current_point_; + Eigen::Vector3f current_point_origin_; Eigen::Vector3i current_block_coordinates_; std::vector::const_iterator input_; }; diff --git a/cartographer/sensor/rangefinder_point.h b/cartographer/sensor/rangefinder_point.h index bb1b3238ce..65798e3f6d 100644 --- a/cartographer/sensor/rangefinder_point.h +++ b/cartographer/sensor/rangefinder_point.h @@ -59,7 +59,7 @@ inline TimedRangefinderPoint operator*(const transform::Rigid3& lhs, inline bool operator==(const RangefinderPoint& lhs, const RangefinderPoint& rhs) { - return lhs.position == rhs.position; + return lhs.position == rhs.position && lhs.origin == rhs.origin; } inline bool operator==(const TimedRangefinderPoint& lhs,