//
// 對 mesh model 的每一個 face 找出中心點與 normal, return point at the center of face and its normal
//
void mesh_convert_plane(const pcl::PolygonMesh::Ptr &mesh, const pcl::PointCloud<pcl::PointNormal>::Ptr &plane_point)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(mesh->cloud, *mesh_cloud);
plane_point->width = mesh->polygons.size();
plane_point->height = 1;
plane_point->resize(plane_point->width);
for (size_t i = 0; i < plane_point->width; ++i) {
pcl::PointXYZ centroid;
pcl::PointXYZ p1 = mesh_cloud->points[mesh->polygons[i].vertices[0]];
pcl::PointXYZ p2 = mesh_cloud->points[mesh->polygons[i].vertices[1]];
pcl::PointXYZ p3 = mesh_cloud->points[mesh->polygons[i].vertices[2]];
Eigen::Vector3f v1(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z),
v2(p3.x - p1.x, p3.y - p1.y, p3.z - p1.z);
Eigen::Vector3f normal = v1.cross(v2).normalized();
plane_point->points[i].x = (p1.x + p2.x + p3.x) / 3;
plane_point->points[i].y = (p1.y + p2.y + p3.y) / 3;
plane_point->points[i].z = (p1.z + p2.z + p3.z) / 3;
plane_point->points[i].normal_x = normal[0];
plane_point->points[i].normal_y = normal[1];
plane_point->points[i].normal_z = normal[2];
plane_point->points[i].curvature = 0;
}
}
int main(int argc, char** argv)
{
if(argc <= 2)
{
std::cerr << "You must supply a pcd filename and output filename" << std::endl;
return 1;
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::io::loadPCDFile<pcl::PointNormal>(std::string(argv[1]), *cloud_with_normals);
std::cout << cloud_with_normals->points.size() << " points in " << std::string(argv[1]) << std::endl;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);
tree->setInputCloud(cloud_with_normals);
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
gp3.setSearchRadius (0.025);
gp3.setMu(2.5);
gp3.setMaximumNearestNeighbors(100);
gp3.setMaximumSurfaceAngle(M_PI/4);
gp3.setMinimumAngle(M_PI/18);
gp3.setMaximumAngle(2*M_PI/3);
gp3.setNormalConsistency(false);
gp3.setInputCloud(cloud_with_normals);
gp3.setSearchMethod(tree);
gp3.reconstruct(triangles);
std::cout << triangles.polygons.size() << " polygons in reconstructed mesh" << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(triangles.cloud, *mesh_cloud);
FILE* mesh_file = fopen(argv[2], "w");
if(!mesh_file)
{
std::cerr << "ERROR: Could not open file " << std::string(argv[2]) << std::endl;
return 2;
}
for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = mesh_cloud->points.begin();
point != mesh_cloud->points.end();
point++)
{
fprintf(mesh_file, "v %f %f %f\n",
point->x,
point->y,
point->z);
}
fprintf(mesh_file, "\n");
for(std::vector<pcl::Vertices>::const_iterator polygon = triangles.polygons.begin();
polygon != triangles.polygons.end();
polygon++)
{
fprintf(mesh_file, "f %d %d %d\n",
polygon->vertices[0]+1,
polygon->vertices[1]+1,
polygon->vertices[2]+1);
}
fclose(mesh_file);
}
template<typename PointInT> void
pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
{
if (mesh.tex_polygons.size () != 1)
return;
pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
std::vector<pcl::Vertices> faces;
for (int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
{
PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ());
// transform mesh into camera's frame
pcl::PointCloud<pcl::PointXYZ>::Ptr camera_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ());
// CREATE UV MAP FOR CURRENT FACES
pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>);
std::vector<pcl::Vertices>::iterator current_face;
std::vector<bool> visibility;
visibility.resize (mesh.tex_polygons[current_cam].size ());
std::vector<UvIndex> indexes_uv_to_points;
// for each current face
//TODO change this
pcl::PointXY nan_point;
nan_point.x = std::numeric_limits<float>::quiet_NaN ();
nan_point.y = std::numeric_limits<float>::quiet_NaN ();
UvIndex u_null;
u_null.idx_cloud = -1;
u_null.idx_face = -1;
int cpt_invisible=0;
for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face)
{
//project each vertice, if one is out of view, stop
pcl::PointXY uv_coord1;
pcl::PointXY uv_coord2;
pcl::PointXY uv_coord3;
if (isFaceProjected (cameras[current_cam],
camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[0]],
camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[1]],
camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[2]],
uv_coord1,
uv_coord2,
uv_coord3))
{
// face is in the camera's FOV
// add UV coordinates
projections->points.push_back (uv_coord1);
projections->points.push_back (uv_coord2);
projections->points.push_back (uv_coord3);
// remember corresponding face
UvIndex u1, u2, u3;
u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0];
u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1];
u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2];
u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face;
indexes_uv_to_points.push_back (u1);
indexes_uv_to_points.push_back (u2);
indexes_uv_to_points.push_back (u3);
//keep track of visibility
visibility[idx_face] = true;
}
else
{
projections->points.push_back (nan_point);
projections->points.push_back (nan_point);
projections->points.push_back (nan_point);
indexes_uv_to_points.push_back (u_null);
indexes_uv_to_points.push_back (u_null);
indexes_uv_to_points.push_back (u_null);
//keep track of visibility
visibility[idx_face] = false;
cpt_invisible++;
}
}
// projections contains all UV points of the current faces
// indexes_uv_to_points links a uv point to its point in the camera cloud
// visibility contains tells if a face was in the camera FOV (false = skip)
// TODO handle case were no face could be projected
if (visibility.size () - cpt_invisible !=0)
{
//create kdtree
pcl::KdTreeFLANN<pcl::PointXY> kdtree;
kdtree.setInputCloud (projections);
std::vector<int> idxNeighbors;
std::vector<float> neighborsSquaredDistance;
// af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
// then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
cpt_invisible = 0;
for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
{
// project all faces
for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
{
if (idx_pcam == current_cam && !visibility[idx_face])
{
// we are now checking for self occlusions within the current faces
// the current face was already declared as occluded.
// therefore, it cannot occlude another face anymore => we skip it
continue;
}
// project each vertice, if one is out of view, stop
pcl::PointXY uv_coord1;
pcl::PointXY uv_coord2;
pcl::PointXY uv_coord3;
if (isFaceProjected (cameras[current_cam],
camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]],
camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]],
camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]],
uv_coord1,
uv_coord2,
uv_coord3))
{
// face is in the camera's FOV
//get its circumsribed circle
double radius;
pcl::PointXY center;
// getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius);
getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize
// get points inside circ.circle
if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
{
// for each neighbor
for (size_t i = 0; i < idxNeighbors.size (); ++i)
{
if (std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z,
camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z))
< camera_cloud->points[indexes_uv_to_points[idxNeighbors[i]].idx_cloud].z)
{
// neighbor is farther than all the face's points. Check if it falls into the triangle
if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, projections->points[idxNeighbors[i]]))
{
// current neighbor is inside triangle and is closer => the corresponding face
visibility[indexes_uv_to_points[idxNeighbors[i]].idx_face] = false;
cpt_invisible++;
//TODO we could remove the projections of this face from the kd-tree cloud, but I fond it slower, and I need the point to keep ordered to querry UV coordinates later
}
}
}
}
}
}
}
}
// now, visibility is true for each face that belongs to the current camera
// if a face is not visible, we push it into the next one.
if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
{
std::vector<Eigen::Vector2f> dummy_container;
mesh.tex_coordinates.push_back (dummy_container);
}
mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());
std::vector<pcl::Vertices> occluded_faces;
occluded_faces.resize (visibility.size ());
std::vector<pcl::Vertices> visible_faces;
visible_faces.resize (visibility.size ());
int cpt_occluded_faces = 0;
int cpt_visible_faces = 0;
for (size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
{
if (visibility[idx_face])
{
// face is visible by the current camera copy UV coordinates
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = projections->points[idx_face*3].x;
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = projections->points[idx_face*3].y;
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = projections->points[idx_face*3 + 1].x;
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = projections->points[idx_face*3 + 1].y;
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = projections->points[idx_face*3 + 2].x;
mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = projections->points[idx_face*3 + 2].y;
visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face];
cpt_visible_faces++;
}
else
{
// face is occluded copy face into temp vector
occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face];
cpt_occluded_faces++;
}
}
mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3);
occluded_faces.resize (cpt_occluded_faces);
mesh.tex_polygons.push_back (occluded_faces);
visible_faces.resize (cpt_visible_faces);
mesh.tex_polygons[current_cam].clear ();
mesh.tex_polygons[current_cam] = visible_faces;
int nb_faces = 0;
for (int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++)
nb_faces += static_cast<int> (mesh.tex_polygons[i].size ());
}
// we have been through all the cameras.
// if any faces are left, they were not visible by any camera
// we still need to produce uv coordinates for them
if (mesh.tex_coordinates.size() <= cameras.size ())
{
std::vector<Eigen::Vector2f> dummy_container;
mesh.tex_coordinates.push_back(dummy_container);
}
for(size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face)
{
Eigen::Vector2f UV1, UV2, UV3;
UV1(0) = -1.0; UV1(1) = -1.0;
UV2(0) = -1.0; UV2(1) = -1.0;
UV3(0) = -1.0; UV3(1) = -1.0;
mesh.tex_coordinates[cameras.size()].push_back(UV1);
mesh.tex_coordinates[cameras.size()].push_back(UV2);
mesh.tex_coordinates[cameras.size()].push_back(UV3);
}
}
int
main (int argc, char *argv[])
{
std::string pcd_file, file_3dm;
if (argc < 3)
{
printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd<PointXYZ>-in-file 3dm-out-file\n\n");
exit (0);
}
pcd_file = argv[1];
file_3dm = argv[2];
pcl::visualization::PCLVisualizer viewer ("B-spline surface fitting");
viewer.setSize (800, 600);
// ############################################################################
// load point cloud
printf (" loading %s\n", pcd_file.c_str ());
pcl::PointCloud<Point>::Ptr cloud (new pcl::PointCloud<Point>);
pcl::PCLPointCloud2 cloud2;
pcl::on_nurbs::NurbsDataSurface data;
if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
throw std::runtime_error (" PCD file not found.");
fromPCLPointCloud2 (cloud2, *cloud);
PointCloud2Vector3d (cloud, data.interior);
pcl::visualization::PointCloudColorHandlerCustom<Point> handler (cloud, 0, 255, 0);
viewer.addPointCloud<Point> (cloud, handler, "cloud_cylinder");
printf (" %lu points in data set\n", cloud->size ());
// ############################################################################
// fit B-spline surface
// parameters
unsigned order (3);
unsigned refinement (5);
unsigned iterations (10);
unsigned mesh_resolution (256);
pcl::on_nurbs::FittingSurface::Parameter params;
params.interior_smoothness = 0.2;
params.interior_weight = 1.0;
params.boundary_smoothness = 0.2;
params.boundary_weight = 0.0;
// initialize
printf (" surface fitting ...\n");
ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data);
pcl::on_nurbs::FittingSurface fit (&data, nurbs);
// fit.setQuiet (false); // enable/disable debug output
// mesh for visualization
pcl::PolygonMesh mesh;
pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<pcl::Vertices> mesh_vertices;
std::string mesh_id = "mesh_nurbs";
pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution);
viewer.addPolygonMesh (mesh, mesh_id);
// surface refinement
for (unsigned i = 0; i < refinement; i++)
{
fit.refine (0);
fit.refine (1);
fit.assemble (params);
fit.solve ();
pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
viewer.spinOnce ();
}
// surface fitting with final refinement level
for (unsigned i = 0; i < iterations; i++)
{
fit.assemble (params);
fit.solve ();
pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
viewer.spinOnce ();
}
// ############################################################################
// fit B-spline curve
// parameters
pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
curve_params.addCPsAccuracy = 5e-2;
curve_params.addCPsIteration = 3;
curve_params.maxCPs = 200;
curve_params.accuracy = 1e-3;
curve_params.iterations = 100;
curve_params.param.closest_point_resolution = 0;
curve_params.param.closest_point_weight = 1.0;
curve_params.param.closest_point_sigma2 = 0.1;
curve_params.param.interior_sigma2 = 0.00001;
curve_params.param.smooth_concavity = 1.0;
curve_params.param.smoothness = 1.0;
// initialisation (circular)
printf (" curve fitting ...\n");
pcl::on_nurbs::NurbsDataCurve2d curve_data;
curve_data.interior = data.interior_param;
curve_data.interior_weight_function.push_back (true);
ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior);
// curve fitting
pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
// curve_fit.setQuiet (false); // enable/disable debug output
curve_fit.fitting (curve_params);
visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer);
// ############################################################################
// triangulation of trimmed surface
printf (" triangulate trimmed surface ...\n");
viewer.removePolygonMesh (mesh_id);
pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh,
mesh_resolution);
viewer.addPolygonMesh (mesh, mesh_id);
// save trimmed B-spline surface
if ( fit.m_nurbs.IsValid() )
{
ONX_Model model;
ONX_Model_Object& surf = model.m_object_table.AppendNew();
surf.m_object = new ON_NurbsSurface(fit.m_nurbs);
surf.m_bDeleteObject = true;
surf.m_attributes.m_layer_index = 1;
surf.m_attributes.m_name = "surface";
ONX_Model_Object& curv = model.m_object_table.AppendNew();
curv.m_object = new ON_NurbsCurve(curve_fit.m_nurbs);
curv.m_bDeleteObject = true;
curv.m_attributes.m_layer_index = 2;
curv.m_attributes.m_name = "trimming curve";
model.Write(file_3dm.c_str());
printf(" model saved: %s\n", file_3dm.c_str());
}
printf (" ... done.\n");
viewer.spin ();
return 0;
}
int main(int argc, char** argv)
{
if(argc <= 2)
{
std::cerr << "You must supply a pcd filename and an output filename" << std::endl;
return 1;
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::io::loadPCDFile<pcl::PointNormal>(std::string(argv[1]), *cloud_with_normals);
std::cout << cloud_with_normals->points.size() << " points in " << std::string(argv[1]) << std::endl;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);
tree->setInputCloud(cloud_with_normals);
pcl::Poisson<pcl::PointNormal> poisson;
pcl::PolygonMesh triangles;
poisson.setDepth(10);
poisson.setInputCloud(cloud_with_normals);
poisson.setSearchMethod(tree);
poisson.reconstruct(triangles);
std::cout << triangles.polygons.size() << " polygons in reconstructed mesh" << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(triangles.cloud, *mesh_cloud);
FILE* mesh_file = fopen(argv[2], "w");
if(!mesh_file)
{
std::cerr << "ERROR: Could not open file " << std::string(argv[2]) << std::endl;
return 2;
}
for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = mesh_cloud->points.begin();
point != mesh_cloud->points.end();
point++)
{
fprintf(mesh_file, "v %f %f %f\n",
point->x,
point->y,
point->z);
}
fprintf(mesh_file, "\n");
for(std::vector<pcl::Vertices>::const_iterator polygon = triangles.polygons.begin();
polygon != triangles.polygons.end();
polygon++)
{
fprintf(mesh_file, "f %d %d %d\n",
polygon->vertices[0]+1,
polygon->vertices[1]+1,
polygon->vertices[2]+1);
}
fclose(mesh_file);
}
