如何使用OSG引擎显示PCL的点云数据？

最近开始学习PCL库，出师不利，一开始就踩了个坑。PCL库的visualization不能同时开两个CloudViewer，一开就报错，但是不开两个我怎么比较处理前和处理后的数据呢？不得不放弃使用CloudViewer，自己用OSG写了一个类似的东西，废话多不说，请看代码：

PointCloudViewer.h

#pragma once

namespace osg
{
    class Group;
    class Geometry;
    class Program;
}
namespace osgViewer
{
    class Viewer;
}

class PointCloudViewer
{
public:
    enum ColorMode
    {
        // 按数据的Z值大小显示不同的颜色
        HeightColor = 0,
        // 显示数据自带的颜色
        DataColor = 1,
        // 把数据自带的颜色处理成黑白进行显示
        GrayColor = 2,
        // 按强度着色
        IntensityColor = 3,
    };

    typedef pcl::PointCloud<pcl::PointXYZRGBA>::Ptr DataType;
public:
    struct NodeData 
    {
        osg::ref_ptr<osg::Geometry> geometry;
        osg::ref_ptr<osg::Geode> geode;
        osg::ref_ptr<osg::Program> shader;
        std::map<std::string, osg::ref_ptr<osg::Uniform>> shaderParameters;
    };
public:
    PointCloudViewer();
    virtual ~PointCloudViewer();
public:
    void showCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr data);
    void setColorMode(ColorMode colorMode);
    void setColorMode(const DataType& data, ColorMode colorMode);
    void setDepthCheck(const DataType& data, bool enabled);
    NodeData* getNodeData(const DataType& data);
    bool isDone() const;
    void renderFrame();
private:
    void initOsg();
private:
    std::map<DataType, NodeData> mDatas;
    osg::ref_ptr<osg::Group> mRoot;
    osg::ref_ptr<osgViewer::Viewer> mViewer;
};

PointCloudViewer.cpp

#include "pch.h"
#include "PointCloudViewer.h"
#include "Shaders.h"

PointCloudViewer::PointCloudViewer()
{
    initOsg();
}
PointCloudViewer::~PointCloudViewer()
{

}

void PointCloudViewer::initOsg()
{
    //创建Viewer对象，场景浏览器
    mViewer = new osgViewer::Viewer();
    mViewer->setThreadingModel(osgViewer::Viewer::SingleThreaded);

    //设置图形环境特性
    osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits();
    traits->x = 0;
    traits->y = 0;
    traits->width = 640;
    traits->height = 480;
    traits->windowDecoration = true;
    traits->doubleBuffer = true;
    traits->sharedContext = 0;
    traits->vsync = false;

    //创建图形环境特性
    osg::ref_ptr<osg::GraphicsContext> gc = osg::GraphicsContext::createGraphicsContext(traits.get());
    if (gc.valid())
    {
        osg::notify(osg::INFO) << "  GraphicsWindow has been created successfully." << std::endl;

        //清除窗口颜色及清除颜色和深度缓冲
        gc->setClearColor(osg::Vec4f(0.2f, 0.2f, 0.6f, 1.0f));
        gc->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    }
    else
    {
        osg::notify(osg::NOTICE) << "  GraphicsWindow has not been created successfully." << std::endl;
    }

    //根据分辨率来确定合适的投影来保证显示的图形不变形
    double newAspectRatio = double(traits->width) / double(traits->height);
    mViewer->getCamera()->setProjectionMatrixAsPerspective(60, newAspectRatio, 1.0, 5000000);
    //设置视口
    mViewer->getCamera()->setViewport(new osg::Viewport(0, 0, traits->width, traits->height));
    //设置图形环境
    mViewer->getCamera()->setGraphicsContext(gc.get());
    auto manipulator = new osgGA::TrackballManipulator();
    manipulator->setAllowThrow(false);
    //manipulator->setWheelZoomFactor(-1);
    mViewer->setCameraManipulator(manipulator);
    mViewer->addEventHandler(new osgViewer::StatsHandler());
    //创建场景组节点
    mRoot = new osg::Group();
    mViewer->setSceneData(mRoot.get());
}

