OCTREE 로 주변 포인트 탐색하기

PCL 라이브러리의 Octree 를 보고 따라한 것!

먼저 octree 를 만들고 난 후 특정 Search point 에서 가까운 points 들을 뽑아내는,
대표적인 방법에는 voxel search, knn search, radius search 가 있다. 푱푱...

1. 랜덤하게 포인트 클라우드 만들어 주기

void make_pcl_randomcloud()
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);

    // Generate pointcloud data

    cloud->width = 1000;
    cloud->height = 1;
    cloud->points.resize (cloud->width * cloud->height);

    for (std::size_t i = 0; i < cloud->size (); ++i)
    {
    (*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].intensity = 100;
    }
    pcl::io::savePCDFileASCII("저장경로", *cloud);
    cout<<"Save Random Point Cloud is Done!"<<endl;
}

이런식으로 해두고 씀. 근데 사실 push_back 을 쓰긴 해도 된다. 벡터니까.
위와 같이 포인트 클라우드를 만들어주면

요런식으로 뜸. 1024 x 1024 x 1024 의 큐브 안에 랜덤한 포인트들이 존재하는 형태.

2. Search Function 사용하기

Search function 들은 어떤 특정한 인덱스 벡터 내에 원본클라우드를 조회할 수 있는 인덱스 정보를 반환해주는 형태이다. 가령 voxelSearch 함수 에서는 Search point 주변의 포인트들 중 voxel 영역에 포함되는 포인트들에 대한 정보를 포인트의 index로 지칭을 해서 vector 타입의 벡터 안에 반환을 해준다.

i	0	1	2	3
idxVector[i]	64435	64438	64434	64436

약간 위와 같은 느낌으로. 그래서 이 인덱스 voxel 영역에 들어온 포인트들을 실제로 조회하고 싶다면 cloud[idxVector[i]] 이렇게 조회를 해줘야 한다. [] 를 두번씩이나 써야해서 이해가 조금 어려움;;

2.1 Voxel Search 사용하기

void usingVoxelSearchMethod()
{
    float resolution = 128.0f;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
    cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    cloud->reserve(200000000);
    pcl::io::loadPCDFile<pcl::PointXYZI>("원본 PC 저장경로", *cloud);
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZI> octree (resolution);

    octree.setInputCloud (cloud);
    octree.addPointsFromInputCloud(); // 이걸 수행시 원본 PC 에 인덱스 붙음.

    
    // Neighbors within voxel search

    std::vector<int> pointIdxVec;

    if (octree.voxelSearch (searchPoint, pointIdxVec))
    {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl;
                    
        for (std::size_t i = 0; i < pointIdxVec.size (); ++i) 
        {
            std::cout << "    " << (*cloud)[pointIdxVec[i]].x << " " << (*cloud)[pointIdxVec[i]].y << " " << (*cloud)[pointIdxVec[i]].z << std::endl;
            (*cloud)[pointIdxVec[i]].intensity = 2000;
        }
    }

    pcl::io::savePCDFileASCII("저장경로", *cloud);
    cout<<"Save VoxelSearch Point Cloud is Done!"<<endl;

   
}

생각 보다 search point 주변의 128 x 128 x 128 voxel 내에서 포인트가 많이 없었던 모양이다.

2.2 knn Search 사용하기

knn 에서 추출할 neighbor 수로 설정한 파라미터 만큼 딱 내 위치 주변의 가장 가까운 점들을 뽑아준다. 여기서는 10개로 설정해보았다.

void usingKnnMethod()
{
    
    float resolution = 128.0f;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
    cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    cloud->reserve(200000000);
    pcl::io::loadPCDFile<pcl::PointXYZI>("/pc경로", *cloud);
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZI> octree (resolution);

    octree.setInputCloud (cloud);
    octree.addPointsFromInputCloud();

    // K nearest neighbor search

    int K = 10;

    std::vector<int> pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;

    std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl;

    if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
    for (std::size_t i = 0; i < pointIdxNKNSearch.size (); ++i) {
        std::cout << "    "  <<   (*cloud)[ pointIdxNKNSearch[i] ].x 
                << " " << (*cloud)[ pointIdxNKNSearch[i] ].y 
                << " " << (*cloud)[ pointIdxNKNSearch[i] ].z 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        (*cloud)[ pointIdxNKNSearch[i] ].intensity = 2000;
    }
    }

    pcl::io::savePCDFileASCII("저장경로", *cloud);
    cout<<"Save knnSearch Point Cloud is Done!"<<endl;


}

정직하게 10개가 찾아지는 것을 확인할 수 있다..^^
하나 주의깊게 볼 점은, 이때는 원본 point 들 중 knn cluster 된 것들의 인덱스 정보를 vector<int> 에 담아줄 뿐만 아니라, search point 와의 거리를 vector<float> 에도 담아준다는 점이다.

그래서 인덱스 담아줄 std::vector<int> pointIdxNKNSearch; 랑 거리 정보 담아줄 std::vector<float> pointNKNSquaredDistance; 이렇게 두개 선언해줬다.

2.3 Radius Search

knn과 유사하게 탐색할 반지름을 설정해주면 된다. 얘도 마찬가지로 인덱스 정보와, search point 까지와의 거리를 거리로 반환해준다.
그래서 std::vector<int> pointIdxRadiusSearch; 랑 std::vector<float> pointRadiusSquaredDistance; 선언 해준다.

void usingRadiusSearchMethod()
{
    float resolution = 128.0f;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
    cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    cloud->reserve(200000000);
    pcl::io::loadPCDFile<pcl::PointXYZI>("원본 pc", *cloud);
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZI> octree (resolution);

    octree.setInputCloud (cloud);
    octree.addPointsFromInputCloud();

    
    // Neighbors within radius search

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

    std::cout << "Neighbors within radius search at (" << searchPoint.x 
        << " " << searchPoint.y 
        << " " << searchPoint.z
        << ") with radius=" << radius << std::endl;


    if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
    for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
        std::cout << "    "  <<   (*cloud)[ pointIdxRadiusSearch[i] ].x << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    (*cloud)[ pointIdxRadiusSearch[i] ].intensity = 2000;
    }
    }
    pcl::io::savePCDFileASCII("저장경로", *cloud);
    cout<<"Save RadiusSearch Point Cloud is Done!"<<endl;
    
}

마니 잘 찾는다.