PCL笔记五：PCL可视化

点云视窗类CloudViewer是简单显示点云的可视化工具类，可以让用户用尽可能少的代码查看点云。注意：点云视窗类不能应用于多线程应用程序中。

CloudViewer的后端是通过PCLVisualizer来实现的。但是它在自己的线程中运行。

1、可视化点云图：

#include <pcl/visualization/cloud_viewer.h>   // visualization模块下的CloudViewer头文件。
#include <iostream>   // 标准输入输出头文件声明。              
#include <pcl/io/io.h>  // IO相关头文件声明。
#include <pcl/io/pcd_io.h>  // FCD文件读取头文件声明。

int user_data;
void  // 回调函数：在主函数中注册后只执行一次：对可视化对象背景颜色设置，添加一个圆球几何体。
viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor(1.0, 0.5, 1.0);   // 背景颜色
    pcl::PointXYZ o;   // 存储球心位置。
    o.x = 1.0;
    o.y = 0;
    o.z = 0;
    viewer.addSphere(o, 0.25, "sphere", 0); // 添加圆球集合体。
    std::cout << "i only run once" << std::endl;

}
 // 回调函数，在主函数中注册后，每帧显示都执行一次，函数具体实现在可视化对象中添加一个刷新显示字符串。
void
viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape("text", 0);
    viewer.addText(ss.str(), 200, 300, "text", 0);
    //FIXME: possible race condition here:
    user_data++;
}

int
main()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("maize.pcd", *cloud); // 加载点云文件。
    pcl::visualization::CloudViewer viewer("Cloud Viewer");  // 创建viewer对象。
    // showCloud函数是同步的，在此处等待直到渲染显示为止
    viewer.showCloud(cloud);
    // 该注册函数在可视化时只调用一次
    viewer.runOnVisualizationThreadOnce(viewerOneOff);
    // 该注册函数在渲染输出时每次都调用
    viewer.runOnVisualizationThread(viewerPsycho);
    while (!viewer.wasStopped())
    {
        // 在此处可以添加其他处理
        user_data++;
    }
    return 0;
}

---

---

2、可视化深度图像

两种方法：

• 在3D视窗中以点云形式进行可视化
• 将深度值映射为颜色，以彩色图像方式可视化深度图像。

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
using namespace std;

typedef pcl::PointXYZ PointType;
// 全局参数
float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;
// -----打印帮助-----
void
printUsage(const char* progName)
{
    std::cout << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
        << "Options:\n"
        << "-------------------------------------------\n"
        << "-r <float>   angular resolution in degrees (default " << angular_resolution << ")\n"
        << "-c <int>     coordinate frame (default " << (int)coordinate_frame << ")\n"
        << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
        << "-h           this help\n"
        << "\n\n";
}

void
setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
    Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
    Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
    Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
    viewer.camera_.pos[0] = pos_vector[0];
    viewer.camera_.pos[1] = pos_vector[1];
    viewer.camera_.pos[2] = pos_vector[2];
    viewer.camera_.focal[0] = look_at_vector[0];
    viewer.camera_.focal[1] = look_at_vector[1];
    viewer.camera_.focal[2] = look_at_vector[2];
    viewer.camera_.view[0] = up_vector[0];
    viewer.camera_.view[1] = up_vector[1];
    viewer.camera_.view[2] = up_vector[2];
    viewer.updateCamera();
}

