static void passThroughFilter(PointCloudPtr cloudPCLInput, PointCloudPtr cloudPCLOutput, string strAxis, double dMinRange, double dMaxRange) { PassThrough<PointT> passThroughFilter; passThroughFilter.setInputCloud(cloudPCLInput); passThroughFilter.setFilterFieldName(strAxis); passThroughFilter.setFilterLimits(dMinRange, dMaxRange); passThroughFilter.filter(*cloudPCLOutput); }
/** * Implements the PassThrough filter. * Gets as arguments - output path, field to filter, from limit and to limit. * Creating the output on the disk and returns filtered point cloud. */ PointCloud<pointType>::Ptr FilterHandler::passThroughFilter(string fieldName, double fromFilterLimit, double toFilterLimit, bool negativeLimits) { PassThrough<pointType> pass; pass.setInputCloud(_cloud); pass.setFilterFieldName(fieldName); pass.setFilterLimits(fromFilterLimit, toFilterLimit); pass.setFilterLimitsNegative(negativeLimits); pass.filter(*_cloud); io::savePCDFile(_output, *_cloud, true); return _cloud; }
// Subsample cloud for faster matching and processing, while filling in normals. void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const { PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced; PointCloud<Normal> normals; PointCloud<PointXYZRGBNormal> cloud_normals; std::vector<int> indices; // Filter out nans. removeNaNFromPointCloud(input, cloud_nan_filtered, indices); indices.clear(); // Filter out everything outside a [200x200x200] box. Eigen::Vector4f min_pt(-100, -100, -100, -100); Eigen::Vector4f max_pt(100, 100, 100, 100); getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices); ExtractIndices<PointXYZRGB> boxfilter; boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered)); boxfilter.setIndices (boost::make_shared<vector<int> > (indices)); boxfilter.filter(cloud_box_filtered); // Reduce pointcloud VoxelGrid<PointXYZRGB> voxelfilter; voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered)); voxelfilter.setLeafSize (0.05, 0.05, 0.05); // voxelfilter.setLeafSize (0.1, 0.1, 0.1); voxelfilter.filter (cloud_voxel_reduced); // Compute normals NormalEstimation<PointXYZRGB, Normal> normalest; normalest.setViewPoint(0, 0, 0); normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ()); //normalest.setKSearch (10); normalest.setRadiusSearch (0.25); // normalest.setRadiusSearch (0.4); normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced)); normalest.compute(normals); pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals); // Filter based on curvature PassThrough<PointXYZRGBNormal> normalfilter; normalfilter.setFilterFieldName("curvature"); // normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals)); normalfilter.filter(output); }
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; }