HullPoint* ConvexHullAlgorithm::computeConvexHull(int l, int r)
{
if ( l == r )
{
HullPoint *a = new HullPoint(*(this->InputPoints[l]));
a->next = a; a->prev = a;
return a;
}
if ( l + 1 == r)
{
HullPoint *a, *b;
a = new HullPoint(*(this->InputPoints[l]));
b = new HullPoint(*(this->InputPoints[r]));
a->next = b; a->prev = b;
b->next = a; b->prev = a;
return a;
}
int mid = (l + r) / 2;
HullPoint *A;
HullPoint *B;
if ( ConvexHullAlgorithm::numThreads >= ConvexHullAlgorithm::maxThreads )
{
A = computeConvexHull(l, mid);
B = computeConvexHull(mid+1, r);
}
else
{
ConvexHullAlgorithm c(InputPoints, l, mid);
std::thread t(&ConvexHullAlgorithm::Start, &c);
ConvexHullAlgorithm::numThreads++;
B = computeConvexHull(mid+1, r);
t.join();
ConvexHullAlgorithm::numThreads--;
A = c.omega;
}
return combine(A,B);
}
void SQMAlgorithm::executeSQMAlgorithm() {
straightenSkeleton();
computeConvexHull();
subdivideConvexHull();
joinBNPs();
finalVertexPlacement();
}
///////////////////////////////////////////////////////////////////////////////
// Driver program to test above functions
int testGrahamScan(const int argv, const char** argc)
{
double points[] = { 0, 3,
1, 1,
2, 2,
4, 4,
0, 0,
1, 2,
3, 1,
3, 3};
size_t n = sizeof(points)/sizeof(points[0]) / 2;
std::vector<Point*> v;
std::vector<Point*> curve;
for(size_t i=0, k=0; i<n; i++, k += 2)
{
v.push_back(newPoint(points[k], points[k+1]));
}
fprintf(stdout, "start\n");
computeConvexHull(v, curve, true);
for(size_t i=0; i<curve.size(); i++)
{
fprintf(stdout, "[%04ld] %s\n", i, curve[i]->toString().c_str());
}
return 0;
}
void SQMAlgorithm::executeSQMAlgorithm(SQMState state) {
if (sqmState > state) {
restart();
}
if (sqmState < state) {
if (sqmState < SQMStraighten && state >= SQMStraighten) {
straightenSkeleton();
}
if (sqmState < SQMComputeConvexHull && state >= SQMComputeConvexHull) {
computeConvexHull();
}
if (sqmState < SQMSubdivideConvexHull && state >= SQMSubdivideConvexHull) {
subdivideConvexHull();
}
if (sqmState < SQMJoinBNPs && state >= SQMJoinBNPs) {
joinBNPs();
}
if (sqmState < SQMFinalPlacement && state >= SQMFinalPlacement) {
finalVertexPlacement();
}
}
}
void computeConvexHull(const std::vector<CgalPoint>& points, std::vector<CgalPoint>& convex_hull) {
CgalPlane plane;
CGAL::linear_least_squares_fitting_3(points.begin(), points.end(), plane, CGAL::Dimension_tag<0>());
computeConvexHull(points, plane, convex_hull);
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" --surfel radius,order............... Compute surface elements\n");
pcl::console::print_info (" --convex ........................... Compute convex hull\n");
pcl::console::print_info (" --concave alpha .................... Compute concave hull\n");
pcl::console::print_info (" --greedy radius,max_nn,mu,surf_angle,min_angle,max_angle ... Compute greedy triangulation\n");
pcl::console::print_info (" -s output.pcd ...................... Save the output cloud\n");
return (1);
}
// Load the points
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
// Compute surface elements
SurfaceElementsPtr surfels (new SurfaceElements);
double mls_radius, polynomial_order;
bool compute_surface_elements =
pcl::console::parse_2x_arguments (argc, argv, "--surfel", mls_radius, polynomial_order) > 0;
if (compute_surface_elements)
{
surfels = computeSurfaceElements (cloud, mls_radius, polynomial_order);
pcl::console::print_info ("Computed surface elements\n");
}
// Find the convex hull
MeshPtr convex_hull;
bool compute_convex_hull = pcl::console::find_argument (argc, argv, "--convex") > 0;
if (compute_convex_hull)
{
convex_hull = computeConvexHull (cloud);
pcl::console::print_info ("Computed convex hull\n");
}
// Find the concave hull
MeshPtr concave_hull;
double alpha;
bool compute_concave_hull = pcl::console::parse_argument (argc, argv, "--concave", alpha) > 0;
if (compute_concave_hull)
{
concave_hull = computeConcaveHull (cloud, alpha);
pcl::console::print_info ("Computed concave hull\n");
}
// Compute a greedy surface triangulation
pcl::PolygonMesh::Ptr greedy_mesh;
std::string params_string;
bool perform_greedy_triangulation = pcl::console::parse_argument (argc, argv, "--greedy", params_string) > 0;
if (perform_greedy_triangulation)
{
assert (surfels);
std::vector<std::string> tokens;
boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
assert (tokens.size () == 6);
float radius = atof(tokens[0].c_str ());
int max_nearest_neighbors = atoi(tokens[1].c_str ());
float mu = atof(tokens[2].c_str ());
float max_surface_angle = atof(tokens[3].c_str ());
float min_angle = atof(tokens[4].c_str ());
float max_angle = atof(tokens[5].c_str ());
greedy_mesh = greedyTriangulation (surfels, radius, max_nearest_neighbors, mu,
max_surface_angle, min_angle, max_angle);
pcl::console::print_info ("Performed greedy surface triangulation\n");
}
// Compute a greedy surface triangulation
pcl::PolygonMesh::Ptr marching_cubes_mesh;
double leaf_size, iso_level;
bool perform_marching_cubes = pcl::console::parse_2x_arguments (argc, argv, "--mc", leaf_size, iso_level) > 0;
if (perform_marching_cubes)
{
assert (surfels);
marching_cubes_mesh = marchingCubesTriangulation (surfels, leaf_size, iso_level);
pcl::console::print_info ("Performed marching cubes surface triangulation\n");
}
// Save output
std::string output_filename;
bool save_output = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
if (save_output)
{
// Save the result
pcl::io::savePCDFile (output_filename, *cloud);
pcl::console::print_info ("Saved result as %s\n", output_filename.c_str ());
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
pcl::visualization::PCLVisualizer vis;
vis.addPointCloud (cloud);
vis.resetCamera ();
if (compute_convex_hull)
{
vis.addPolygonMesh<PointT> (convex_hull->points, convex_hull->faces, "convex_hull");
}
if (compute_concave_hull)
{
vis.addPolygonMesh<PointT> (concave_hull->points, concave_hull->faces, "concave_hull");
}
if (perform_greedy_triangulation)
{
vis.addPolygonMesh(*greedy_mesh, "greedy_mesh");
}
if (perform_marching_cubes)
{
vis.addPolygonMesh(*marching_cubes_mesh, "marching_cubes_mesh");
}
vis.spin ();
}
}