void PointCloudViewer::showCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr data)
{
    using namespace osg;
    NodeData nodeData;

    // 创建几何体
    ref_ptr<Geometry> geometry = new Geometry();
    ref_ptr<Vec3Array> vertex = new Vec3Array();
    ref_ptr<Vec4Array> color = new Vec4Array();
    long long count = data->size();
    vertex->resize(count);
    color->resize(count);
    const pcl::PointXYZRGBA* pData = data->data();
    Vec3* pVertex = vertex->asVector().data();
    Vec4* pColor = color->asVector().data();
#pragma omp parallel for
    for (long long i = 0; i < count; i++)
    {
        pVertex[i].set(pData[i].x, pData[i].y, pData[i].z);
        pColor[i].set(pData[i].r / 255.0f, pData[i].g / 255.0f, pData[i].b / 255.0f, pData[i].a / 255.0f);
    }
    geometry->setVertexArray(vertex);
    geometry->setColorArray(color, Array::BIND_PER_VERTEX);
    geometry->addPrimitiveSet(new DrawArrays(PrimitiveSet::POINTS, 0, count));
    // 创建叶子节点
    ref_ptr<Geode> geode = new Geode();
    geode->addDrawable(geometry);
    osg::StateSet * ss = geode->getOrCreateStateSet();
    ss->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    // 启用Shader
    osg::ref_ptr<osg::Program> program = new osg::Program();
    bool vsAdded = program->addShader(new osg::Shader(osg::Shader::VERTEX, vs_PointCloudRender));
    bool psAdded = program->addShader(new osg::Shader(osg::Shader::FRAGMENT, ps_PointCloudRender));
    if (vsAdded && psAdded)
    {
        auto boundBox = geode->getBoundingBox();
        osg::ref_ptr<osg::Uniform> heightMin = new osg::Uniform("heightMin", boundBox.zMin());
        osg::ref_ptr<osg::Uniform> heightMax = new osg::Uniform("heightMax", boundBox.zMax());
        osg::ref_ptr<osg::Uniform> renderType = new osg::Uniform("renderType", DataColor);
        osg::ref_ptr<osg::Uniform> colorMin = new osg::Uniform("colorMin", osg::Vec4(1, 0, 0, 1));
        osg::ref_ptr<osg::Uniform> colorMax = new osg::Uniform("colorMax", osg::Vec4(0, 1, 0, 1));
        ss->addUniform(heightMin);
        ss->addUniform(heightMax);
        ss->addUniform(renderType);
        ss->addUniform(colorMin);
        ss->addUniform(colorMax);
        ss->setAttributeAndModes(program, osg::StateAttribute::ON);
        nodeData.shaderParameters["heightMin"] = heightMin;
        nodeData.shaderParameters["heightMax"] = heightMax;
        nodeData.shaderParameters["renderType"] = renderType;
        nodeData.shaderParameters["colorMin"] = colorMin;
        nodeData.shaderParameters["colorMax"] = colorMax;
    }
    // 加入场景
    mRoot->addChild(geode);
    
    // 设置相机初始位置
    auto bound = mRoot->computeBound();
    auto manipulator = mViewer->getCameraManipulator();
    if (manipulator)
    {
        osgGA::TrackballManipulator* trackballManipulator = 
            dynamic_cast<osgGA::TrackballManipulator*>(manipulator);
        if (trackballManipulator)
        {
            osg::Vec3 eye(bound.radius() + 1, bound.radius() / 2, 0);
            trackballManipulator->setTransformation(eye, bound.center(), osg::Vec3(0, 1, 0));
        }
    }
    
    nodeData.geometry = geometry;
    nodeData.geode = geode;
    nodeData.shader = program;
    mDatas.insert(std::make_pair(data, nodeData));
}

void PointCloudViewer::setColorMode(ColorMode colorMode)
{
    for (auto it = mDatas.begin(); it != mDatas.end(); it++)
    {
        const NodeData& nd = it->second;
        auto itFound = nd.shaderParameters.find("renderType");
        if (itFound != nd.shaderParameters.end())
        {
            itFound->second->set((int)colorMode);
        }
    }
}
void PointCloudViewer::setColorMode(const DataType& data, ColorMode colorMode)
{
    auto itData = mDatas.find(data);
    if (itData != mDatas.end())
    {
        auto itUniform = itData->second.shaderParameters.find("renderType");
        if (itUniform != itData->second.shaderParameters.end())
        {
            itUniform->second->set((int)colorMode);
        }
    }
}

bool PointCloudViewer::isDone() const
{
    if (mViewer)
    {
        return mViewer->done();
    }
    return false;
}

