From add8b5757e573c3e66aa615e2e3ec3c77bc172bd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Se=CC=81rgio=20Agostinho?= <sergio.r.agostinho@gmail.com>
Date: Sat, 28 Nov 2015 17:35:33 +0100
Subject: [PATCH] Fixes incorrect distance calculation in getMaxDistance

Fixes #1449. getMaxDistance was using the norm of a Eigen::Vector4f
to compute the 3D distance between points. The norm was using the
4th element of xyz/data[4] which is often just bit trash. When an
index vector is provided, the function was returning the wrong
point. Added tests for both
---
 common/include/pcl/common/impl/common.hpp | 20 +++++++++-------
 test/common/test_common.cpp               | 29 +++++++++++++++++++++++
 2 files changed, 40 insertions(+), 9 deletions(-)

diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp
index 0f6a340a9dd..520f20008e5 100644
--- a/common/include/pcl/common/impl/common.hpp
+++ b/common/include/pcl/common/impl/common.hpp
@@ -132,14 +132,15 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f
   float max_dist = -FLT_MAX;
   int max_idx = -1;
   float dist;
+  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
 
   // If the data is dense, we don't need to check for NaN
   if (cloud.is_dense)
   {
     for (size_t i = 0; i < cloud.points.size (); ++i)
     {
-      pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
-      dist = (pivot_pt - pt).norm ();
+      pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
+      dist = (pivot_pt3 - pt).norm ();
       if (dist > max_dist)
       {
         max_idx = int (i);
@@ -155,8 +156,8 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f
       // Check if the point is invalid
       if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
         continue;
-      pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
-      dist = (pivot_pt - pt).norm ();
+      pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
+      dist = (pivot_pt3 - pt).norm ();
       if (dist > max_dist)
       {
         max_idx = int (i);
@@ -179,14 +180,15 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int
   float max_dist = -FLT_MAX;
   int max_idx = -1;
   float dist;
+  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
 
   // If the data is dense, we don't need to check for NaN
   if (cloud.is_dense)
   {
     for (size_t i = 0; i < indices.size (); ++i)
     {
-      pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
-      dist = (pivot_pt - pt).norm ();
+      pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
+      dist = (pivot_pt3 - pt).norm ();
       if (dist > max_dist)
       {
         max_idx = static_cast<int> (i);
@@ -205,8 +207,8 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int
           !pcl_isfinite (cloud.points[indices[i]].z))
         continue;
 
-      pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
-      dist = (pivot_pt - pt).norm ();
+      pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
+      dist = (pivot_pt3 - pt).norm ();
       if (dist > max_dist)
       {
         max_idx = static_cast<int> (i);
@@ -216,7 +218,7 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int
   }
 
   if(max_idx != -1)
-    max_pt = cloud.points[max_idx].getVector4fMap ();
+    max_pt = cloud.points[indices[max_idx]].getVector4fMap ();
   else
     max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
 }
diff --git a/test/common/test_common.cpp b/test/common/test_common.cpp
index a753406935e..5ade2ff22f3 100644
--- a/test/common/test_common.cpp
+++ b/test/common/test_common.cpp
@@ -522,6 +522,35 @@ TEST (PCL, HasField)
   EXPECT_FALSE ((pcl::traits::has_label<pcl::Normal>::value));
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, GetMaxDistance)
+{
+  PointCloud<PointXYZ> cloud;
+  Eigen::Vector4f max_pt, max_exp_pt;
+  const Eigen::Vector4f pivot_pt (Eigen::Vector4f::Zero ());
+
+  // populate cloud
+  cloud.points.resize (3);
+  cloud[0].data[0] = 4.f; cloud[0].data[1] = 3.f;
+  cloud[0].data[2] = 0.f; cloud[0].data[3] = 0.f;
+  cloud[1].data[0] = 0.f; cloud[1].data[1] = 0.f;
+  cloud[1].data[2] = 0.f; cloud[1].data[3] = 1000.f;  //This term should not influence max dist
+  cloud[2].data[0] = -1.5f; cloud[2].data[1] = 1.5f;
+  cloud[2].data[2] = -.5f; cloud[2].data[3] = 0.f;
+
+  // No indices specified
+  max_exp_pt = cloud[0].getVector4fMap ();
+  getMaxDistance (cloud, pivot_pt, max_pt);
+  EXPECT_EQ (max_exp_pt, max_pt);
+
+  // Specifying indices
+  std::vector<int> idx (2);
+  idx[0] = 1; idx[1] = 2;
+  max_exp_pt = cloud[2].getVector4fMap ();
+  getMaxDistance (cloud, idx, pivot_pt, max_pt);
+  EXPECT_EQ (max_exp_pt, max_pt);
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)