next batch

PointCloudLibrary · Apr 29, 2020 · 69564bb · 69564bb
1 parent c6ce4a5
commit 69564bb
Showing 1 changed file with 7 additions and 39 deletions.
diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp
@@ -100,7 +100,7 @@ pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
 template <typename PointT> inline void
 pcl::getPointsInBox (const pcl::PointCloud<PointT> &cloud, 
                      Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
-                     std::vector<int> &indices)
+                     Indices &indices)
 {
   indices.resize (cloud.points.size ());
   int l = 0;
@@ -188,7 +188,7 @@ pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> inline void
-pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
+pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
                      const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
 {
   float max_dist = -FLT_MAX;
@@ -317,42 +317,12 @@ template <typename PointT> inline void
 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
                   Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
 {
-  Eigen::Array4f min_p, max_p;
-  min_p.setConstant (FLT_MAX);
-  max_p.setConstant (-FLT_MAX);
-
-  // If the data is dense, we don't need to check for NaN
-  if (cloud.is_dense)
-  {
-    for (const int &index : indices.indices)
-    {
-      pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
-  // NaN or Inf values could exist => check for them
-  else
-  {
-    for (const int &index : indices.indices)
-    {
-      // Check if the point is invalid
-      if (!std::isfinite (cloud.points[index].x) || 
-          !std::isfinite (cloud.points[index].y) || 
-          !std::isfinite (cloud.points[index].z))
-        continue;
-      pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap ();
-      min_p = min_p.min (pt);
-      max_p = max_p.max (pt);
-    }
-  }
-  min_pt = min_p;
-  max_pt = max_p;
+  getMinMax3D(cloud, indices.indices, min_pt, max_pt);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> inline void
-pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
+pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
                   Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
 {
   min_pt.setConstant (FLT_MAX);
@@ -363,7 +333,7 @@ pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &
   {
     for (const int &index : indices)
     {
-      pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap ();
+      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
       min_pt = min_pt.array ().min (pt);
       max_pt = max_pt.array ().max (pt);
     }
@@ -374,11 +344,9 @@ pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &
     for (const int &index : indices)
     {
       // Check if the point is invalid
-      if (!std::isfinite (cloud.points[index].x) || 
-          !std::isfinite (cloud.points[index].y) || 
-          !std::isfinite (cloud.points[index].z))
+      if (!isXYZFinite(cloud[index]))
         continue;
-      pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap ();
+      pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
       min_pt = min_pt.array ().min (pt);
       max_pt = max_pt.array ().max (pt);
     }