Calculate the center of mass from the solid contained withing a point cloud or mesh.

CalculateCenterOfMassFromMesh.cpp

// Reference: http://mathworld.wolfram.com/Tetrahedron.html
void Common::CalculateObjectsProperties(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
    Eigen::Vector3d* center_of_mass, double* totalVolume, double* weight) {
  LOG(INFO) << "CalculateObjectsProperties start.\n";

  CHECK_NOTNULL(cloud.get());
  CHECK_NOTNULL(center_of_mass);
  CHECK_NOTNULL(totalVolume);
  CHECK_NOTNULL(weight);

  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  for (size_t i = 0; i < cloud->points.size(); ++i) {
    points->InsertNextPoint(cloud->points[i].x, cloud->points[i].y,
                            cloud->points[i].z);
  }
  vtkSmartPointer<vtkPolyData> original_vtk_mesh =
      vtkSmartPointer<vtkPolyData>::New();
  original_vtk_mesh->SetPoints(points);

  vtkSmartPointer<vtkPolyData> convex_hull =
      vtkSmartPointer<vtkPolyData>::New();
  CalculateConvexHull(original_vtk_mesh, convex_hull);

  LOG(INFO) << "Number of cells " << convex_hull->GetNumberOfCells();
  LOG(INFO) << "Number of points " << convex_hull->GetNumberOfPoints();
  LOG(INFO) << "Number of polys " << convex_hull->GetNumberOfPolys();

  int numTriangles = convex_hull->GetNumberOfCells();
  LOG(INFO) << "Number of triangles is " << numTriangles;
  CHECK_GT(numTriangles, 0);

  /*!
 * Struct which defines a triangle in a mesh.
 */
struct Triangle  // 3 vertices of each triangle
{
  float x1, y1, z1;
  float x2, y2, z2;
  float x3, y3, z3;
};
  
  Triangle* triangles = new Triangle[convex_hull->GetNumberOfCells()];

  double p0[3];
  double p1[3];
  double p2[3];

  vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();
  for (size_t i = 0; i < convex_hull->GetNumberOfCells(); ++i) {
    vtkCell* cell = convex_hull->GetCell(i);
    vtkTriangle* triangle = dynamic_cast<vtkTriangle*>(cell);
    pts = cell->GetPoints();
    CHECK(pts->GetNumberOfPoints() == 3);

    triangle->GetPoints()->GetPoint(0, p0);
    triangle->GetPoints()->GetPoint(1, p1);
    triangle->GetPoints()->GetPoint(2, p2);

    triangles[i].x1 = p0[0];
    triangles[i].y1 = p0[1];
    triangles[i].z1 = p0[2];

    triangles[i].x2 = p1[0];
    triangles[i].y2 = p1[1];
    triangles[i].z2 = p1[2];

    triangles[i].x3 = p2[0];
    triangles[i].y3 = p2[1];
    triangles[i].z3 = p2[2];
  }

  *totalVolume = 0;
  double currentVolume;
  double xCenter = 0, yCenter = 0, zCenter = 0;

  for (int i = 0; i < convex_hull->GetNumberOfCells(); i++) {
    *totalVolume += currentVolume =
        (triangles[i].x1 * triangles[i].y2 * triangles[i].z3 -
         triangles[i].x1 * triangles[i].y3 * triangles[i].z2 -
         triangles[i].x2 * triangles[i].y1 * triangles[i].z3 +
         triangles[i].x2 * triangles[i].y3 * triangles[i].z1 +
         triangles[i].x3 * triangles[i].y1 * triangles[i].z2 -
         triangles[i].x3 * triangles[i].y2 * triangles[i].z1) /
        6;
    xCenter += ((triangles[i].x1 + triangles[i].x2 + triangles[i].x3) / 4) *
               currentVolume;
    yCenter += ((triangles[i].y1 + triangles[i].y2 + triangles[i].y3) / 4) *
               currentVolume;
    zCenter += ((triangles[i].z1 + triangles[i].z2 + triangles[i].z3) / 4) *
               currentVolume;
  }

  LOG(INFO) << "Total Volume = " << *totalVolume << " m³,  are "
            << (*totalVolume) * 1000.0 << " liter.";
  LOG(INFO) << "X center = " << xCenter / *totalVolume;
  LOG(INFO) << "Y center = " << yCenter / *totalVolume;
  LOG(INFO) << "Z center = " << zCenter / *totalVolume;

  center_of_mass->x() = xCenter / *totalVolume;
  center_of_mass->y() = yCenter / *totalVolume;
  center_of_mass->z() = zCenter / *totalVolume;
}