예제 #1
0
  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;
  }
예제 #2
0
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;
}