使用PCLVisualizer进行点云的可视化

#include <iostream>
#include <thread>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

using namespace std;

void printUsage(const char* programName){
    cout<<"\n\nUsage: "<<programName<<" [options]\n\n"
        <<"-------------------------------------------\n"
        <<"-h this help\n"
        <<"-s Simple visualisation example\n"
        <<"-r RGB Color Visualisation example\n"
        <<"-c Custom color visualisation example\n"
        <<"-n Normals visualisation example\n"
        <<"-a Shapes visualisation example\n"
        <<"-v Viewports example\n"
        <<"-i Interaction Customization example\n"
        <<"\n\n";
}
/**************************************************************************************************
 **********************可视化点云：应用PCL Visualizer可视化类显示单个具有XYZ信息的点云**********************
 **************************************************************************************************/
 //该函数实现最基本的点云可视化操作
pcl::visualization::PCLVisualizer::Ptr simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud){
    //创建可视化对象，并将其标题栏名称设置为3D Viewer，将其设置为boost::shared_ptr智能共享指针，保证指针在程序中全局使用，而不引起内存错误
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    //设置窗口的背景颜色，这里设置为黑色(0,0,0)
    viewer->setBackgroundColor(0,0,0);
    /*这是最重要的一行，我们将点云添加到可视化窗口中，并且给他一个唯一的字符串作为ID号，利用此字符串保证在其他成员中也能
     *标志引用该点云，多次调用addPointCloud可以实现多个点云的添加，每调用一次就会创建一个新的ID号，如果想要更新一个已经显示的
     * 点云，则必须先调用removePointCloud(),并提供需要更新的点云号
     */
    viewer->addPointCloud<pcl::PointXYZ>(cloud,"sample cloud");
    //用于改变点云的渲染属性。这里的第二个参数表示点云的缩放程度
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"sample cloud");
    //查看复杂的点云没有方向感，为了保证正确的坐标判断，需需要显示坐标系统方向，可以通过使用x(red),y(blue),z(green)来
    //代表坐标轴的显示方式来解决，圆柱体的大小可以通过scale参数来设置，这里设置为1
    viewer->addCoordinateSystem(1.0);
    //通过设置相机参数使得我们从默认的角度和方向观察点云
    viewer->initCameraParameters();
    return viewer;
}
/***********************可视化点云：应用PCL Visualizer可视化显示单个具有XYZRGB信息的点云**********************
 * 多数情况下点云显示不采用简单的XYZ类型，常用的点云类型是XYZRGB点，包括颜色数据，除此之外，还可以给指定的点云定制颜色
 * 以使得点云在视窗中比较容易区分，点赋予不同的颜色表征其对应的z轴值不同，PCL Visualizer可以根据所存储的颜色数据为点云
 * 赋色。
 * @param cloud
 * @return
 */
 //该函数实现RGB点云的可视化
pcl::visualization::PCLVisualizer::Ptr rgbvis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud){
    //实例化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
    //背景设置
    viewer->setBackgroundColor(0,0,0);
    //创建一个颜色处理对象PointCloudColorHandlerRGBAField，利用这样的对象显示自定义颜色数据，PointCloudColorHandlerRGBAField会得到每个点云的RGB颜色字段
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    //添加点云到窗口，我们可以看到，多了一个RGB的点云类型，用于表示点云的颜色信息，其他与上面一致
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud,rgb,"sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return viewer;
}
/***********************可视化点云：应用PCL Visualizer为点云上单独的一种颜色*********************************
 * 利用这个方法为单独点云上色，以区分其他的点云
 */
 //该函数实现点云的单独上色
pcl::visualization::PCLVisualizer::Ptr customColorVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud){
    //实例化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
    //背景
    viewer->setBackgroundColor(0,0,0);
    //PointCloudColorHandlerCustom创建一种颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud,0,255,0);
    //参数与RGB的可视化相似
    viewer->addPointCloud<pcl::PointXYZ>(cloud,single_color,"sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return viewer;
}
/**********************可视化点云：应用PCL Visualizer可视化点云法线特征*************************************
 * 显示点云是理解点云的一个重要步骤，点云法线特曾是非常重要的基础特征，PCL Visualizer可视化类可用于绘制法线，也可以绘制表征点云的其他特征，
 * 比如主曲率，和几何特征
 * @param cloud
 * @param normals
 * @return
 */
//该函数用于可视化点云法线等特征
pcl::visualization::PCLVisualizer::Ptr normalsVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr  cloud,pcl::PointCloud<pcl::Normal>::ConstPtr normals){
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer);
    viewer->setBackgroundColor(0,0,0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud,rgb,"sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud");
    //将点云法线添加到可视化对象中，cloud为原始点云模型，normal为法向信息，10表示需要显示法向的点云间隔，即每10个点显示一次法向，0.05表示法向长度。
    viewer->addPointCloudNormals<pcl::PointXYZRGB,pcl::Normal>(cloud,normals,10,0.05,"normals");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return viewer;
}
/************************可视化点云:应用PCL Visualizer绘制普通形状************************
 *PCL Visualizer可视化类允许用户在窗口中绘制一般的图形，这个类常用于显示点云处理算法的可视化结果，例如：通过可视化球体包围聚类得到的点云集以显示聚类结果
 * @param cloud
 * @return
 */
 //该函数是掀了添加形状到视图中，一共添加了四种形状：从点云中的一个点到最后一个点之间的连线，原点所在的平面，以点云中第一个点为中心的球体，沿y轴的锥体
