Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

tools: Add option to write normals to mesh_sampling #1795

Merged
merged 2 commits into from
Apr 18, 2017
+50 −19
Merged

tools: Add option to write normals to mesh_sampling #1795

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fad5579
tools: Add option to write normals to mesh_sampling
Jan 12, 2017
fa73be3
tools: Rename res to voxel_cloud
Jan 19, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 50 additions & 19 deletions tools/mesh_sampling.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ randomPointTriangle (float a1, float a2, float a3, float b1, float b2, float b3,
}

inline void
randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea, Eigen::Vector4f& p)
randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea, Eigen::Vector4f& p, bool calcNormal, Eigen::Vector3f& n)
{
float r = static_cast<float> (uniform_deviate (rand ()) * totalArea);

Expand All @@ -95,13 +95,21 @@ randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, dou
polydata->GetPoint (ptIds[0], A);
polydata->GetPoint (ptIds[1], B);
polydata->GetPoint (ptIds[2], C);
randomPointTriangle (float (A[0]), float (A[1]), float (A[2]),
float (B[0]), float (B[1]), float (B[2]),
if (calcNormal)
{
// OBJ: Vertices are stored in a counter-clockwise order by default
Eigen::Vector3f v1 = Eigen::Vector3f (A[0], A[1], A[2]) - Eigen::Vector3f (C[0], C[1], C[2]);
Eigen::Vector3f v2 = Eigen::Vector3f (B[0], B[1], B[2]) - Eigen::Vector3f (C[0], C[1], C[2]);
n = v1.cross (v2);
n.normalize ();
}
randomPointTriangle (float (A[0]), float (A[1]), float (A[2]),
float (B[0]), float (B[1]), float (B[2]),
float (C[0]), float (C[1]), float (C[2]), p);
}

void
uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, size_t n_samples, pcl::PointCloud<pcl::PointXYZ> & cloud_out)
uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, size_t n_samples, bool calc_normal, pcl::PointCloud<pcl::PointNormal> & cloud_out)
{
polydata->BuildCells ();
vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
Expand All @@ -126,19 +134,26 @@ uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, size_t n_samples, pcl::
for (i = 0; i < n_samples; i++)
{
Eigen::Vector4f p;
randPSurface (polydata, &cumulativeAreas, totalArea, p);
Eigen::Vector3f n;
randPSurface (polydata, &cumulativeAreas, totalArea, p, calc_normal, n);
cloud_out.points[i].x = p[0];
cloud_out.points[i].y = p[1];
cloud_out.points[i].z = p[2];
if (calc_normal)
{
cloud_out.points[i].normal_x = n[0];
cloud_out.points[i].normal_y = n[1];
cloud_out.points[i].normal_z = n[2];
}
}
}

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

int default_number_samples = 100000;
float default_leaf_size = 0.01f;
const int default_number_samples = 100000;
const float default_leaf_size = 0.01f;

void
printHelp (int, char **argv)
Expand All @@ -152,6 +167,7 @@ printHelp (int, char **argv)
" -leaf_size X = the XYZ leaf size for the VoxelGrid -- for data reduction (default: ");
print_value ("%f", default_leaf_size);
print_info (" m)\n");
print_info (" -write_normals = flag to write normals to the output pcd\n");
print_info (
" -no_vis_result = flag to stop visualizing the generated pcd\n");
}
Expand All @@ -175,6 +191,7 @@ main (int argc, char **argv)
float leaf_size = default_leaf_size;
parse_argument (argc, argv, "-leaf_size", leaf_size);
bool vis_result = ! find_switch (argc, argv, "-no_vis_result");
const bool write_normals = find_switch (argc, argv, "-write_normals");

// Parse the command line arguments for .ply and PCD files
std::vector<int> pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
Expand All @@ -191,7 +208,7 @@ main (int argc, char **argv)
return (-1);
}

vtkSmartPointer<vtkPolyData> polydata1 = vtkSmartPointer<vtkPolyData>::New ();;
vtkSmartPointer<vtkPolyData> polydata1 = vtkSmartPointer<vtkPolyData>::New ();
if (ply_file_indices.size () == 1)
{
pcl::PolygonMesh mesh;
Expand All @@ -217,8 +234,8 @@ main (int argc, char **argv)

vtkSmartPointer<vtkPolyDataMapper> triangleMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
triangleMapper->SetInputConnection (triangleFilter->GetOutputPort ());
triangleMapper->Update();
polydata1 = triangleMapper->GetInput();
triangleMapper->Update ();
polydata1 = triangleMapper->GetInput ();

bool INTER_VIS = false;

Expand All @@ -227,33 +244,47 @@ main (int argc, char **argv)
visualization::PCLVisualizer vis;
vis.addModelFromPolyData (polydata1, "mesh1", 0);
vis.setRepresentationToSurfaceForAllActors ();
vis.spin();
vis.spin ();
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointXYZ>);
uniform_sampling (polydata1, SAMPLE_POINTS_, *cloud_1);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointNormal>);
uniform_sampling (polydata1, SAMPLE_POINTS_, write_normals, *cloud_1);

if (INTER_VIS)
{
visualization::PCLVisualizer vis_sampled;
vis_sampled.addPointCloud (cloud_1);
vis_sampled.addPointCloud<pcl::PointNormal> (cloud_1);
if (write_normals)
vis_sampled.addPointCloudNormals<pcl::PointNormal> (cloud_1, 1, 0.02f, "cloud_normals");
vis_sampled.spin ();
}

// Voxelgrid
VoxelGrid<PointXYZ> grid_;
VoxelGrid<PointNormal> grid_;
grid_.setInputCloud (cloud_1);
grid_.setLeafSize (leaf_size, leaf_size, leaf_size);

pcl::PointCloud<pcl::PointXYZ>::Ptr res(new pcl::PointCloud<pcl::PointXYZ>);
grid_.filter (*res);
pcl::PointCloud<pcl::PointNormal>::Ptr voxel_cloud (new pcl::PointCloud<pcl::PointNormal>);
grid_.filter (*voxel_cloud);

if (vis_result)
{
visualization::PCLVisualizer vis3 ("VOXELIZED SAMPLES CLOUD");
vis3.addPointCloud (res);
vis3.addPointCloud<pcl::PointNormal> (voxel_cloud);
if (write_normals)
vis3.addPointCloudNormals<pcl::PointNormal> (voxel_cloud, 1, 0.02f, "cloud_normals");
vis3.spin ();
}

savePCDFileASCII (argv[pcd_file_indices[0]], *res);
if (!write_normals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
// Strip uninitialized normals from cloud:
pcl::copyPointCloud (*voxel_cloud, *cloud_xyz);
savePCDFileASCII (argv[pcd_file_indices[0]], *cloud_xyz);
}
else
{
savePCDFileASCII (argv[pcd_file_indices[0]], *voxel_cloud);
}
}