Use pcl common functions

Author: truhoang

recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp

@@ -44,7 +44,8 @@
 #include <pcl/point_cloud.h>
 #include <limits>
 #include <Eigen/Dense>
-
+#include <pcl/common/common.h>
+#include <pcl/common/centroid.h>
 
 namespace pcl
 {
@@ -167,36 +168,20 @@ pcl::LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxAndCenterTemplatePointClo
   bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
   bb.width = bb.height = bb.depth = 0.0f;
 
-  Eigen::Vector3f geometric_center = Eigen::Vector3f::Zero ();
-  Eigen::Vector3f min_pos, max_pos;
+  Eigen::Vector4f geometric_center = Eigen::Vector4f::Zero ();
+  Eigen::Vector4f min_pos, max_pos;
   min_pos.fill (std::numeric_limits<float>::max ());
   max_pos.fill (std::numeric_limits<float>::lowest ());
-  std::size_t counter = 0;
-  for (std::size_t j = 0; j < template_point_cloud.size (); ++j) 
-  {
-    const PointXYZRGBA & p = template_point_cloud.points[j];
-
-    if (!isXYZFinite(p))
-      continue;
-
-    for (std::size_t row=0; row<3; ++row)
-    {
-      min_pos[row] = std::min (min_pos[row], p.getVector3fMap ()[row]);
-      max_pos[row] = std::max (max_pos[row], p.getVector3fMap ()[row]);
-    }
-
-    geometric_center += p.getVector3fMap ();
-
-    ++counter;
-  }
-  geometric_center /= static_cast<float> (counter);
+  
+  pcl::getMinMax3D(template_point_cloud, min_pos, max_pos);
+  pcl::compute3DCentroid(template_point_cloud, geometric_center);
 
-  Eigen::Vector3f bb_dim = max_pos - min_pos;
+  Eigen::Vector3f bb_dim = (max_pos - min_pos).head<3>();
   bb.width  = bb_dim[0];
   bb.height = bb_dim[1];
   bb.depth  = bb_dim[2];
 
-  Eigen::Vector3f diff_pos = min_pos - geometric_center;
+  Eigen::Vector3f diff_pos = (min_pos - geometric_center).head<3>();
   bb.x = diff_pos[0];
   bb.y = diff_pos[1];
   bb.z = diff_pos[2];
@@ -208,7 +193,7 @@ pcl::LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxAndCenterTemplatePointClo
     if (!isXYZFinite(p))
       continue;
 
-    p.getVector3fMap () -= geometric_center;
+    p.getVector4fMap () -= geometric_center;
 
     template_point_cloud.points[j] = p;
   }