centroid.cc

Go to the documentation of this file.

/*
 * centroid.cc 
 * Mac Mason <mmason@willowgarage.com>
 *
 * Implementation; see centroid.hh.
 */

#include "semanticmodel/centroid.hh"

namespace semanticmodel
{

Eigen::Vector4f centroid(
    const pcl16::PointCloud<pcl16::PointXYZRGB>::ConstPtr& cloud)
{
  Eigen::Vector4f centroid;
  centroid.setZero();
  if (cloud->is_dense)
  {
    for (size_t i = 0 ; i < cloud->points.size() ; ++i)
      centroid += cloud->points[i].getVector4fMap();
    centroid[3] = 0.0;
    centroid /= cloud->points.size();
  }
  else
  {
    int points = 0;
    for (size_t i = 0 ; i < cloud->points.size() ; ++i)
    {
      if (!pcl_isfinite(cloud->points[i].x) || 
          !pcl_isfinite(cloud->points[i].y) || 
          !pcl_isfinite(cloud->points[i].z))
        continue;
      centroid += cloud->points[i].getVector4fMap();
      points++;
    }
    centroid[3] = 0.0;
    centroid /= points;
  }
  return centroid;
}

}