pcl中octree三种检测方法比较并实现可视化

#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <vector>
#include <ctime>
//pcl::PointCloud<pcl::PointXYZ>::points是vector类型，可以使用push_back添加随机点

const float R = 5; //半径内近邻搜索的半径

int main() {
    std::cout << "Hello, World!" << std::endl;
    srand((unsigned int) time(NULL));
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    //读入点云
    if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd",*pCloud_in) == -1)
    {
        PCL_ERROR("ERROR Load...");
        return -1;
    }
    std::cout<<"load succeed"<<std::endl;

    float resolution = 128.0f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

    octree.setInputCloud(pCloud_in);
    octree.addPointsFromInputCloud();

    //指定搜索中心
    pcl::PointXYZ searchPoint;
    //searchPoint.x = 1024.0f * rand()/(RAND_MAX + 1.0f);
    //searchPoint.y = 1024.0f * rand()/(RAND_MAX + 1.0f);
    //searchPoint.z = 1024.0f * rand()/(RAND_MAX + 1.0f);
    searchPoint.x = 0.0;
    searchPoint.y = 0.0;
    searchPoint.z = 0.0;

    //体素内近邻搜索
    std::vector<int> pointIdxVec;
    if(octree.voxelSearch(searchPoint,pointIdxVec))
    {
        std::cout<<"Neighbors within voxel search ar ("<<searchPoint.x<<", "<<searchPoint.y<<", "<<searchPoint.z<<")"<<std::endl;
        std::cout<<pointIdxVec.size()<<" points found"<<std::endl;
        for(size_t i=0;i<pointIdxVec.size();i++)
        {
            std::cout<<"   "<<pCloud_in->points[pointIdxVec[i]].x<<", "<<pCloud_in->points[pointIdxVec[i]].y<<", "<<pCloud_in->points[pointIdxVec[i]].z<<std::endl;
        }
        std::cout<<pointIdxVec.size()<<" points found"<<std::endl;
    }

    //将体素内近邻搜索结果放到result1
    pcl::PointCloud<pcl::PointXYZ>::Ptr result1(new pcl::PointCloud<pcl::PointXYZ>);
    result1->width = 50000;
    result1->height = 1;
    result1->is_dense = false;
    result1->points.resize(result1->height*result1->width);
    for(size_t i=0;i<pointIdxVec.size();i++)
    {
        result1->points[i].x = pCloud_in->points[pointIdxVec[i]].x;
        result1->points[i].y = pCloud_in->points[pointIdxVec[i]].y;
        result1->points[i].z = pCloud_in->points[pointIdxVec[i]].z;
    }
    //体素内近邻搜索结果保存
    pcl::io::savePCDFileASCII("result1.pcd",*result1);
    std::cout<<pointIdxVec.size()<<" result1.pcd saved "<<std::endl;

    //K近邻搜索
    int K=100000;
    std::vector<int > pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;
    if(octree.nearestKSearch(searchPoint,K,pointIdxNKNSearch, pointNKNSquaredDistance ) > 0)
    {
        std::cout<<"K nearest neighbor search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<") with K = "<<K<<std::endl;
        for(size_t i= 0;i < pointIdxNKNSearch.size();++i)
            std::cout<<"  "<< pCloud_in->points[pointIdxNKNSearch[i]].x<<" "<<pCloud_in->points[pointIdxNKNSearch[i]].y<<" "<<pCloud_in->points[pointIdxNKNSearch[i]].z<<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
    }


    //将k近邻搜索结果保存至result2
    pcl::PointCloud<pcl::PointXYZ>::Ptr result2(new pcl::PointCloud<pcl::PointXYZ>);
    result2->width = 100000;
    result2->height = 1;
    result2->is_dense = false;
    result2->points.resize(result2->height*result2->width);
    for(size_t i =0;i<pointIdxNKNSearch.size();i++)
    {
        result2->points[i].x = pCloud_in->points[pointIdxNKNSearch[i]].x;
        result2->points[i].y = pCloud_in->points[pointIdxNKNSearch[i]].y;
        result2->points[i].z = pCloud_in->points[pointIdxNKNSearch[i]].z;
    }
    //K近邻搜索结果保存保存
    pcl::io::savePCDFileASCII("result2.pcd",*result2);
    std::cout<<pointIdxNKNSearch.size()<<" result2.pcd saved "<<std::endl;

    //半径内近邻搜索
    std::vector<int > pointIdxRadiusSearch;
    std::vector<float > pointRadiusSquaredDistance;
    //float radius = 256.f*rand() / (RAND_MAX + 1.0f);
    float radius = R;
    std::cout<<"Neighbor within radius search at ( "<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<" ) with radius = "<<radius<<endl;
    if(octree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointRadiusSquaredDistance) > 0 )
    {
        std::cout<<pointIdxRadiusSearch.size()<<"points found"<<std::endl;
        for(size_t i=0;i<pointIdxRadiusSearch.size();i++)
        {
            std::cout<<"  "<<"["<<i<<"]  "<<pCloud_in->points[pointIdxRadiusSearch[i]].x<<", "
                     <<pCloud_in->points[pointIdxRadiusSearch[i]].y<<", "
                     <<pCloud_in->points[pointIdxRadiusSearch[i]].z<<std::endl;
            std::cout<<"(squared distance: "<<pointRadiusSquaredDistance[i]<<" )"<<std::endl;
        }
        std::cout<<pointIdxRadiusSearch.size()<<std::endl;
    }

    //将半径内近邻搜索结果放到result3
    pcl::PointCloud<pcl::PointXYZ>::Ptr result3(new pcl::PointCloud<pcl::PointXYZ>);
    result3->width = 500000;
    result3->height = 1;
    result3->is_dense = false;
    result3->points.resize(result3->height*result3->width);
    for(size_t i = 0;i < pointIdxRadiusSearch.size();i++)
    {
        result3->points[i].x = pCloud_in->points[pointIdxRadiusSearch[i]].x;
        result3->points[i].y = pCloud_in->points[pointIdxRadiusSearch[i]].y;
        result3->points[i].z = pCloud_in->points[pointIdxRadiusSearch[i]].z;
    }


    //将半径内近邻搜索结果保存到result3.pcd文件
    pcl::io::savePCDFileASCII("result3.pcd",*result3);

    //输出内容
    std::cout<<"搜索中心： ("<<searchPoint.x<<", "<<searchPoint.y<<", "<<searchPoint.z<<" )"<<std::endl;
    std::cout<<pCloud_in->points.size()<<" scan_room1.pcd(原输入点云)"<<std::endl;
    std::cout<<pointIdxVec.size()<<" result1.pcd saved(体素内近邻搜索) "<<std::endl;
    std::cout<<pointIdxNKNSearch.size()<<" result2.pcd saved(K=100000近邻搜索) "<<std::endl;
    std::cout<<pointIdxRadiusSearch.size()<<" result3.pcd saved(radius="<< R << "半径内近邻搜索)"<<std::endl;


    //声明可视化对象
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->initCameraParameters();


    //输入点云的窗口
    int v(0);
    viewer->createViewPort(0.0,0.0,0.25,1,v);
    viewer->setBackgroundColor(255,0,255,v);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(pCloud_in,0,255,0);
    viewer->addPointCloud<pcl::PointXYZ>(pCloud_in,single_color,"pCloud_in",v);
    viewer->addText("room_scan1",8,8,"room_scan1",v);


    //体素内近邻搜索结果的窗口
    int v1(0);
    viewer->createViewPort(0.25,0.0,0.5,1,v1);
    viewer->setBackgroundColor(0,0,0,v1);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(result1,255,255,255);
    viewer->addPointCloud<pcl::PointXYZ>(result1,single_color1,"result1",v1);
    viewer->addText("Voxel search",8,8,"Voxel search",v1);


    //K近邻搜索结果的窗口
    int v2(0);
    viewer->createViewPort(0.5,0.0,0.75,1,v2);
    viewer->setBackgroundColor(255,0,255,v2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(result2,0,255,0);
    viewer->addPointCloud<pcl::PointXYZ>(result2,single_color2,"result2",v2);
    viewer->addText("K=100000 search",8,8,"K=100000 search",v2);//标签以第一个字符串为准


    int v3(0);
    viewer->createViewPort(0.75,0.0,1,1,v3);
    viewer->setBackgroundColor(0,0,0,v3);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(result3,255,255,255);
    viewer->addPointCloud<pcl::PointXYZ>(result3,single_color3,"result3",v3);
    viewer->addText("radius=5 search",8,8,"radius=5 search",v3);

    viewer->addCoordinateSystem(0.5);//添加坐标系（大小）
    viewer->spin();

    return 0;
}

可视化： 依次为：体素近邻搜索、K近邻搜索（按照八叉树的分类方式最近的K个点）、半径内近邻（指定一个中心点和一个半径）搜索