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

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

//.........这里部分代码省略.........

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

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