点云随笔（五）同区域数据圈定

       上图中红色点为检索点，圆圈为检索邻域范围。在检索过程中左边及中间的检索点是道路内部点，而最右侧点则为外部点，并根据该原理进而圈定点云数据。首先对需要判别的点云数据进行KD-Tree构建，然后将该点云数据内的所有点都作为检索点进行遍历，以r=1作为检索半径进行邻域检索，然后采用下述公式进行判别：

         式中Count 为在同位置另一期点云数据邻域应有点数，density为同位置点云数据密度，即通过判断检索点邻域点数是否大于0.5*Count,进而提取内部点云数据。

         该圈定的目的主要是有两期点云数据，用于检索的那期是待处理的点云数据，而同位置另一期点云数据是处理好的点云数据。就比如说我现在裁剪了一部分点云数据并且降采样后用于研究，这部分作为出处理好的，如果我想在原始点云数据中同样裁剪相同区域的点云数据，就可以使用这种方法，而且只要有一期数据，采用这种方法可以实现对多期其他时间的同区域点云进行裁剪。

#include<Windows.h>
#include <iostream>
#include <string>
#include <algorithm>  
#include <string>
#include <cstdlib>
#include <cstdio>
#include <Eigen\Dense>
#include <Eigen/Eigenvalues>
#include <map>
#include <math.h>
#include <vector>
#include <list>
#include <pcl/common/common.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/normal_space.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/features/boundary.h>
#include <pcl/common/geometry.h>
#include <pcl/pcl_macros.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <iomanip>

using Eigen::MatrixXd;
using Eigen::Matrix2d;
using namespace Eigen;
using namespace std;

#define PI acos(-1)

double
Dendity(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
{
    double resolution = 0.0;
    int numberOfPoints = 0;
    int nres;
    std::vector<int> indices(2);
    std::vector<float> squaredDistances(2);
    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
    kdtree.setInputCloud(cloud);

    for (size_t i = 0; i < cloud->size(); ++i)
    {
        nres = kdtree.nearestKSearch(i, 2, indices, squaredDistances);
        if (nres == 2)
        {
            resolution += sqrt(squaredDistances[1]);
            ++numberOfPoints;
        }
    }
    if (numberOfPoints != 0)
        resolution /= numberOfPoints;

    return resolution;
}

int main()
{
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr origin(new pcl::PointCloud<pcl::PointXYZRGB>), road(new pcl::PointCloud<pcl::PointXYZRGB>),
        cloud2save(new pcl::PointCloud<pcl::PointXYZRGB>);

	reader.read("D:\\npdata\\data_enhance\\Data_f.pcd", *road);
   /* reader.read("D:\\npdata\\limestone.pcd", *origin);*/

    double radius{ 1 };
    double dy = Dendity(road);
    int pt = (4 / 3) * dy * PI * pow(radius, 2) / 2;
    pcl::KdTreeFLANN<pcl::PointXYZRGB> tree1;
    tree1.setInputCloud(road);
    pcl::PointXYZRGB searchPoint;
    for (int i{ 0 }; i < origin->size(); i++) 
    {
        searchPoint = origin->at(i);
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;
        if (tree1.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > pt)
        {
            cloud2save->push_back(searchPoint);
        }
    }

    pcl::PCDWriter writer;
	std::stringstream ss;
	ss << "D:\\npdata\\data_enhance\\Data_f.pcd";
	pcl::io::savePCDFileASCII(ss.str(), *cloud2save);

	return 0;
}