Change computeMedian function

common/include/pcl/common/common.h

@@ -263,12 +263,40 @@ namespace pcl
   getMeanStdDev (const std::vector<float> &values, double &mean, double &stddev);
 
   /** \brief Compute the median of a list of values (fast). If the number of values is even, take the mean of the two middle values.
-    * \param[in,out] values a vector with float or double values. Will be modified!
-    * \return the median of values
+    * This function can be used like this:
+    * \code{.cpp}
+    * std::vector<double> vector{1.0, 25.0, 9.0, 4.0, 16.0};
+    * const double median = pcl::computeMedian (vector.begin (), vector.end (), static_cast<double(*)(double)>(std::sqrt)); // = 3
+    * \endcode
+    * \param[in,out] begin,end Iterators that mark the beginning and end of the value range. These values will be reordered!
+    * \param[in] f A lamda, function pointer, or similar that is implicitly applied to all values before median computation. In reality, it will be applied lazily (i.e. at most twice) and thus may not change the sorting order (e.g. monotonic functions like sqrt are allowed)
+    * \return the median
     * \ingroup common
     */
-  template<typename real> inline real
-  computeMedian (std::vector<real>& values);
+  template<typename IteratorT, typename Functor> inline auto
+  computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::iterator_traits<IteratorT>::value_type
+  {
+    const std::size_t size = std::distance(begin, end);
+    const std::size_t mid = size/2;
+    if (size%2==0)
+    { // Even number of values
+      std::nth_element (begin, begin + (mid-1), end);
+      return (f(begin[mid-1]) + f(*(std::min_element (begin + mid, end)))) / 2.0;
+    }
+    else
+    { // Odd number of values
+      std::nth_element (begin, begin + mid, end);
+      return f(begin[mid]);
+    }
+  }
+
+  /** \brief Compute the median of a list of values (fast). See the other overloaded function for more information.
+    */
+  template<typename IteratorT> inline auto
+  computeMedian (IteratorT begin, IteratorT end) noexcept -> typename std::iterator_traits<IteratorT>::value_type
+  {
+    return computeMedian (begin, end, [](const auto& x){return x;});
+  }
 }
 /*@}*/
 #include <pcl/common/impl/common.hpp>

common/include/pcl/common/impl/common.hpp

@@ -489,21 +489,5 @@ pcl::calculatePolygonArea (const pcl::PointCloud<PointT> &polygon)
   return (area*0.5);
 }
 
-template<typename real> inline real
-pcl::computeMedian (std::vector<real>& values)
-{
-  const std::size_t mid=values.size()/2;
-  if (values.size()%2==0)
-  {
-    std::nth_element (values.begin (), values.begin () + (mid - 1), values.end());
-    return (values[mid-1] + *(std::min_element (values.begin () + mid, values.end()))) / 2.0;
-  }
-  else
-  {
-    std::nth_element (values.begin (), values.begin () + mid, values.end());
-    return values[mid];
-  }
-}
-
 #endif  //#ifndef PCL_COMMON_IMPL_H_
 

sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp

@@ -42,6 +42,7 @@
 #define PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_
 
 #include <pcl/sample_consensus/lmeds.h>
+#include <pcl/common/common.h> // for computeMedian
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
@@ -102,31 +103,14 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
     const auto nr_valid_dists = std::distance (distances.begin (), new_end);
 
     // d_cur_penalty = median (distances)
-    const std::size_t mid = nr_valid_dists / 2;
     PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n", nr_valid_dists);
     if (nr_valid_dists == 0)
     {
       //iterations_++;
       ++skipped_count;
       continue;
     }
-
-    // Do we have a "middle" point or should we "estimate" one ?
-    if ((nr_valid_dists % 2) == 0)
-    {
-      // Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake
-      std::nth_element (distances.begin (), distances.begin () + (mid - 1), new_end);
-      const double tmp = distances[mid-1];
-      const double tmp2 = *(std::min_element (distances.begin () + mid, new_end));
-      d_cur_penalty = (sqrt (tmp) + sqrt (tmp2)) / 2.0;
-      PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with two values (%g and %g) because number of distances is even.\n", tmp, distances[mid]);
-    }
-    else
-    {
-      std::nth_element (distances.begin (), distances.begin () + mid, new_end);
-      d_cur_penalty = sqrt (distances[mid]);
-      PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with one value (%g) because number of distances is odd.\n", distances[mid]);
-    }
+    d_cur_penalty = pcl::computeMedian (distances.begin (), new_end, static_cast<double(*)(double)>(std::sqrt));
 
     // Better match ?
     if (d_cur_penalty < d_best_penalty)

sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp

@@ -226,22 +226,7 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedianAbsoluteDeviation (
     distances[i] = ptdiff.dot (ptdiff);
   }
 
-  double result;
-  std::size_t mid = indices->size () / 2;
-  // Do we have a "middle" point or should we "estimate" one ?
-  if (indices->size () % 2 == 0)
-  {
-    // Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake
-    std::nth_element (distances.begin (), distances.begin () + (mid - 1), distances.end ());
-    const double tmp1 = distances[mid-1];
-    const double tmp2 = *(std::min_element (distances.begin () + mid, distances.end ()));
-    result = (sqrt (tmp1) + sqrt (tmp2)) / 2;
-  }
-  else
-  {
-    std::nth_element (distances.begin (), distances.begin () + mid, distances.end ());
-    result = sqrt (distances[mid]);
-  }
+  const double result = pcl::computeMedian (distances.begin (), distances.end (), static_cast<double(*)(double)>(std::sqrt));
   return (sigma * result);
 }
 
@@ -287,9 +272,9 @@ pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedian (
     z[i] = (*cloud)[(*indices)[i]].z;
   }
 
-  median[0] = pcl::computeMedian (x);
-  median[1] = pcl::computeMedian (y);
-  median[2] = pcl::computeMedian (z);
+  median[0] = pcl::computeMedian (x.begin(), x.end());
+  median[1] = pcl::computeMedian (y.begin(), y.end());
+  median[2] = pcl::computeMedian (z.begin(), z.end());
   median[3] = 0;
 }
 

test/common/test_common.cpp

@@ -560,6 +560,25 @@ TEST (PCL, GetMaxDistance)
   test::EXPECT_EQ_VECTORS (max_exp_pt, max_pt);
 }
 
+TEST (PCL, computeMedian)
+{
+  std::vector<float> vector1{4.0f, 2.0f, 1.0f, 5.0f, 3.0f, 6.0f};
+  const auto median1 = computeMedian (vector1.begin (), vector1.end ());
+  EXPECT_EQ(median1, 3.5f);
+
+  std::vector<double> vector2{1.0, 25.0, 9.0, 4.0, 16.0};
+  const auto median2 = computeMedian (vector2.begin (), vector2.end (), static_cast<double(*)(double)>(std::sqrt));
+  EXPECT_EQ(median2, 3.0);
+
+  std::vector<double> vector3{1.0, 2.0, 6.0, 5.0, 4.0, 3.0};
+  const auto median3 = computeMedian (vector3.begin (), vector3.end (), [](const double& x){ return x+1.0; });
+  EXPECT_EQ(median3, 4.5);
+
+  std::vector<int> vector4{-1, 1, 2, 9, 15, 16};
+  const auto median4 = computeMedian (vector4.begin (), vector4.end ());
+  //EXPECT_EQ(median4, 5.5);
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)