PCL求到线段距离较近的的点

pcl的八叉数的函数getIntersectedVoxelIndices可以求距离射线较近的点，但是没有求距离线段较近点的功能，也找不到相关资料，突发奇想，其实可以做两次射线取交集就可以了。

int getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0)

这个函数可以获取以origin为起点，direction为方向的射线通过的所有体素，得到了射线经过的点，再设置下范围，就可以得到射线旁的点了

但是射线往往不太好用，一般应用的时候我们都求的是线段旁的点

其实很简单，如下图所示，求线段AB附近的点集的话，就把线段的两个端点分别作为起始点。分别求两个射线上的点集PointAB和PointBA，再对这两个点集取交集POINT，就是线段上的点咯。

代码如下：

    Eigen::Vector3f origin1(A.x, A.y, A.z);//起始点A
    Eigen::Vector3f direction1(B.x-A.x,B.y-A.y,B.z-A.z);//AB方向
    std::vector<int> k_indices1;

    Eigen::Vector3f origin2(B.x,B.y,B.z);//起始点B
    Eigen::Vector3f direction2(A.x-B.x, A.y-B.y, A.z-B.z);//BA方向
    std::vector<int> k_indices2;

    double octree_pix = 0.0135;//八叉树分辨率
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(octree_pix);
    octree.setInputCloud(cloud);//输入点云
    octree.addPointsFromInputCloud();
    octree.getIntersectedVoxelIndices(origin2, direction2, k_indices2,1);
    octree.getIntersectedVoxelIndices(origin1, direction1, k_indices1,1);
    std::vector<int> pointofgreen = vectors_intersection(k_indices1, k_indices2);


//两个vector求交集
std::vector<int> vectors_intersection(std::vector<int> v1, std::vector<int> v2) {
    std::vector<int> v;
    sort(v1.begin(), v1.end());
    sort(v2.begin(), v2.end());
    set_intersection(v1.begin(), v1.end(), v2.begin(), v2.end(), back_inserter(v));//求交集 
    return v;
}

然后就搞定了，很简单。

参考：
PCL通过八叉数获取直线线段附近的点

两个vector求交集