// -----Main-----
int
main(int argc, char** argv)
{
    //解析命令行参数
    //打印help信息
    if (pcl::console::find_argument(argc, argv, "-h") >= 0)
    {
        printUsage(argv[0]);
        return 0;
    }
    // 根据当前视角更新深度图：随时更新2d深度图，相应可视化窗口中当前视角。
    if (pcl::console::find_argument(argc, argv, "-l") >= 0)
    {
        live_update = true;
        std::cout << "Live update is on.\n";
    }

    if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
        std::cout << "Setting angular resolution to " << angular_resolution << "deg.\n";
    int tmp_coordinate_frame;
    if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0)
    {
        coordinate_frame = pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
        std::cout << "Using coordinate frame " << (int)coordinate_frame << ".\n";
    }
    angular_resolution = pcl::deg2rad(angular_resolution);

    // 读取给定的pcd点云文件或者自行创建随机点云
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
    std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument(argc, argv, "pcd");
    if (!pcd_filename_indices.empty())
    {
        std::string filename = argv[pcd_filename_indices[0]];
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1)
        {
            std::cout << "Was not able to open file \"" << filename << "\".\n";
            printUsage(argv[0]);
            return 0;
        }
        scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
            point_cloud.sensor_origin_[1],
            point_cloud.sensor_origin_[2])) *
            Eigen::Affine3f(point_cloud.sensor_orientation_);
    }
    else
    {
        std::cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
        for (float x = -0.5f; x <= 0.5f; x += 0.01f)
        {
            for (float y = -0.5f; y <= 0.5f; y += 0.01f)
            {
                PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
                point_cloud.points.push_back(point);
            }
        }
        point_cloud.width = (int)point_cloud.points.size();  point_cloud.height = 1;
    }
    //从点云创建深度图像对象
    float noise_level = 0.0;
    float min_range = 0.0f;
    int border_size = 1;
    boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);// 深度图智能共享指针
    pcl::RangeImage& range_image = *range_image_ptr;  // 解引用。
    range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);  // 从点云中创建深度图

    //创建3D视图并且添加点云进行显示
    pcl::visualization::PCLVisualizer viewer("3D Viewer");  // 实例化显示对象。
    viewer.setBackgroundColor(1, 1, 1);  // 全白背景。
    // 全黑点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr, 0, 0, 0);
    viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
    //viewer.addCoordinateSystem (1.0f);
    //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
    //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
    viewer.initCameraParameters();  // 相机参数初始化。
    setViewerPose(viewer, range_image.getTransformationToWorldSystem());  // 视角
    //显示深度图像
    pcl::visualization::RangeImageVisualizer range_image_widget("Range image");  // 先打开窗口
    range_image_widget.showRangeImage(range_image);  // 然后显示
    //主循环
    while (!viewer.wasStopped())
    {
        range_image_widget.spinOnce();
        viewer.spinOnce();
        pcl_sleep(0.01);
        if (live_update)
        {
            scene_sensor_pose = viewer.getViewerPose();
            range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
            range_image_widget.showRangeImage(range_image);
        }
    }
}

---

---

3、PCLVisualizer可视化类

PCLVisualizer可视化类是PCL种功能最全的可视化类，与CloudViewer可视化类相比，PCLVisualizer使用起来更为复杂。可以显示法线、绘制多种形状、多个视图窗口。

头文件和帮助信息：

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
// 帮助
void
printUsage(const char* progName)
{
    std::cout << "\n\nUsage: " << progName << " [options]\n\n"
        << "Options:\n"
        << "-------------------------------------------\n"
        << "-h           this help\n"
        << "-s           Simple visualisation example\n"
        << "-r           RGB colour visualisation example\n"
        << "-c           Custom colour visualisation example\n"
        << "-n           Normals visualisation example\n"
        << "-a           Shapes visualisation example\n"
        << "-v           Viewports example\n"
        << "-i           Interaction Customization example\n"
        << "\n\n";
}

---

simpleVis函数实现最基本的点云可视化操作。

// 最基本的点云可视化操作。
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    //创建3D窗口并添加点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);  // 背景。
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");  // 点云入窗。
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");  // 设置点云点大小。
    viewer->addCoordinateSystem(1.0);  // 添加坐标系。
    viewer->initCameraParameters();   // 设置相机参数使用户从默认的角度和方向观察点云。
    return (viewer);
}

• 将视窗对象定义为boost::shared_ptr智能共享指针。该指针全局可用。

以下三个函数：有个参数：唯一的字符串作为ID号。全局区分。

