가장 근접 거리의 포인트 검색

가장 근접거리의 포인트를 단시간 내에 찾는 알고리즘 중에.. octree, kd-tree 가 제일 유명한듯하다..
은근히 이 알고리즘이 구현된 쓸만한 라이브러리가 없다..
찾아보다 결국 PCL (Point cloud library) 이라는 라이브러리가 제일 유명하더라는..
모든 포인트를 클라우드라는 영역에 집어넣고 특정 포인트에서 쿼리를 때리면.. 가장 근접한 몇개의 포인트를 찾아준다..

보통 octree나 kd-tree는 방향성을 주어 검색을 할 수 없다..
PCL은 다행히 octree 에 한정하여 원점과 방향을 지정하여 근접 포인트 쿼리가 가능하다.

아래는 테스트한 코드..

pcl::PointCloud<pcl::PointXYZL>::Ptr m_point_cloud (new pcl::PointCloud<pcl::PointXYZL> ());
pcl::octree::OctreePointCloudSearch<pcl::PointXYZL> m_octree_search (0.2f);
 
// 포인트와 해당 ID를 집어넣는다.
for(int i=0; i<10000; i++) {
    ...
    m_point_cloud->push_back (pcl::PointXYZL ((float)px, (float)py, (float)pz, id));
}
 
// 원점 지정
Eigen::Vector3f _origin (origin[0],origin[1],origin[2]);
// 방향 지정
Eigen::Vector3f _ray (ray[0],ray[1],ray[2]);
 
// resolution 지정 (반지름 30을 갖는 sphre 정도라고 생각하면 될려나?)
float resolution = 30.f;
 
printf("octree ray intersection with resolution %f\n", resolution);
fflush(stdout);
 
// 쿼리
std::vector<int> indicesInRay;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZL> octree(resolution);
octree.setInputCloud (m_point_cloud);
octree.addPointsFromInputCloud ();
octree.getIntersectedVoxelIndices (_origin, _ray, indicesInRay);
 
// 탐색된 포인트 확인
for(int i=0; i<indicesInRay.size(); i++) {
    int idx = indicesInRay[i];
    pcl::PointXYZL pt = m_point_cloud->points[idx];
    //printf("[%d]: %d: %f, %f, %f\n", idx, pt.label, pt.x, pt.y, pt.z);
}
 
octree.deleteTree();