bool compute3DCentroid(std::vector<Point3d>& cloud, Eigen::Vector4d& centroid)
{
if (cloud.empty())
return false;
// Initialize to all 0
centroid.setZero();
for(int i=0; i< cloud.size(); ++i)
{
centroid[0] += cloud[i].x;
centroid[1] += cloud[i].y;
centroid[2] += cloud[i].z;
// printf("Point[%d]: %lf %lf %lf\n", cloud[i].x, cloud[i].y, cloud[i].z);
}
centroid[3] = 0.0;
centroid /= static_cast<double>(cloud.size());
return true;
}
Eigen::Vector4d Centroid3D::computeCentroid (PointCloud3D *inCloud, const std::vector<int> &indices) {
Eigen::Vector4d centroid;
centroid.setZero ();
if (indices.size () == 0) {
return Eigen::Vector4d(0,0,0,0);
}
// For each point in the cloud
int cp = 0;
for (size_t i = 0; i < indices.size (); ++i) {
// Check if the point is invalid
if ( isnan( (*inCloud->getPointCloud())[indices[i]].getX() ) || isnan( (*inCloud->getPointCloud())[indices[i]].getY() ) || isnan( (*inCloud->getPointCloud())[indices[i]].getZ() ))
continue;
centroid[0] += (*inCloud->getPointCloud())[indices[i]].getX();
centroid[1] += (*inCloud->getPointCloud())[indices[i]].getY();
centroid[2] += (*inCloud->getPointCloud())[indices[i]].getZ();
cp++;
}
centroid /= cp;
return centroid;
}