void PointCloudViewer::renderFrame()
{
    if (mViewer)
    {
        mViewer->frame();
    }
}

void PointCloudViewer::setDepthCheck(const DataType& data, bool enabled)
{
    auto itData = mDatas.find(data);
    if (itData != mDatas.end())
    {
        NodeData& data = itData->second;
        data.geode->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
    }
}

PointCloudViewer::NodeData* PointCloudViewer::getNodeData(const DataType& data)
{
    auto itData = mDatas.find(data);
    if (itData != mDatas.end())
    {
        return &itData->second;
    }
    return nullptr;
}

使用方法

#include <iostream>
#include <thread>
#include <chrono>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#define WINGDIAPI
#include <osgDB/ReadFile>
#include <osgViewer/Viewer>
#include "LasLoader.h"
#include "pclColor.h"
#include "PointCloudViewer.h"


int main()
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud = LasLoader::loadLas("420-432_realColor_small_Cloud.las");
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGBA>());

    // Create the filtering object
    pcl::PassThrough<pcl::PointXYZRGBA> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 50.0);
    //pass.setFilterLimitsNegative (true);
    pass.filter(*cloud_filtered);

    long long count = cloud_filtered->size();
    pcl::PointXYZRGBA* data = cloud_filtered->data();
#pragma omp parallel for
    for (long long i = 0; i < count; i++)
    {
        data[i].rgba = fromRgba(255, 255, 0, 255);
    }

    PointCloudViewer viewer;
    viewer.showCloud(cloud);
    PointCloudViewer viewer2;
    viewer2.showCloud(cloud_filtered);
    while (viewer.isDone() == false || viewer2.isDone() == false)
    {
        if (viewer.isDone() == false)
        {
            viewer.renderFrame();
        }
        if (viewer2.isDone() == false)
        {
            viewer2.renderFrame();
        }
    }

    return (0);
}

LasLoader是我自己写的加载LAS数据的类，代码如下：

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/io.h>
#include "LasLoader.h"
#include "liblas/liblas.hpp"
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr LasLoader::loadLas(const char* lasFilePath)
{
    //打开las文件
    std::ifstream ifs;
    ifs.open(lasFilePath, std::ios::in | std::ios::binary);
    if (!ifs.is_open())
    {
        std::cout << "无法打开.las" << std::endl;
        return nullptr;
    }
    liblas::ReaderFactory readerFactory;
    liblas::Reader reader = readerFactory.CreateWithStream(ifs);
    const liblas::Header& lasHeader = reader.GetHeader();
    double minX = lasHeader.GetMinX();
    double minY = lasHeader.GetMinY();
    double minZ = lasHeader.GetMinZ();
    //写点云
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOutput(new pcl::PointCloud<pcl::PointXYZRGBA>());
    cloudOutput->reserve(lasHeader.GetPointRecordsCount());
    pcl::PointXYZRGBA thePt;
    while (reader.ReadNextPoint())
    {
        const liblas::Point& lasPoint = reader.GetPoint();
        const liblas::Color& lasColor = lasPoint.GetColor();
        double x = lasPoint.GetX() - minX;
        double y = lasPoint.GetY() - minY;
        double z = lasPoint.GetZ() - minZ;
        uint16_t red   = lasColor.GetRed();
        uint16_t green = lasColor.GetGreen();
        uint16_t blue  = lasColor.GetBlue();

        /*****颜色说明
        *   uint16_t  范围为0-256*256 ；
        *   pcl 中 R  G  B利用的unsigned char  0-256；
        *   因此这里将uint16_t 除以256  得到  三位数的0-256的值
        *   从而进行rgba 值32位 的位运算。
        *
        ******/
        thePt.x = x;
        thePt.y = y; 
        thePt.z = z;
        if (red == 0 && green == 0 && blue == 0)
        {
            thePt.rgba = 0xFFFFFFFF;
        }
        else
        {
            thePt.rgba = (uint32_t)255 << 24 | (uint32_t)(red / 256) << 16 | (uint32_t)(green / 256) << 8 | (uint32_t)(blue / 256);
        }
        cloudOutput->push_back(thePt);
    }

    return cloudOutput;
}

OsgVisualizer完整代码下载

OsgVisualizer.rar
代码在vcpkg+VS2017+Win7下编译通过，vcpkg需安装OSG、PCL包。

芸芸小站首发，阅读原文：http://xiaoyunyun.net/index.php/archives/185.html