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

@@ -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) {

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<int32>::const_iterator input_;
 };

cartographer/sensor/rangefinder_point.h

@@ -59,7 +59,7 @@ inline TimedRangefinderPoint operator*(const transform::Rigid3<T>& 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,