pcl::visualization::PCLVisualizer::Ptr shapesVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud){
    //实例化
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer);
    //背景颜色
    viewer->setBackgroundColor(0,0,0);
    //颜色处理对象
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    //添加点云
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud,rgb,"sample cloud");
    //设定大小，这里为3
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud");
    //设定坐标系信息
    viewer->addCoordinateSystem(1.0);
    //初始化相机观察角度和位置
    viewer->initCameraParameters();
    //添加一条线，从cloud的第0个点到最后一个点，将其记为line
    viewer->addLine<pcl::PointXYZRGB>((*cloud)[0],
                                      (*cloud)[cloud->size()-1],"line");
    //添加一个以cloud第0个点为中心，半径为0.2的球体，同时自定义其颜色，将其记为sphere
    viewer->addSphere((*cloud)[0],0.2,0.5,0.5,0.0,"sphere");
    //添加绘制平面使用的标准平面方程：ax+by+cz+d=0来定义平面，这个平面以原点为中心，方向沿着z方向
    //设定参数a=0,b=0,c=1,d=0,则现在的点云平面方程为z=0;这里的values为一个向量，我们不断push的过程就是将参数写入values的过程，就是设定平面参数的过程，
    pcl::ModelCoefficients coeffs;
    coeffs.values.push_back(0.0);//a
    coeffs.values.push_back(0.0);//b
    coeffs.values.push_back(1.0);//c
    coeffs.values.push_back(0.0);//d
    viewer->addPlane(coeffs,"plane");//添加平面进入cloud中，标记为plane
    //添加追星的参数
    coeffs.values.clear();//清除参数
    coeffs.values.push_back(0.3);
    coeffs.values.push_back(0.3);
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(1.0);
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(5.0);
    viewer->addCone(coeffs,"cone");
    return (viewer);
}
/*************************PCL可视化：使用PCL Visualize来多视角显示点云数据*****************************
 * PCL Visualizer可视化类允许用户通过不同的窗口(Viewport)绘制多个点云，这样方便对点云比较
 * @param cloud
 * @param normals1
 * @param normals2
 * @return
 */
 //该函数实现多个视角观察同一个点云数据
pcl::visualization::PCLVisualizer::Ptr viewportVis(
        pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud,
        pcl::PointCloud<pcl::Normal>::ConstPtr normals1,
        pcl::PointCloud<pcl::Normal>::ConstPtr normals2){
    //实例化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
    //初始化相机参数
    viewer->initCameraParameters();
    //视点v1=0
    int v1(0);
    //设定视角，如果窗口内没有渲染器，则将id设为0，从而创建一个渲染器，否则要设为-1；这四个参数分别为x的最小值，最大值，y轴的最小值，最大值，取值为0-1,v1为标识
    viewer->createViewPort(0.0,0.0,0.5,1.0,v1);
    //后面的参数都添加视点进去
    viewer->setBackgroundColor(0,0,0,v1);
    viewer->addText("Radius 0.01",10,10,"v1 text",v1);//添加一个标签区别其他的窗口，利用RGB颜色着色器并添加点云到视口中
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud,rgb,"sample cloud1",v1);
    //对第二视口做同样的操作，使得创建的点云分布于右半窗口，将该视口背景赋值为灰色，即使是同样的点云也能便于明显区别，最后给点云自定义的颜色
    int v2(0);
    viewer->createViewPort(0.5,0.0,1.0,1.0,v2);
    viewer->setBackgroundColor(0.3,0.3,0.3,v2);
    viewer->addText("Radius 0.01",10,10,"v2 text",v2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(cloud,0,255,0);//为单个点云设定颜色
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud,single_color,"sample cloud2",v2);
    //设定两个点云的渲染参数
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud1");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud2");
    //设定相机参数
    viewer->addCoordinateSystem(1.0);
    //添加法线，每个视图都由一组对应的法线
    viewer->addPointCloudNormals<pcl::PointXYZRGB,pcl::Normal>(cloud,normals1,10,0.05,"normals1",v1);
    viewer->addPointCloudNormals<pcl::PointXYZRGB,pcl::Normal>(cloud,normals2,10,0.05,"normals2",v2);
    return viewer;
}
unsigned int text_id=0;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event,void* viewer_void){
    pcl::visualization::PCLVisualizer *viewer=static_cast<pcl::visualization::PCLVisualizer *>(viewer_void);
    if (event.getKeySym()=="r"&& event.keyDown()){
        std::cout <<"r was pressed ==> removing all text"<<std::endl;
        char str[512];
        for (unsigned int i = 0; i < text_id; ++i) {
            cout<<str<<"text#%03d"<<i<<endl;
            viewer->removeShape(str);
        }
        text_id=0;
    }
}
void mouseEventOccurred(const pcl::visualization::MouseEvent &event,
                        void* viewer_void){
    pcl::visualization::PCLVisualizer *viewer =static_cast<pcl::visualization::PCLVisualizer *>(viewer_void);
    if (event.getButton()==pcl::visualization::MouseEvent::LeftButton &&
        event.getType()==pcl::visualization::MouseEvent::MouseButtonRelease){
        cout<<"Left mouse button released at position ("<<event.getX()<<", "<<event.getY()<<")"<<endl;
        char str[512];
        sprintf(str,"text#%03d",text_id++);
        viewer->addText("clicked here",event.getX(),event.getY(),str);
    }
}
pcl::visualization::PCLVisualizer::Ptr interactionCustomizationVis(){
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer);
    viewer->setBackgroundColor(0,0,0);
    viewer->addCoordinateSystem(1.0);
    viewer->registerKeyboardCallback(keyboardEventOccurred,(void *)viewer.get());
    viewer->registerMouseCallback(mouseEventOccurred,(void *)viewer.get());
    return viewer;
}

