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;
}
然后就搞定了,很简单。