C++ VoxelGrid类代码示例

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);
//.........这里部分代码省略.........

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;
}