Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

preserve color in surface reconstruction, skip resampling surface

  • Loading branch information...
commit 79c76dc856d57363d00063aeb07d90f05a5cc804 1 parent 51f2697
@markluffel markluffel authored
Showing with 157 additions and 1 deletion.
  1. +157 −1 surface-reconstructor/surface-reconstructor.cpp
View
158 surface-reconstructor/surface-reconstructor.cpp
@@ -5,7 +5,9 @@
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
+#include <string>
#include <iostream>
+#include <cstdio>
#include <unistd.h>
#include <pcl/visualization/pcl_visualizer.h>
@@ -17,10 +19,12 @@
void mls(std::string fname, std::string outname);
void viz(std::string fname);
void recons(std::string infile, std::string outfile);
+void colorRecons(std::string infile, std::string outfile);
void print_usage() {
std::cout << "usage: surface-reconstructor [-s something.pcd | -v | -r ]\n"
<< " -s smooth points using mls, dump to scan-mls.pcd\n"
+ << " -c reconstruct surface, preserve color, dump to scan.obj\n"
<< " -v visualize smoothed points, load from scan-mls.pcd\n"
<< " -r smooth and reconstruct surface, load from scan-mls.pcd dump to scan.vtk\n";
}
@@ -42,6 +46,17 @@ int main (int argc, char** argv) {
print_usage();
std::cout << " ! unable to find " << infile << std::endl;
}
+ } else if(strcmp(argv[1], "-c") == 0) {
+ if(argc > 2) {
+ infile = argv[2];
+ }
+ if(access(infile.c_str(), R_OK) >= 0) {
+ std::cout << "recons color\n";
+ colorRecons(infile, "scan.obj");
+ } else {
+ print_usage();
+ std::cout << " ! unable to find " << infile << std::endl;
+ }
} else if(strcmp(argv[1], "-v") == 0) {
if(access(smoothfile.c_str(), R_OK) >= 0) {
viz(smoothfile);
@@ -64,6 +79,7 @@ int main (int argc, char** argv) {
} else {
print_usage();
}
+ return 0;
}
void mls(std::string fname, std::string outname) {
@@ -130,7 +146,8 @@ void handleKeyEvent(const pcl::visualization::KeyboardEvent& ev, void *data) {
//if(ev
}
-float mag(pcl::PointNormal& p) {
+template <typename PointType>
+float mag(PointType& p) {
return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}
@@ -225,3 +242,142 @@ void recons(std::string infile, std::string outfile) {
pcl::io::saveVTKFile(outfile, triangles);
}
+// uhh, is the GreedyProjectionTriangulation template broken somehow?
+/*
+void colorRecons(std::string infile, std::string outfile) {
+ sensor_msgs::PointCloud2 cloud_blob;
+ // run this from BodyScanner/build/surface_reconstructor/
+ pcl::io::loadPCDFile(infile, cloud_blob);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
+ pcl::fromROSMsg (cloud_blob, *cloud);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+ int l = 0;
+ for(int j = 0; j < 480; j += 1) {
+ for(int i = j%2; i < 640; i += 2) {
+ int k = i + j*640;
+ pcl::PointXYZRGB& p = (*cloud)[k];
+ if((i&1 && j&1) // keep 1/4 of the points
+ && mag(p) < 3) { // keep only points within a close distance to the origin
+ filtered_cloud->push_back(p);
+ }
+ }
+ }
+
+ // Create search tree*
+ pcl::KdTree<pcl::PointXYZRGB>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
+ tree2->setInputCloud(filtered_cloud);
+
+ // Initialize objects
+ pcl::GreedyProjectionTriangulation<pcl::PointXYZRGB> gp3;
+ pcl::PolygonMesh triangles;
+
+ // Set the maximum distance between connected points (maximum edge length)
+ gp3.setSearchRadius (0.025);
+
+ // Set typical values for the parameters
+ gp3.setMu (2.5);
+ gp3.setMaximumNearestNeighbors (100);
+ gp3.setMaximumSurfaceAgle(M_PI/4); // 45 degrees
+ gp3.setMinimumAngle(M_PI/18); // 10 degrees
+ gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
+ gp3.setNormalConsistency(false);
+
+ // Get result
+ gp3.setInputCloud(filtered_cloud);
+ gp3.setSearchMethod(tree2);
+ gp3.reconstruct(triangles);
+
+ //pcl::io::savePLYFile(outfile, triangles);
+ pcl::io::saveVTKFile(outfile, triangles);
+}
+*/
+
+void saveObj(std::string outfile, pcl::PolygonMesh& mesh, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointColors) {
+ FILE *f = fopen(outfile.c_str(), "w");
+
+ for(int v = 0; v < pointColors->size(); v++) {
+ pcl::PointXYZRGB p = (*pointColors)[v];
+ fprintf(f, "v %f %f %f %f %f %f\n", p.x, p.y, p.z, p.r/255., p.g/255., p.b/255.);
+ }
+ for(int t = 0; t < mesh.polygons.size(); t++) {
+ pcl::Vertices triangle = mesh.polygons[t];
+ fprintf(f, "f %i %i %i\n", triangle.vertices[0]+1, triangle.vertices[1]+1, triangle.vertices[2]+1);
+ }
+ fclose(f);
+}
+
+void colorRecons(std::string infile, std::string outfile) {
+ sensor_msgs::PointCloud2 cloud_blob;
+ // run this from BodyScanner/build/surface_reconstructor/
+ pcl::io::loadPCDFile(infile, cloud_blob);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
+ pcl::fromROSMsg(cloud_blob, *cloud);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+ int l = 0;
+ for(int j = 0; j < 480; j += 1) {
+ for(int i = j%2; i < 640; i += 2) {
+ int k = i + j*640;
+ pcl::PointXYZRGB& p = (*cloud)[k];
+ pcl::PointXYZ pp;
+ pcl::PointXYZRGB pc;
+ pc.x = pp.x = p.x;
+ pc.y = pp.y = p.y;
+ pc.z = pp.z = p.z;
+ pc.r = p.r;
+ pc.g = p.g;
+ pc.b = p.b;
+ //pp.nx = 0; pp.ny = 0; pp.nz = 0;
+ //if((i&1 && j&1) // keep 1/4 of the points
+ if(mag(p) < 3) { // keep only points within a close distance to the origin
+ filtered_cloud->push_back(pp);
+ filtered_cloud_color->push_back(pc);
+ }
+ }
+ }
+
+ // Normal estimation*
+ pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+ pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+ pcl::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>);
+ tree->setInputCloud(filtered_cloud);
+ n.setInputCloud(filtered_cloud);
+ n.setSearchMethod(tree);
+ n.setKSearch(20);
+ n.compute(*normals);
+
+ // Concatenate the XYZ and normal fields*
+ pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
+ pcl::concatenateFields(*filtered_cloud, *normals, *filtered_cloud_with_normals);
+
+ // Create search tree*
+ pcl::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointNormal>);
+ tree2->setInputCloud(filtered_cloud_with_normals);
+
+ // Initialize objects
+ pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
+ pcl::PolygonMesh triangles;
+
+ // Set the maximum distance between connected points (maximum edge length)
+ gp3.setSearchRadius(0.07);
+
+ // Set typical values for the parameters
+ gp3.setMu(2.5);
+ gp3.setMaximumNearestNeighbors(50); // reducing this didn't fix flips
+ gp3.setMaximumSurfaceAgle(M_PI/4); // 45 degrees
+ gp3.setMinimumAngle(M_PI/18); // 10 degrees
+ gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
+ gp3.setNormalConsistency(true); // changing this didn't fix flips
+
+ // Get result
+ gp3.setInputCloud(filtered_cloud_with_normals);
+ gp3.setSearchMethod(tree2);
+ gp3.reconstruct(triangles);
+
+
+ //pcl::io::savePLYFile(outfile, triangles);
+ //pcl::io::saveVTKFile(outfile, triangles);
+ saveObj(outfile, triangles, filtered_cloud_color);
+}
Please sign in to comment.
Something went wrong with that request. Please try again.