int
main (int argc, char *argv[])
{
std::string pcd_file;
if (argc > 1)
{
pcd_file = argv[1];
}
else
{
printf ("\nUsage: pcl_example_nurbs_fitting_curve pcd-file \n\n");
printf (" pcd-file point-cloud file\n");
exit (0);
}
// #################### LOAD FILE #########################
printf (" loading %s\n", pcd_file.c_str ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud2;
if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
throw std::runtime_error (" PCD file not found.");
fromPCLPointCloud2 (cloud2, *cloud);
// convert to NURBS data structure
pcl::on_nurbs::NurbsDataCurve2d data;
PointCloud2Vector2d (cloud, data.interior);
viewer.setSize (800, 600);
viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");
// #################### CURVE PARAMETERS #########################
unsigned order (3);
unsigned n_control_points (10);
pcl::on_nurbs::FittingCurve2d::Parameter curve_params;
curve_params.smoothness = 0.000001;
curve_params.rScale = 1.0;
// #################### CURVE FITTING #########################
ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2d::initNurbsPCA (order, &data, n_control_points);
pcl::on_nurbs::FittingCurve2d fit (&data, curve);
fit.assemble (curve_params);
Eigen::Vector2d fix1 (0.1, 0.1);
Eigen::Vector2d fix2 (1.0, 0.0);
// fit.addControlPointConstraint (0, fix1, 100.0);
// fit.addControlPointConstraint (curve.CVCount () - 1, fix2, 100.0);
fit.solve ();
// visualize
VisualizeCurve (fit.m_nurbs, 1.0, 0.0, 0.0, true);
viewer.spin ();
return 0;
}
int main(int argc, char *argv[]) {
if (argc < 2) {
printf("%s << ERROR! No input PLY (mesh/cloud) file has been provided.\n", __FUNCTION__);
return 1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PolygonMesh::Ptr mesh;
mesh = pcl::PolygonMesh::Ptr(new pcl::PolygonMesh());
char input_ply_name[256];
sprintf(input_ply_name, "%s", argv[1]);
if (pcl::io::loadPLYFile(input_ply_name, *mesh) < 0) {
printf("%s << ERROR! Loading of file (%s) failed.\n", __FUNCTION__, input_ply_name);
return 1;
}
fromPCLPointCloud2(mesh->cloud, *cloud);
cv::Mat depth;
int cols = 640, rows = 480;
depth = cv::Mat::zeros(rows, cols, CV_16UC1);
double fx = 570.6, fy = 558.8;
double cx = 320.7, cy = 243.0;
int iii, jjj;
for (int xxx = 0; xxx < cloud->points.size(); xxx++) {
iii = float2int((cloud->points[xxx].x*fx)/float(-cloud->points[xxx].z) + cx);
jjj = float2int((-cloud->points[xxx].y*fy)/float(-cloud->points[xxx].z) + cy);
if ((iii < cols) && (iii >= 0) && (jjj < rows) && (jjj >= 0)) {
int depth_val = -cloud->points[xxx].z*DEFAULT_LEVELS_PER_MM;
if ((depth.at<unsigned short>(jjj,iii) == 0) || (depth_val < depth.at<unsigned short>(jjj,iii))) { // Only fill pixel if current point lies closer to the camera than previous
depth.at<unsigned short>(jjj,iii) = depth_val; // is this correct??
}
} else {
printf("%s << Dodgy point: (%d, %d) from (%f, %f, %f)\n", __FUNCTION__, iii, jjj, cloud->points[xxx].x, cloud->points[xxx].y, cloud->points[xxx].z);
}
}
char output_png_name[256];
sprintf(output_png_name, "%s_depth.png", input_ply_name);
cv::imwrite(output_png_name, depth);
return 0;
}
template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::runMarchingCubes ()
{
//Preparing the pointers and variables
const TsdfVolume::Ptr tsdf_volume_const_ = tsdf_volume_gpu_;
pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_buffer_device_;
//Creating Marching cubes instance
MarchingCubes::Ptr marching_cubes_ = MarchingCubes::Ptr ( new MarchingCubes() );
//Running marching cubes
pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_device = marching_cubes_->run (*tsdf_volume_const_, triangles_buffer_device_);
//Creating mesh
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
if(mesh_ptr_ != nullptr)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( mesh_ptr_->cloud, *cloud_tmp_ptr);
}
return (mesh_ptr_);
}
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;
}
//template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
{
std::vector< MeshPtr > meshes_vector;
int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
float cell_size = volume_size_ / voxels_x_;
int mesh_counter = 0;
for(int i = 0; i < max_iterations; ++i)
{
PCL_INFO ("Processing cube number %d\n", i);
//Making cloud local
Eigen::Affine3f cloud_transform;
float originX = (tsdf_offsets[i]).x();
float originY = (tsdf_offsets[i]).y();
float originZ = (tsdf_offsets[i]).z();
cloud_transform.linear ().setIdentity ();
cloud_transform.translation ()[0] = -originX;
cloud_transform.translation ()[1] = -originY;
cloud_transform.translation ()[2] = -originZ;
transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
//Get mesh
MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
if(tmp != nullptr)
{
meshes_vector.push_back (tmp);
mesh_counter++;
}
else
{
PCL_INFO ("This cloud returned no faces, we skip it!\n");
continue;
}
//Making cloud global
cloud_transform.translation ()[0] = originX * cell_size;
cloud_transform.translation ()[1] = originY * cell_size;
cloud_transform.translation ()[2] = originZ * cell_size;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
std::stringstream name;
name << "mesh_" << mesh_counter << ".ply";
PCL_INFO ("Saving mesh...%d \n", mesh_counter);
pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
}
return;
}
bool
pcl::ProjectInliers<pcl::PCLPointCloud2>::initSACModel (int model_type)
{
// Convert the input data
PointCloud<PointXYZ> cloud;
fromPCLPointCloud2 (*input_, cloud);
PointCloud<PointXYZ>::Ptr cloud_ptr = cloud.makeShared ();
// Build the model
switch (model_type)
{
case SACMODEL_PLANE:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelPlane<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_LINE:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelLine<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_CIRCLE2D:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_SPHERE:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelSphere<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_PARALLEL_LINE:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelParallelLine<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_PERPENDICULAR_PLANE:
{
//PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<pcl::PointXYZ> (cloud_ptr));
break;
}
case SACMODEL_CYLINDER:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelCylinder<pcl::PointXYZ, Normal> (cloud_ptr));
break;
}
case SACMODEL_NORMAL_PLANE:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelNormalPlane<pcl::PointXYZ, Normal> (cloud_ptr));
break;
}
case SACMODEL_CONE:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelCone<pcl::PointXYZ, Normal> (cloud_ptr));
break;
}
case SACMODEL_NORMAL_SPHERE:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelNormalSphere<pcl::PointXYZ, Normal> (cloud_ptr));
break;
}
case SACMODEL_NORMAL_PARALLEL_PLANE:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, Normal> (cloud_ptr));
break;
}
case SACMODEL_PARALLEL_PLANE:
{
//PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
sacmodel_.reset (new SampleConsensusModelParallelPlane<pcl::PointXYZ> (cloud_ptr));
break;
}
default:
{
PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
return (false);
}
}
return (true);
}
