semanticmodel: semanticmodel: centroid.cc Source File

00001 /*
00002  * centroid.cc 
00003  * Mac Mason <mmason@willowgarage.com>
00004  *
00005  * Implementation; see centroid.hh.
00006  */
00007 
00008 #include "semanticmodel/centroid.hh"
00009 
00010 namespace semanticmodel
00011 {
00012 
00013 Eigen::Vector4f centroid(
00014     const pcl16::PointCloud<pcl16::PointXYZRGB>::ConstPtr& cloud)
00015 {
00016   Eigen::Vector4f centroid;
00017   centroid.setZero();
00018   if (cloud->is_dense)
00019   {
00020     for (size_t i = 0 ; i < cloud->points.size() ; ++i)
00021       centroid += cloud->points[i].getVector4fMap();
00022     centroid[3] = 0.0;
00023     centroid /= cloud->points.size();
00024   }
00025   else
00026   {
00027     int points = 0;
00028     for (size_t i = 0 ; i < cloud->points.size() ; ++i)
00029     {
00030       if (!pcl_isfinite(cloud->points[i].x) || 
00031           !pcl_isfinite(cloud->points[i].y) || 
00032           !pcl_isfinite(cloud->points[i].z))
00033         continue;
00034       centroid += cloud->points[i].getVector4fMap();
00035       points++;
00036     }
00037     centroid[3] = 0.0;
00038     centroid /= points;
00039   }
00040   return centroid;
00041 }
00042 
00043 }