int main(int argc,char** argv){
    if (pcl::console::find_argument(argc,argv,"-h")>=0){
        printUsage(argv[0]);
        return 0;
    }
    bool simple(false),rgb(false),custom_c(false),normals(false),shapes(false),viewports(false),
        interavtion_customization(false);
    if (pcl::console::find_argument(argc,argv,"-s")>=0){
        simple= true;
        cout<<"Simple visualisation example\n";
    } else if (pcl::console::find_argument(argc,argv,"-c")>=0){
        custom_c= true;
        cout<<"Custom colour visualisation example\n";
    }else if(pcl::console::find_argument(argc,argv,"-r")>=0){
        rgb= true;
        cout<<"RGB color visualization example\n";
    }else if(pcl::console::find_argument(argc,argv,"-n")>=0){
        normals= true;
        cout<<"Normals visualization example\n";
    }else if(pcl::console::find_argument(argc,argv,"-a")>=0){
        shapes= true;
        cout<<"Shapes visualization example\n";
    }else if(pcl::console::find_argument(argc,argv,"-v")>=0){
        viewports= true;
        cout<<"Viewports example\n";
    }else if(pcl::console::find_argument(argc,argv,"-i")>=0){
        interavtion_customization= true;
        cout<<"Interaction Customization example\n";
    } else{
        printUsage(argv[0]);
        return 0;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    cout<<"Generating example point clouds.\n\n";
    uint8_t r(255),g(15),b(15);
    for (float z(-1.0); z <=1.0 ; z+=0.05) {
        for (float angle(0.0); angle <=360.0 ; angle+=5.0) {
            pcl::PointXYZ basic_point;
            basic_point.x = 0.5 * std::cos (pcl::deg2rad(angle));
            basic_point.y = sinf (pcl::deg2rad(angle));
            basic_point.z = z;
            basic_cloud_ptr->points.push_back(basic_point);

            pcl::PointXYZRGB point;
            point.x = basic_point.x;
            point.y = basic_point.y;
            point.z = basic_point.z;
            std::uint32_t rgb=(static_cast<std::uint32_t>(r)<<16 | static_cast<std::uint32_t>(g)<<8 |
                     static_cast<std::uint32_t>(b));
            point.rgb=*reinterpret_cast<float *>(&rgb);
            point_cloud_ptr->points.push_back(point);
        }
        if (z<0.0){
            r-=12;
            g+=12;
        } else{
            g-=12;
            b+=12;
        }
    }
    basic_cloud_ptr->width=basic_cloud_ptr->size();
    basic_cloud_ptr->height=1;
    point_cloud_ptr->width=point_cloud_ptr->size();
    point_cloud_ptr->height=1;

    pcl::NormalEstimation<pcl::PointXYZRGB,pcl::Normal> ne;
    ne.setInputCloud(point_cloud_ptr);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
    ne.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(0.05);
    ne.compute(*cloud_normals1);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(1.0);
    ne.compute(*cloud_normals2);

    pcl::visualization::PCLVisualizer::Ptr viewer;
    if (simple){
        viewer= simpleVis(basic_cloud_ptr);
    } else if(rgb){
        viewer= rgbvis(point_cloud_ptr);
    } else if(custom_c){
        viewer= customColorVis(basic_cloud_ptr);
    } else if(normals){
        viewer= normalsVis(point_cloud_ptr,cloud_normals2);
    } else if(shapes){
        viewer= shapesVis(point_cloud_ptr);
    } else if(viewports){
        viewer= viewportVis(point_cloud_ptr,cloud_normals1,cloud_normals2);
    } else if(interavtion_customization){
        viewer=interactionCustomizationVis();
    }
    while (!viewer->wasStopped()){
        viewer->spinOnce(100);
        this_thread::sleep_for(100ms);
    }
}