void CreateGridMesh( const VoxelGrid<Discretizer>& vg, std::vector<Vector3>& vertices) { std::vector<Vector3> voxel_mesh; CreateBoxMesh(vg.res().x(), vg.res().y(), vg.res().z(), voxel_mesh); vertices.reserve(voxel_mesh.size() * vg.sizeX() * vg.sizeY() * vg.sizeZ()); for (int x = 0; x < vg.sizeX(); ++x) { for (int y = 0; y < vg.sizeY(); ++y) { for (int z = 0; z < vg.sizeZ(); ++z) { const MemoryCoord mc(x, y, z); const WorldCoord wc(vg.memoryToWorld(mc)); Vector3 wp(wc.x, wc.y, wc.z); std::printf("%d, %d, %d -> %0.3f, %0.3f, %0.3f\n", x, y, z, wc.x, wc.y, wc.z); // translate all triangles by the voxel position for (const Vector3& v : voxel_mesh) { vertices.push_back(v + wp); } } } } }
static void downsampling(PointCloudPtr cloudPCLInput, PointCloudPtr cloudPCLOutput, double dLeafSize) { VoxelGrid<PointT> downsampler; downsampler.setInputCloud(cloudPCLInput); downsampler.setLeafSize(dLeafSize, dLeafSize, dLeafSize); downsampler.filter(*cloudPCLOutput); }
/** * Implements the Voxel Grid Filter. * Gets the leafs size as arguments (floating point). * Returns a pointer to the filtered cloud. */ PointCloud<pointType>::Ptr FilterHandler::voxelGridFilter(float xLeafSize, float yLeafSize, float zLeafSize) { VoxelGrid<pointType> sor; sor.setInputCloud(_cloud); sor.setLeafSize(xLeafSize, yLeafSize, zLeafSize); sor.filter(*_cloud); io::savePCDFile(_output, *_cloud, true); return _cloud; }
/** * Reducing the number of elements in a point cloud using a * voxel grid with configured leaf size. * The main goal is to increase processing speed. */ void scaleCloud( TheiaCloudPtr in, double inLeafSize, TheiaCloudPtr out ){ VoxelGrid<TheiaPoint> grid; grid.setLeafSize(inLeafSize, inLeafSize, inLeafSize); grid.setInputCloud(in); grid.filter(*out); }
// 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); }
OccupancyGrid::OccupancyGrid( const VoxelGrid& voxel_grid, const size_t density_channel_index, const float occupancy_threshold) : m_grid( voxel_grid.get_xres(), voxel_grid.get_yres(), voxel_grid.get_zres(), 1) { initialize( voxel_grid, density_channel_index, occupancy_threshold); }
PointCloud<PointXYZI>::Ptr PointCloudFunctions::downSampleCloud(pcl::PointCloud<PointXYZI>::Ptr inputCloud, float leafSize, bool save, string fileNameToSave) { PointCloud<PointXYZI>::Ptr downsampled(new PointCloud<PointXYZI> ()); VoxelGrid<PointXYZI> sor; sor.setInputCloud (inputCloud); sor.setFilterLimits(0, 2000); sor.setLeafSize (leafSize, leafSize, leafSize); sor.filter (*downsampled); if (save) { savePCDFileASCII (fileNameToSave, *downsampled); } return downsampled; }
void write_voxel_grid( const char* filename, const VoxelGrid& grid) { FILE* file = fopen(filename, "wt"); if (file == 0) return; const size_t xres = grid.get_xres(); const size_t yres = grid.get_yres(); const size_t zres = grid.get_zres(); const size_t channel_count = grid.get_channel_count(); for (size_t z = 0; z < zres; ++z) { fprintf(file, "z " FMT_SIZE_T "\n\n", z); for (size_t y = 0; y < yres; ++y) { for (size_t x = 0; x < xres; ++x) { if (x > 0) fprintf(file, " "); const float* voxel = grid.voxel(x, y, z); for (size_t i = 0; i < channel_count; ++i) { if (i > 0) fprintf(file, ","); fprintf(file, "%f", voxel[i]); } } fprintf(file, "\n"); } fprintf(file, "\n"); } fclose(file); }
void callback( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info) { Time begin = Time::now(); // Debug info cerr << "Recieved frame..." << endl; cerr << "Cam info: fx:" << cam_info->K[0] << " fy:" << cam_info->K[4] << " cx:" << cam_info->K[2] <<" cy:" << cam_info->K[5] << endl; cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl; //get image from message cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(dep); Mat depth = cv_image->image; Normals normal(depth, cam_info); PointCloud<pcl::PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>); for (int i = 0; i < normal.m_points.rows; ++i) for (int j = 0; j < normal.m_points.cols; ++j) { Vec3f vector = normal.m_points.at<Vec3f>(i, j); //pcl::Vec cloud->push_back(pcl::PointXYZ(vector[0], vector[1], vector[2])); } VoxelGrid<PointXYZ> voxelgrid; voxelgrid.setInputCloud(cloud); voxelgrid.setLeafSize(0.05, 0.05, 0.05); voxelgrid.filter(*cloud); cloud->header.frame_id = OUTPUT_POINT_CLOUD_FRAMEID; stringstream name; name << "model_" << modelNo << ".pcd"; io::savePCDFile(name.str(), *cloud); ++modelNo; pub.publish(cloud); Time end = ros::Time::now(); cerr << "Computation time: " << (end-begin).nsec/1000000.0 << " ms." << endl; cerr << "=========================================================" << endl; }
bool VoxelmapTest::runTest() { std::vector<bool> testResults; { { // Basic Test, simple grid. std::vector<float> boxVert; std::vector<size_t> boxInd; makeBoxVertexIndices(Vector3(1.2f), Vector3(0.0f), boxVert, boxInd); float voxelWidth = 1.0f; // With box size 1.2f, we should have center voxel empty, but immediately surrounded voxels full. { VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth); int numVoxels = resultGrid->numVoxels(); bool hasCorrectNumEntries = numVoxels == 26; //27 - 1 [This test is set up so that the central box is empty] assert(hasCorrectNumEntries); testResults.push_back(hasCorrectNumEntries); } { // Add another box around 4.0, 4.0, 4.0 makeBoxVertexIndices(Vector3(1.2f), Vector3(4.0f), boxVert, boxInd); VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth); int numVoxels = resultGrid->numVoxels(); bool hasCorrectNumEntries = numVoxels == 52; //27 - 1 [This test is set up so that the central box is empty] assert(hasCorrectNumEntries); testResults.push_back(hasCorrectNumEntries); } } { // Finer grid test // Basic Test, simple grid. std::vector<float> boxVert; std::vector<size_t> boxInd; makeBoxVertexIndices(Vector3(1.2f), Vector3(0.0f), boxVert, boxInd); float voxelWidth = 0.2f; // With box size 1.2f, we should have center voxel empty, but immediately surrounded voxels full. { VoxelGrid* resultGrid = VoxelGridFactory::generateVoxelGridFromMesh((const float*)&boxVert[0], boxVert.size() / 3, &boxInd[0], boxInd.size() / 3, voxelWidth); int numVoxels = resultGrid->numVoxels(); bool hasCorrectNumEntries = numVoxels == 7 * 7 * 7 - 5 * 5 * 5; assert(hasCorrectNumEntries); testResults.push_back(hasCorrectNumEntries); } } } for (auto testResult : testResults) { if (!testResult) { return false; } } return true; }
float OccupancyGrid::get_density_sum( const VoxelGrid& voxel_grid, const size_t density_channel_index, const size_t x, const size_t y, const size_t z) const { float density_sum = 0.0f; for (int dx = -1; dx <= +1; ++dx) { for (int dy = -1; dy <= +1; ++dy) { for (int dz = -1; dz <= +1; ++dz) { const int ix = static_cast<int>(x) + dx; const int iy = static_cast<int>(y) + dy; const int iz = static_cast<int>(z) + dz; if (ix < 0 || iy < 0 || iz < 0 || ix >= static_cast<int>(voxel_grid.get_xres()) || iy >= static_cast<int>(voxel_grid.get_yres()) || iz >= static_cast<int>(voxel_grid.get_zres())) continue; const float* voxel = voxel_grid.voxel(ix, iy, iz); assert(voxel[density_channel_index] >= 0.0f); density_sum += voxel[density_channel_index]; } } } return density_sum; }
PartitionCellCount:: PartitionCellCount(const VoxelGrid & grid, Rect3i halfCellBounds, int runlineDirection ) : mGridDescription(grid.gridDescription()), mNumCells(8), mHalfCellBounds(halfCellBounds), m_nnx(halfCellBounds.size(0)+1), m_nny(halfCellBounds.size(1)+1), m_nnz(halfCellBounds.size(2)+1) { long long allocSize = m_nnx*m_nny*m_nnz; if (mMaterialIndexHalfCells.max_size() < allocSize) { cerr << "Warning: PartitionCellCount is going to attempt to allocate a " << m_nnx << "x" << m_nny << "x" << m_nnz << " cell array with " "std::vector; the total size is " << allocSize << " and the vector" " maximum size is " << mMaterialIndexHalfCells.max_size() << ", so this will likely fail." << endl; } mMaterialIndexHalfCells.resize(m_nnx*m_nny*m_nnz); calcMaterialIndices(grid, runlineDirection); allocateAuxiliaryDataSpace(grid); }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float leaf_x, float leaf_y, float leaf_z, const std::string &field, double fmin, double fmax) { TicToc tt; tt.tic (); print_highlight ("Computing "); VoxelGrid<sensor_msgs::PointCloud2> grid; grid.setInputCloud (input); grid.setFilterFieldName (field); grid.setFilterLimits (fmin, fmax); grid.setLeafSize (leaf_x, leaf_y, leaf_z); grid.filter (output); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n"); }
int main(int argc, char** argv) { if(argc <= 1 || console::find_argument(argc, argv, "-h") >= 0) { printUsage(argv[0]); } //read file vector<string> paths; if(console::find_argument(argc,argv,"--file")>= 0) { vector<int> indices(pcl::console::parse_file_extension_argument(argc, argv, "pcd")); if (pcl::console::find_argument(argc, argv, "--save") >= 0) { indices.erase(indices.end()-1); } Utilities::getFiles(argv, indices, paths); indices.clear(); indices = pcl::console::parse_file_extension_argument(argc, argv, "ply"); Utilities::getFiles(argv, indices, paths); } // or read a folder if(console::find_argument(argc,argv,"--folder")>= 0) { Utilities::getFiles(argv[pcl::console::find_argument(argc, argv, "--folder") + 1], paths); } vector<PCLPointCloud2> cloud_blob; PointCloud<PointXYZ>::Ptr ptr_cloud (new PointCloud<PointXYZ>); Utilities::read(paths, cloud_blob); Utilities::convert2XYZ(cloud_blob, ptr_cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); float media = 50, devest = 1.0, size; string axis ("z"); string savePath; if(console::find_argument(argc,argv,"--save")>= 0) { savePath = argv[console::find_argument(argc,argv,"--save") + 1]; } Timer timer; Log* ptr_log; Log log(savePath); ptr_log = &log; string configuration("Filter:\n"); /* Statistical Filter */ if(console::find_argument(argc,argv,"-s")>= 0) { if(!isAlpha(argv[console::find_argument(argc,argv,"-s") + 1])) { media = atof(argv[console::find_argument(argc,argv,"-s") + 1]); devest = atof(argv[console::find_argument(argc,argv,"-s") + 2]); } StatisticalOutlierRemoval<PointXYZ> sor; sor.setInputCloud (ptr_cloud); sor.setMeanK (media); sor.setStddevMulThresh (devest); sor.filter (*cloud_filtered); configuration += "Statistical Outlier Removal\n"; configuration += "media: "+ to_string(media) +"\n"; configuration += "Desvest: "+ to_string(devest) +"\n"; configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n"; configuration += "Time to complete: "+ timer.report() +"\n"; cout << configuration << endl; ptr_log->write(configuration); } timer.reset(); /* Voxel Filter */ if(console::find_argument(argc,argv, "-v") >= 0) { if(!isAlpha(argv[console::find_argument(argc,argv,"-v") + 1])) { size = atof(argv[console::find_argument(argc,argv,"-v") + 1]); } // Create the filtering object VoxelGrid<PointXYZ> sor; sor.setInputCloud (ptr_cloud); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered); configuration += "Voxel Grid\n"; configuration += "size of voxel: "+ to_string(size) +"\n"; configuration += "lief size: "+ to_string(0.01) +","+ to_string(0.01) +"," +to_string(0.01)+"\n"; configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n"; configuration += "Time to complete: "+ timer.report() +"\n"; cout << configuration << endl; ptr_log->write(configuration); } timer.reset(); /* PassThroug Filter */ if(console::find_argument(argc,argv, "-p") >= 0) { axis = argv[console::find_argument(argc,argv,"-p") + 1]; // Create the filtering object pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (ptr_cloud); pass.setFilterFieldName (axis); pass.setFilterLimits (0.0, 1.0); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered); configuration += "PassThroug\n"; configuration += "axis: "+ axis +"\n"; configuration += "range: "+ to_string(0.0) +","+ to_string(1.0) + "\n"; configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n"; configuration += "Time to complete: "+ timer.report() +"\n"; cout << configuration << endl; ptr_log->write(configuration); } /* Save */ if(console::find_argument(argc,argv,"--save")>= 0) { int how_many_files = atoi(argv[console::find_argument(argc,argv,"--save") + 2]); Utilities::writePCDFile(ptr_cloud, savePath, how_many_files ); } ptr_log->close(); return 0; }
int main(int argc, char** argv) { if (argc < 2) { cout << "Input a PCD file name...\n"; return 0; } PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>); PCDReader reader; reader.read(argv[1], *cloud); cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n"; PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>); VoxelGrid<PointXYZ> vg; vg.setInputCloud(cloud); vg.setLeafSize(0.01f, 0.01f, 0.01f); vg.filter(*cloud_filtered); cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n"; SACSegmentation<PointXYZ> seg; PointIndices::Ptr inliers(new PointIndices); PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>); ModelCoefficients::Ptr coefficients(new ModelCoefficients); seg.setOptimizeCoefficients(true); seg.setModelType(SACMODEL_PLANE); seg.setMethodType(SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.02); int i=0, nr_points = (int)cloud_filtered->points.size(); while (cloud_filtered->points.size() > 0.3 * nr_points) { seg.setInputCloud(cloud_filtered); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { cout << "Coud not estimate a planar model for the given dataset.\n"; break; } ExtractIndices<PointXYZ> extract; extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_plane); cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n"; extract.setNegative(true); extract.filter(*cloud_f); cloud_filtered->swap(*cloud_f); } search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>); kdtree->setInputCloud(cloud_filtered); vector<PointIndices> cluster_indices; EuclideanClusterExtraction<PointXYZ> ece; ece.setClusterTolerance(0.02); ece.setMinClusterSize(100); ece.setMaxClusterSize(25000); ece.setSearchMethod(kdtree); ece.setInputCloud(cloud_filtered); ece.extract(cluster_indices); PCDWriter writer; int j = 0; for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it) { PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>); for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit) cluster_cloud->points.push_back(cloud_filtered->points[*pit]); cluster_cloud->width = cluster_cloud->points.size(); cluster_cloud->height = 1; cluster_cloud->is_dense = true; cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n"; stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<PointXYZ>(ss.str(), *cluster_cloud, false); j++; } return 0; }
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); }
/*! @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 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; }
void CreateIndexedGridMesh( const VoxelGrid<Discretizer>& vg, std::vector<Vector3>& vertices, std::vector<int>& indices) { // create all the vertices const size_t vertex_count = (vg.sizeX() + 1) * (vg.sizeY() + 1) * (vg.sizeZ() + 1); vertices.reserve(vertex_count); for (int ix = 0; ix < vg.sizeX() + 1; ix++) { for (int iy = 0; iy < vg.sizeY() + 1; iy++) { for (int iz = 0; iz < vg.sizeZ() + 1; iz++) { MemoryCoord mc(ix, iy, iz); WorldCoord wc(vg.memoryToWorld(mc)); Vector3 p = Vector3(wc.x, wc.y, wc.z) - 0.5 * vg.res(); vertices.push_back(p); } } } const size_t voxel_count = vg.sizeX() * vg.sizeY() * vg.sizeZ(); const size_t xplane_count = vg.sizeY() * vg.sizeZ(); const size_t yplane_count = vg.sizeX() * vg.sizeZ(); const size_t zplane_count = vg.sizeX() * vg.sizeY(); indices.reserve(6 * voxel_count * + 2 * xplane_count + 2 * yplane_count + 3 * zplane_count); // for every voxel there are 12 triangles for (int ix = 0; ix < vg.sizeX(); ++ix) { for (int iy = 0; iy < vg.sizeY(); ++iy) { for (int iz = 0; iz < vg.sizeZ(); ++iz) { MemoryCoord a(ix, iy, iz); MemoryCoord b(ix, iy, iz + 1); MemoryCoord c(ix, iy + 1, iz); MemoryCoord d(ix, iy + 1, iz + 1); MemoryCoord e(ix + 1, iy, iz); MemoryCoord f(ix + 1, iy, iz + 1); MemoryCoord g(ix + 1, iy + 1, iz); MemoryCoord h(ix + 1, iy + 1, iz + 1); auto to_index = [&](const MemoryCoord& c) -> size_t { return c.x * (vg.sizeY() + 1) * (vg.sizeZ() + 1) + c.y * (vg.sizeZ() + 1) + c.z; }; // back face indices.push_back(to_index(a)); indices.push_back(to_index(b)); indices.push_back(to_index(d)); indices.push_back(to_index(a)); indices.push_back(to_index(d)); indices.push_back(to_index(c)); // left face indices.push_back(to_index(a)); indices.push_back(to_index(f)); indices.push_back(to_index(b)); indices.push_back(to_index(a)); indices.push_back(to_index(e)); indices.push_back(to_index(f)); // bottom face indices.push_back(to_index(a)); indices.push_back(to_index(g)); indices.push_back(to_index(e)); indices.push_back(to_index(a)); indices.push_back(to_index(c)); indices.push_back(to_index(g)); // front face? if (ix == vg.sizeX() - 1) { indices.push_back(to_index(f)); indices.push_back(to_index(g)); indices.push_back(to_index(e)); indices.push_back(to_index(f)); indices.push_back(to_index(h)); indices.push_back(to_index(g)); } // right face? if (iy == vg.sizeY() - 1) { indices.push_back(to_index(h)); indices.push_back(to_index(d)); indices.push_back(to_index(c)); indices.push_back(to_index(h)); indices.push_back(to_index(c)); indices.push_back(to_index(g)); } // top face? if (iz == vg.sizeZ() - 1) { indices.push_back(to_index(b)); indices.push_back(to_index(d)); indices.push_back(to_index(h)); indices.push_back(to_index(b)); indices.push_back(to_index(h)); indices.push_back(to_index(f)); } } } } }