void reconstructMesh(const PointCloud<PointXYZ>::ConstPtr &cloud, PointCloud<PointXYZ> &output_cloud, std::vector<Vertices> &triangles) { boost::shared_ptr<std::vector<int> > indices(new std::vector<int>); indices->resize(cloud->points.size ()); for (size_t i = 0; i < indices->size (); ++i) { (*indices)[i] = i; } pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>); tree->setInputCloud(cloud); PointCloud<PointXYZ>::Ptr mls_points(new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr mls_normals(new PointCloud<PointNormal>); MovingLeastSquares<PointXYZ, PointNormal> mls; mls.setInputCloud(cloud); mls.setIndices(indices); mls.setPolynomialFit(true); mls.setSearchMethod(tree); mls.setSearchRadius(0.03); mls.process(*mls_normals); ConvexHull<PointXYZ> ch; ch.setInputCloud(mls_points); ch.reconstruct(output_cloud, triangles); }
void HxCPDSpatialGraphWarp::applyNLDeformationToSlice( SpatialGraphSelection& slice, HxSpatialGraph* spatialGraph, const McDArray<McVec3f>& origCoords, const McDArray<McVec3f>& shiftedCoords) { McWatch watch; watch.start(); MovingLeastSquares mls; mls.setAlpha(portAlphaForMLS.getValue()); mls.setLandmarks(origCoords, shiftedCoords); ma::SliceSelector ssh(spatialGraph, "TransformInfo"); SpatialGraphSelection fullSliceSelection; ssh.getSlice(ssh.getSliceAttributeValueFromIndex(1), fullSliceSelection); for (int i = 0; i < fullSliceSelection.getNumSelectedEdges(); i++) { int edge = fullSliceSelection.getSelectedEdge(i); McDArray<McVec3f> newEdgePoints; for (int j = 0; j < spatialGraph->getNumEdgePoints(edge); j++) { McVec3f edgePoint = spatialGraph->getEdgePoint(edge, j); warpPoint(edgePoint, edgePoint, mls); newEdgePoints.append(edgePoint); } spatialGraph->setEdgePoints(edge, newEdgePoints); } for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) { int curVertex = fullSliceSelection.getSelectedVertex(i); McVec3f curCoord = spatialGraph->getVertexCoords(curVertex); McVec3f curCoordWarped; warpPoint(curCoord, curCoordWarped, mls); spatialGraph->setVertexCoords(curVertex, curCoordWarped); // Add new segments that indicate shift. if (slice.isSelectedVertex(curVertex)) { int newVertex = spatialGraph->addVertex(curCoord); McDArray<McVec3f> edgePoints(2); edgePoints[0] = curCoord; edgePoints[1] = curCoordWarped; spatialGraph->addEdge(newVertex, curVertex, edgePoints); } } std::cout << "\n Apply deformation took " << watch.stop() << " seconds."; }
int main (int argc, char **argv) { /* if (argc != 1) { PCL_ERROR ("Syntax: %s input.pcd output.ply\n", argv[0]); return -1; }*/ PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ()); io::loadPCDFile (argv[1], *cloud); MovingLeastSquares<PointXYZ, PointXYZ> mls; mls.setInputCloud (cloud); mls.setSearchRadius (4); mls.setPolynomialFit (true); mls.setPolynomialOrder (1); mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointXYZ>::SAMPLE_LOCAL_PLANE); mls.setUpsamplingRadius (1); mls.setUpsamplingStepSize (0.03); PointCloud<PointXYZ>::Ptr cloud_smoothed (new PointCloud<PointXYZ> ()); mls.process (*cloud_smoothed); NormalEstimationOMP<PointXYZ, Normal> ne; ne.setNumberOfThreads (8); ne.setInputCloud (cloud_smoothed); ne.setRadiusSearch (0.8); Eigen::Vector4f centroid; compute3DCentroid (*cloud_smoothed, centroid); ne.setViewPoint (centroid[0], centroid[1], centroid[2]); PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal> ()); ne.compute (*cloud_normals); for (size_t i = 0; i < cloud_normals->size (); ++i) { cloud_normals->points[i].normal_x *= -1; cloud_normals->points[i].normal_y *= -1; cloud_normals->points[i].normal_z *= -1; } PointCloud<PointNormal>::Ptr cloud_smoothed_normals (new PointCloud<PointNormal> ()); concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals); Poisson<pcl::PointNormal> poisson; poisson.setDepth (9); poisson.setInputCloud (cloud_smoothed_normals); PolygonMesh mesh_poisson; poisson.reconstruct (mesh_poisson); pcl::io::savePLYFile("mesh.ply", mesh_poisson); return 0; }
TEST(MovingLeastSquares, shouldInterpolateSmoothly) { const double rbegin = 0; const double rend = 5; const double repsmarks = 2.0; // Spacing for landmarks. const double shift = 1; // Fixed shift. const double sigma = 0.2; // For random shifts. const double tolerance = 0.01; const Landmarks marks = randomGrid(rbegin, rend, repsmarks, shift, sigma); MovingLeastSquares mls; mls.setLandmarks(marks.src, marks.dst); // Exactly interpolate landmarks. for (int i = 0; i < marks.src.size(); i++) { const McVec2d pos = McVec2d(marks.src[i][0], marks.src[i][1]); const McVec2d tpos = mls.interpolate(pos); const McVec2d tdiff = tpos - McVec2d(marks.dst[i][0], marks.dst[i][1]); EXPECT_FLOAT_EQ(0, tdiff.length()); } const int seed = 38273; McRandom rand(seed); // Interpolate points close to landmarks. for (int i = 0; i < marks.src.size(); i++) { const double d = sigma * (rand.nextNumber() - 0.5); const McVec2d delta(d, d); const McVec2d pos = McVec2d(marks.src[i][0], marks.src[i][1]) + delta; const McVec2d tpos = mls.interpolate(pos); const McVec2d tdiff = tpos - delta - McVec2d(marks.dst[i][0], marks.dst[i][1]); EXPECT_THAT(tdiff.length(), Lt(tolerance)); #if 0 // Useful for debugging. printf("tdiff = %f.\n", tdiff.length()); #endif } }
static void computeNormals(PointCloudPtr pcl_cloud_input, pcl::PointCloud<PointNormal>::Ptr pcl_cloud_normals, const double search_radius) { typedef pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; // Create a KD-Tree KdTreePtr tree = boost::make_shared<pcl::search::KdTree<PointT> >(); MovingLeastSquares<PointT, PointNormal> mls; mls.setComputeNormals(true); mls.setInputCloud(pcl_cloud_input); //mls.setPolynomialFit(true); mls.setSearchMethod(tree); mls.setSearchRadius(search_radius); mls.process(*pcl_cloud_normals); }
void RScloud::smooth_cloud() { //MLS SMOOTHING MovingLeastSquares<pointT, pointT> mls; mls.setInputCloud (pointcloud); mls.setSearchRadius (0.004); mls.setPolynomialFit (true); mls.setPolynomialOrder (2); PointCloud<pointT>::Ptr cloud_smoothed (new PointCloud<pointT> ()); mls.process (*cloud_smoothed); //remove nans for(int i=0;i<cloud_smoothed->points.size();i++){ if (!(pcl::isFinite(cloud_smoothed->at(i)))){ cloud_smoothed->at(i).x = 0.0; cloud_smoothed->at(i).y = 0.0; cloud_smoothed->at(i).z = 0.0; } } pointcloud = cloud_smoothed; }
TEST(MovingLeastSquares, isInvertibleByExchangingLandmarks) { const double rbegin = 0; const double rend = 5; const double repsmarks = 2.0; // Spacing for landmarks. const double repstest = 0.1; // Spacing for test positions. const double shift = 1; // Fixed shift. const double sigma = 0.2; // For random shifts. const double tolerance = 0.03; const Landmarks marks = randomGrid(rbegin, rend, repsmarks, shift, sigma); MovingLeastSquares forward; forward.setLandmarks(marks.src, marks.dst); MovingLeastSquares backward; backward.setLandmarks(marks.dst, marks.src); // Test that forward transform does something and forward-backward // transform gets back to original position. for (double x = rbegin; x < rend; x += repstest) { for (double y = rbegin; y < rend; y += repstest) { const McVec2d pos(x, y); const McVec2d tpos = forward.interpolate(pos); const McVec2d ttpos = backward.interpolate(tpos); const McVec2d tdiff = tpos - pos; const McVec2d ttdiff = ttpos - pos; EXPECT_THAT(tdiff.length(), Gt(tolerance)); EXPECT_THAT(ttdiff.length(), Lt(tolerance)); #if 0 // Useful for debugging. printf("tdiff = %f, ttdiff = %f\n", tdiff.length(), ttdiff.length()); #endif } } }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param, bool use_polynomial_fit, int polynomial_order) { PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()), xyz_cloud (new pcl::PointCloud<PointXYZ> ()); fromROSMsg (*input, *xyz_cloud_pre); // Filter the NaNs from the cloud for (size_t i = 0; i < xyz_cloud_pre->size (); ++i) if (pcl_isfinite (xyz_cloud_pre->points[i].x)) xyz_cloud->push_back (xyz_cloud_pre->points[i]); xyz_cloud->header = xyz_cloud_pre->header; xyz_cloud->height = 1; xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ()); xyz_cloud->is_dense = false; PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ()); MovingLeastSquares<PointXYZ, PointNormal> mls; mls.setInputCloud (xyz_cloud); mls.setSearchRadius (search_radius); if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param); mls.setPolynomialFit (use_polynomial_fit); mls.setPolynomialOrder (polynomial_order); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION); mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE); mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius mls.setUpsamplingRadius (0.025); mls.setUpsamplingStepSize (0.015); mls.setDilationIterations (2); mls.setDilationVoxelSize (0.01f); search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ()); mls.setSearchMethod (tree); mls.setComputeNormals (true); PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n", mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder()); TicToc tt; tt.tic (); mls.process (*xyz_cloud_smoothed); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n"); toROSMsg (*xyz_cloud_smoothed, output); }
void HxMovingLeastSquaresWarp::compute() { if (!portAction.wasHit()) return; HxUniformScalarField3* fromImage = (HxUniformScalarField3*)portFromImage.getSource(); HxLandmarkSet* pointSet = hxconnection_cast<HxLandmarkSet>(portData); if (!fromImage) return; /* It is ok to warp without landmarks, if method is rigid and input has a transformation */ if (!pointSet) { return; } else if (pointSet->getNumSets() < 2) { theMsg->printf( "Error: LandmarkWarp data has to contain at least 2 sets."); return; } HxUniformScalarField3* outputImage; HxUniformVectorField3* outputVectorField = 0; outputImage = createOutputDataSet(); outputVectorField = createOutputVectorDataSet(); float* outputImageData = (float*)outputImage->lattice().dataPtr(); McDArray<McVec2d> landmarks1, landmarks2; prepareLandmarks(landmarks1, landmarks2); MovingLeastSquares mls; mls.setAlpha(portAlpha.getValue()); mls.setLandmarks(landmarks2, landmarks1); const McDim3l& dims = outputImage->lattice().getDims(); McVec3f voxelSizeInOutputImage = outputImage->getVoxelSize(); const McBox3f& bboxOfOutputImage = outputImage->getBoundingBox(); const McBox3f& bboxOfFromImage = fromImage->getBoundingBox(); #ifdef _OPENMP #pragma omp parallel for #endif for (int i = 0; i < dims[0]; i++) { HxLocation3* locationInFromImage = fromImage->createLocation(); std::cout << "\n" << i << " of " << dims[0]; for (int j = 0; j < dims[1]; j++) { McVec2d currentPositionInOutput = McVec2d( bboxOfOutputImage[0] + (float)(i)*voxelSizeInOutputImage.x, bboxOfOutputImage[2] + (float)(j)*voxelSizeInOutputImage.y); McVec2d warpedCoordInFromImage = mls.interpolate(currentPositionInOutput); McVec3f displacement = McVec3f( warpedCoordInFromImage.x - currentPositionInOutput.x, warpedCoordInFromImage.y - currentPositionInOutput.y, 0); displacement = displacement * -1; locationInFromImage->move(McVec3f(warpedCoordInFromImage.x, warpedCoordInFromImage.y, bboxOfFromImage[4])); float resultValue[1]; fromImage->eval(*locationInFromImage, resultValue); unsigned long pos = latticePos(i, j, 0, dims); outputImageData[pos] = resultValue[0]; outputVectorField->lattice().set(i, j, 0, displacement.getValue()); } delete locationInFromImage; } outputImage->touch(); outputImage->fire(); }
void HxCPDSpatialGraphWarp::warpPoint(const McVec3f& source, McVec3f& target, MovingLeastSquares& mlsInterpolator) { McVec2d curPoint = McVec2d(source.x, source.y); McVec2d warpedPoint = mlsInterpolator.interpolate(curPoint); target = McVec3f(warpedPoint.x, warpedPoint.y, source.z); }
int main(int argc, char** argv) { readOptions(argc, argv); PointCloud<PointXYZRGB>::Ptr in(new PointCloud<PointXYZRGB>); PointCloud<Normal>::Ptr n(new PointCloud<Normal>); PointCloud<Normal>::Ptr n_mls(new PointCloud<Normal>); PointCloud<PointXYZRGB>::Ptr p_mls(new PointCloud<PointXYZRGB>); PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>); PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>); //PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>); for (size_t i = 0; i < scenes_.size(); i++) { io::loadPCDFile<PointXYZRGB>(folder_ + "raw_labeled/" + scenes_[i] + ".pcd", *in); PassThrough<PointXYZRGB> pass; pass.setInputCloud(in); pass.setFilterFieldName("z"); pass.setFilterLimits(0.0f, limit_); pass.filter(*in); #ifdef PCL_VERSION_COMPARE //fuerte pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>()); #else //electric pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ()); #endif tree->setInputCloud(in); for (float r_n = r_min_; r_n <= r_max_; r_n += r_step_) { string str_rn = fl2label(r_n,3) + "rn"; NormalEstimationOMP<PointXYZRGB, Normal> norm; norm.setNumberOfThreads(1); norm.setInputCloud(in); norm.setSearchMethod(tree); norm.setRadiusSearch(r_n); norm.compute(*n); #ifdef PCL_MINOR_VERSION #if PCL_MINOR_VERSION >=6 std::cerr << "current implementation of mls doesn't work with pcl1.7" << std::endl; exit(-1); #else MovingLeastSquares<PointXYZRGB, Normal> mls; mls.setInputCloud(in); mls.setOutputNormals(n_mls); mls.setPolynomialFit(true); mls.setPolynomialOrder(2); mls.setSearchMethod(tree); mls.setSearchRadius(r_n); mls.reconstruct(*p_mls); #endif #endif for (size_t p = 0; p < n_mls->points.size(); ++p) { flipNormalTowardsViewpoint(p_mls->points[p], 0.0f, 0.0f, 0.0f, n_mls->points[p].normal[0], n_mls->points[p].normal[1], n_mls->points[p].normal[2]); } io::savePCDFileASCII<PointXYZRGB>(folder_+"normals/"+scenes_[i]+ "/mls_"+scenes_[i]+"_"+str_rn+".pcd",*p_mls); #ifdef PCL_VERSION_COMPARE //fuerte pcl::search::KdTree<PointXYZRGB>::Ptr tree_mls (new pcl::search::KdTree<PointXYZRGB>()); #else //electric pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree_mls (new pcl::KdTreeFLANN<PointXYZRGB> ()); #endif tree_mls->setInputCloud(p_mls); for (float r_f = r_n; r_f <= r_max_; r_f += r_step_) { cout << "scene: " << scenes_[i] << "\tnormals: " << r_n << "\tfeatures: " << r_f << endl; string str_rf = fl2label(r_f,3) + "rf"; FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est; fpfh_est.setNumberOfThreads(1); fpfh_est.setInputCloud(in); fpfh_est.setInputNormals(n); fpfh_est.setSearchMethod(tree); fpfh_est.setRadiusSearch(r_f); fpfh_est.compute(*fpfh); // filename example: // <folder_>/fpfh/kitchen01/fpfh_kitchen01_020rn_025rf.pcd // <folder_>/fpfh/kitchen02/fpfh_kitchen02_030rnmls_060rf.pcd io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] + "/fpfh_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd", *fpfh); FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est_mls; fpfh_est_mls.setNumberOfThreads(1); fpfh_est_mls.setInputCloud(p_mls); fpfh_est_mls.setInputNormals(n_mls); fpfh_est_mls.setSearchMethod(tree_mls); fpfh_est_mls.setRadiusSearch(r_f); fpfh_est_mls.compute(*fpfh); io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] + "/fpfh_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd", *fpfh); PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est; pc_est.setInputCloud(in); pc_est.setInputNormals(n); pc_est.setSearchMethod(tree); pc_est.setRadiusSearch(r_f); pc_est.compute(*pc); io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] + "/pc_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd", *pc); PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est_mls; pc_est_mls.setInputCloud(p_mls); pc_est_mls.setInputNormals(n_mls); pc_est_mls.setSearchMethod(tree_mls); pc_est_mls.setRadiusSearch(r_f); pc_est_mls.compute(*pc); io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] + "/pc_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd", *pc); /*RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est; rsd_est.setInputCloud(in); rsd_est.setInputNormals(n); rsd_est.setSearchMethod(tree); rsd_est.setPlaneRadius(0.5); rsd_est.setRadiusSearch(r_f); rsd_est.compute(*rsd); io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] + "/rsd_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd", *rsd); RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est_mls; rsd_est_mls.setInputCloud(p_mls); rsd_est_mls.setInputNormals(n_mls); rsd_est_mls.setSearchMethod(tree_mls); rsd_est_mls.setPlaneRadius(0.5); rsd_est_mls.setRadiusSearch(r_f); rsd_est_mls.compute(*rsd); io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] + "/rsd_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd", *rsd);*/ } } } return 0; }
void processRSD(const PointCloud<PointXYZRGB>::Ptr in, PointCloud<PointXYZRGB>::Ptr ref_out, PointCloud<PointXYZRGB>::Ptr rsd_out) { PointCloud<Normal>::Ptr n(new PointCloud<Normal>()); PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>()); // passthrough filtering (needed to remove NaNs) cout << "RSD: Pass (with " << in->points.size() << " points)" << endl; PassThrough<PointXYZRGB> pass; pass.setInputCloud(in); pass.setFilterFieldName("z"); pass.setFilterLimits(0.0f, pass_depth_); pass.filter(*ref_out); // optional voxelgrid filtering if (rsd_vox_enable_) { cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl; VoxelGrid<PointXYZRGB> vox; vox.setInputCloud(ref_out); vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_); vox.filter(*ref_out); } #ifdef PCL_VERSION_COMPARE //fuerte pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>()); #else //electric KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ()); #endif tree->setInputCloud(ref_out); // optional surface smoothing if(rsd_mls_enable_) { cout << "RSD: MLS (with " << ref_out->points.size() << " points)" << endl; #ifdef PCL_VERSION_COMPARE std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl; exit(0); #else MovingLeastSquares<PointXYZRGB, Normal> mls; mls.setInputCloud(ref_out); mls.setOutputNormals(n); mls.setPolynomialFit(true); mls.setPolynomialOrder(2); mls.setSearchMethod(tree); mls.setSearchRadius(rsd_rn_); mls.reconstruct(*ref_out); #endif cout << "RSD: flip normals (with " << ref_out->points.size() << " points)" << endl; for (size_t i = 0; i < ref_out->points.size(); ++i) { flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f, n->points[i].normal[0], n->points[i].normal[1], n->points[i].normal[2]); } } else { cout << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl; NormalEstimation<PointXYZRGB, Normal> norm; norm.setInputCloud(ref_out); norm.setSearchMethod(tree); norm.setRadiusSearch(rsd_rn_); norm.compute(*n); } tree->setInputCloud(ref_out); // RSD estimation cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl; RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE; rsdE.setInputCloud(ref_out); rsdE.setInputNormals(n); rsdE.setSearchMethod(tree); rsdE.setPlaneRadius(r_limit_); rsdE.setRadiusSearch(rsd_rf_); rsdE.compute(*rsd); cout << "RSD: classification " << endl; *rsd_out = *ref_out; // apply RSD rules for classification int exp_rgb, pre_rgb; float r_max,r_min; cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_mls_enable_); for (size_t idx = 0; idx < ref_out->points.size(); idx++) { exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label r_max = rsd->points[idx].r_max; r_min = rsd->points[idx].r_min; if ( r_min > r_high ) { pre_rgb = LBL_PLANE; if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++; } else if (r_min < r_low) { if (r_max < r_r2 * r_min) { pre_rgb = LBL_COR; if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++; } else { pre_rgb = LBL_EDGE; if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++; } // special case: combined class for corner and edge if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++; } else { if (r_max < r_r1 * r_min) { pre_rgb = LBL_SPH; if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++; } else { pre_rgb = LBL_CYL; if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++; } // special case: combined class for sphere and cylinder if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++; } switch(exp_rgb) { case LBL_PLANE: if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++; stats.exp[EVAL_PLANE]++; break; case LBL_EDGE: if (pre_rgb != exp_rgb) { stats.fn[EVAL_EDGE]++; if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_EDGE]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_COR: if (pre_rgb != exp_rgb) { stats.fn[EVAL_COR]++; if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_COR]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_SPH: if (pre_rgb != exp_rgb) { stats.fn[EVAL_SPH]++; if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_SPH]++; stats.exp[EVAL_CURVED]++; break; case LBL_CYL: if (pre_rgb != exp_rgb) { stats.fn[EVAL_CYL]++; if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_CYL]++; stats.exp[EVAL_CURVED]++; break; default: stats.undef++; break; } rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb); } cout << "RSD:\n" << stats << endl << endl; }
/*! @brief runs the whole processing pipeline for FPFH features * * @note At the moment the evaluation results will be printed to console. * * @param[in] in the labeled input point cloud * @param[out] ref_out the reference point cloud after the preprocessing steps * @param[out] fpfh_out the labeled point cloud after the classifing process */ void processFPFH(const PointCloud<PointXYZRGB>::Ptr in, PointCloud<PointXYZRGB>::Ptr ref_out, PointCloud<PointXYZRGB>::Ptr fpfh_out) { PointCloud<Normal>::Ptr n(new PointCloud<Normal>()); PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>()); // Passthrough filtering (needs to be done to remove NaNs) cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl; PassThrough<PointXYZRGB> pass; pass.setInputCloud(in); pass.setFilterFieldName("z"); pass.setFilterLimits(0.0f, pass_depth_); pass.filter(*ref_out); // Optional voxelgrid filtering if (fpfh_vox_enable_) { cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl; VoxelGrid<PointXYZRGB> vox; vox.setInputCloud(ref_out); vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_); vox.filter(*ref_out); } #ifdef PCL_VERSION_COMPARE //fuerte pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>()); #else //electric pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ()); #endif //KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>()); tree->setInputCloud(ref_out); // Optional surface smoothing if(fpfh_mls_enable_) { cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl; #ifdef PCL_VERSION_COMPARE std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl; exit(0); #else MovingLeastSquares<PointXYZRGB, Normal> mls; mls.setInputCloud(ref_out); mls.setOutputNormals(n); mls.setPolynomialFit(true); mls.setPolynomialOrder(2); mls.setSearchMethod(tree); mls.setSearchRadius(fpfh_rn_); mls.reconstruct(*ref_out); #endif cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl; for (size_t i = 0; i < ref_out->points.size(); ++i) { flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f, n->points[i].normal[0], n->points[i].normal[1], n->points[i].normal[2]); } } else { cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl; NormalEstimation<PointXYZRGB, Normal> norm; norm.setInputCloud(ref_out); norm.setSearchMethod(tree); norm.setRadiusSearch(fpfh_rn_); norm.compute(*n); } // FPFH estimation #ifdef PCL_VERSION_COMPARE //fuerte tree.reset(new pcl::search::KdTree<PointXYZRGB>()); #else //electric tree.reset(new KdTreeFLANN<PointXYZRGB> ()); #endif tree->setInputCloud(ref_out); cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl; FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE; fpfhE.setInputCloud(ref_out); fpfhE.setInputNormals(n); fpfhE.setSearchMethod(tree); fpfhE.setRadiusSearch(fpfh_rf_); fpfhE.compute(*fpfh); cout << "FPFH: classification " << endl; *fpfh_out = *ref_out; CvSVM svm; svm.load(fpfh_svm_model_.c_str()); cv::Mat fpfh_histo(1, 33, CV_32FC1); int exp_rgb, pre_rgb, predict; cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_); for (size_t idx = 0; idx < ref_out->points.size(); idx++) { exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram)); predict = (int)svm.predict(fpfh_histo); //cout << predict << endl; switch(predict) { case SVM_PLANE: pre_rgb = LBL_PLANE; if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++; break; case SVM_EDGE: pre_rgb = LBL_EDGE; if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++; if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++; break; case SVM_COR: pre_rgb = LBL_COR; if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++; if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++; break; case SVM_SPH: pre_rgb = LBL_SPH; if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++; if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++; break; case SVM_CYL: pre_rgb = LBL_CYL; if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++; if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++; break; default: pre_rgb = LBL_UNDEF; break; } switch(exp_rgb) { case LBL_PLANE: if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++; stats.exp[EVAL_PLANE]++; break; case LBL_EDGE: if (pre_rgb != exp_rgb) { stats.fn[EVAL_EDGE]++; if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_EDGE]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_COR: if (pre_rgb != exp_rgb) { stats.fn[EVAL_COR]++; if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_COR]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_SPH: if (pre_rgb != exp_rgb) { stats.fn[EVAL_SPH]++; if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_SPH]++; stats.exp[EVAL_CURVED]++; break; case LBL_CYL: if (pre_rgb != exp_rgb) { stats.fn[EVAL_CYL]++; if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_CYL]++; stats.exp[EVAL_CURVED]++; break; default: stats.undef++; break; } fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb); } cout << "FPFH:\n" << stats << endl << endl; }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param, bool use_polynomial_fit, int polynomial_order) { PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()), xyz_cloud (new pcl::PointCloud<PointXYZ> ()); fromROSMsg (*input, *xyz_cloud_pre); // Filter the NaNs from the cloud for (size_t i = 0; i < xyz_cloud_pre->size (); ++i) if (pcl_isfinite (xyz_cloud_pre->points[i].x)) xyz_cloud->push_back (xyz_cloud_pre->points[i]); xyz_cloud->header = xyz_cloud_pre->header; xyz_cloud->height = 1; xyz_cloud->width = xyz_cloud->size (); xyz_cloud->is_dense = false; // io::savePCDFile ("test.pcd", *xyz_cloud); PointCloud<PointXYZ>::Ptr xyz_cloud_smoothed (new PointCloud<PointXYZ> ()); MovingLeastSquares<PointXYZ, Normal> mls; mls.setInputCloud (xyz_cloud); mls.setSearchRadius (search_radius); if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param); mls.setPolynomialFit (use_polynomial_fit); mls.setPolynomialOrder (polynomial_order); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::SAMPLE_LOCAL_PLANE); mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::UNIFORM_DENSITY); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::FILL_HOLES); // mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::NONE); mls.setFillingStepSize (0.02); mls.setPointDensity (50000*search_radius); // 300 points in a 5 cm radius mls.setUpsamplingRadius (0.025); mls.setUpsamplingStepSize (0.015); search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ()); mls.setSearchMethod (tree); PointCloud<Normal>::Ptr mls_normals (new PointCloud<Normal> ()); mls.setOutputNormals (mls_normals); PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n", mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder()); TicToc tt; tt.tic (); mls.reconstruct (*xyz_cloud_smoothed); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n"); sensor_msgs::PointCloud2 output_positions, output_normals; // printf ("sizes: %d %d %d\n", xyz_cloud_smoothed->width, xyz_cloud_smoothed->height, xyz_cloud_smoothed->size ()); toROSMsg (*xyz_cloud_smoothed, output_positions); toROSMsg (*mls_normals, output_normals); concatenateFields (output_positions, output_normals, output); PointCloud<PointXYZ> xyz_vg; VoxelGrid<PointXYZ> vg; vg.setInputCloud (xyz_cloud_smoothed); vg.setLeafSize (0.005, 0.005, 0.005); vg.filter (xyz_vg); sensor_msgs::PointCloud2 xyz_vg_2; toROSMsg (xyz_vg, xyz_vg_2); pcl::io::savePCDFile ("cloud_vg.pcd", xyz_vg_2, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); }