• addPointCloud():调用这个方法，可以添加点云。viewer支持多个点云。
• removePointCloud()：删除。提供需要更新的点云ID号。
• updatePointCloud()：更新点云。提供需要更新的点云ID号。

---

可视化点云颜色特征

boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    //创建3D窗口并添加点云	
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    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");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return (viewer);
}

PointCloudColorHandlerRGB颜色处理对象。

---

可视化点云自定义颜色特征

boost::shared_ptr<pcl::visualization::PCLVisualizer> customColourVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    //创建3D窗口并添加点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
    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);
}

自定义着色处理对象PointCloudColorHandlerCustom。

---

可视化点云法线

boost::shared_ptr<pcl::visualization::PCLVisualizer> normalsVis(
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::ConstPtr normals)
{
    //创建3D窗口并添加点云其包括法线  
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    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");
    viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(cloud, normals, 10, 0.05, "normals");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return (viewer);
}

viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(cloud, normals, 10, 0.05, "normals");

这行代码放在绘制点云的代码后面，可实现对点云法线的显示。

---

绘制普通形状

boost::shared_ptr<pcl::visualization::PCLVisualizer> shapesVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    //创建3D窗口并添加点云    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    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");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    //在点云上添加直线和球体模型：从第一个点到最后一个点的连线
    viewer->addLine<pcl::PointXYZRGB>(cloud->points[0],
        cloud->points[cloud->size() - 1], "line");
    viewer->addSphere(cloud->points[0], 0.2, 0.5, 0.5, 0.0, "sphere");
    //在其他位置添加基于模型参数的平面及圆锥体
    pcl::ModelCoefficients coeffs;
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(0.0);
    coeffs.values.push_back(1.0);
    coeffs.values.push_back(0.0);
    viewer->addPlane(coeffs, "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);
}

---

多视窗显示

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewportsVis(
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::ConstPtr normals1, pcl::PointCloud<pcl::Normal>::ConstPtr normals2)
{
    // 创建3D窗口并添加显示点云其包括法线
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->initCameraParameters();
    int v1(0);
    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);
    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.1", 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);
}

 创建新的视窗口：creatViewPort(double xmin，double ymin, double xmax, double ymax, int &viewportID)

最后两行：添加法线。每个视窗都有一组对应的法线。

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);

---

---

自定义交互

捕捉鼠标和键盘事件。

捕捉键盘事件：

unsigned int text_id = 0;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
    void* viewer_void)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<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)
        {
            sprintf(str, "text#%03d", i);
            viewer->removeShape(str);
        }
        text_id = 0;
    }
}

捕捉鼠标事件：

void mouseEventOccurred(const pcl::visualization::MouseEvent& event,
    void* viewer_void)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
    if (event.getButton() == pcl::visualization::MouseEvent::LeftButton &&
        event.getType() == pcl::visualization::MouseEvent::MouseButtonRelease)
    {
        std::cout << "Left mouse button released at position (" << event.getX() << ", " << event.getY() << ")" << std::endl;

        char str[512];
        sprintf(str, "text#%03d", text_id++);
        viewer->addText("clicked here", event.getX(), event.getY(), str);
    }
}

事件注册订阅

boost::shared_ptr<pcl::visualization::PCLVisualizer> interactionCustomizationVis()
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);

    viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);
    viewer->registerMouseCallback(mouseEventOccurred, (void*)&viewer);

    return (viewer);
}

注册响应键盘和鼠标事件的keyboardEventOccurred和mouseEventOccurred的回调函数。

viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);
viewer->registerMouseCallback(mouseEventOccurred, (void*)&viewer);

viewer对象绑定了这两个事件，当事件发生时，viewer响应。事件函数是上面的两个事件函数。第二个参数（void *）&viewer是回调时传递给回调函数的参数，即把viewer这个视窗对象传递过去。目的是当交互发生时，调用回调函数可以在视窗中根据需求改变视窗状态。

特别注意：因为这个参数是void *类型，所以需要将viewer的类型从boost::shared_ptr强制转换为void *。

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);