Skip to content

Instantly share code, notes, and snippets.

@gocarlos
Last active February 28, 2017 15:42
Show Gist options
  • Save gocarlos/029aaa003c465704d5f04bbbecaedcc8 to your computer and use it in GitHub Desktop.
Save gocarlos/029aaa003c465704d5f04bbbecaedcc8 to your computer and use it in GitHub Desktop.
Calculate the center of mass from the solid contained withing a point cloud or mesh.
// 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment