OpenNi2+PCL显示彩色点云

一、openni2使用摄像头获取图像步骤：

1.openni初始化
2.openni打开设备
3.创建数据流并设置数据流模式

二、转化点云步骤

1.使用openni从数据流读取一帧图像
2.使用openni从图像读取像素数据
3.处理数据，将结果存入点云

三、示例代码

#include <iostream> 
#include <OpenNI.h>
#include <pcl/common/common_headers.h>       // for pcl::PointCloud
#include <pcl/visualization/pcl_visualizer.h>

openni::Device mDevice;
openni::VideoStream mColorStream;
openni::VideoStream mDepthStream;

bool init(){
    // Initial OpenNI
    if(openni::OpenNI::initialize() != openni::STATUS_OK){
        std::cerr << "OpenNI Initial Error: "  << openni::OpenNI::getExtendedError() << std::endl;
        return false;
    }
    // Open Device
    if(mDevice.open(openni::ANY_DEVICE) != openni::STATUS_OK) {
        std::cerr << "Can't Open Device: "  << openni::OpenNI::getExtendedError() << std::endl;
        return false;
    }
    return true;
}

bool createColorStream() {
    if(mDevice.hasSensor(openni::SENSOR_COLOR)) {
        if(mColorStream.create(mDevice, openni::SENSOR_COLOR) == openni::STATUS_OK) {
            // set video mode
            openni::VideoMode mMode;
            mMode.setResolution(640, 480);
            mMode.setFps(30);
            mMode.setPixelFormat( openni::PIXEL_FORMAT_RGB888 );

            if(mColorStream.setVideoMode(mMode) != openni::STATUS_OK) {
                std::cout << "Can't apply VideoMode: "  << openni::OpenNI::getExtendedError() << std::endl;
                return false;
            }
        } else {
            std::cerr << "Can't create color stream on device: " << openni::OpenNI::getExtendedError() << std::endl;
            return false;
        }

        // start color stream
        mColorStream.start();
        return true;
    }
    return false;
}

bool createDepthStream(){
    if(mDevice.hasSensor(openni::SENSOR_DEPTH)) {
        if(mDepthStream.create(mDevice, openni::SENSOR_DEPTH) == openni::STATUS_OK) {
            // set video mode
            openni::VideoMode mMode;
            mMode.setResolution(640,480);
            mMode.setFps(30);
            mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);

            if(mDepthStream.setVideoMode(mMode) != openni::STATUS_OK) {
                std::cout << "Can't apply VideoMode to depth stream: " << openni::OpenNI::getExtendedError() << std::endl;
                return false;
            }
        } else {
            std::cerr << "Can't create depth stream on device: " << openni::OpenNI::getExtendedError() << std::endl;
            return false;
        }
        // start depth stream
        mDepthStream.start();
        // image registration
    if( mDevice.isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) )
            mDevice.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
    else
        std::cerr << "Don't support registration" << std::endl;
        return true;
    } else {
        std::cerr << "ERROR: This device does not have depth sensor" << std::endl;
        return false;
    }
}

//openni图像流转化成点云
bool getCloudXYZCoordinate(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_XYZRGB)  {
    openni::VideoFrameRef  colorFrame;
    mColorStream.readFrame(&colorFrame);
    openni::RGB888Pixel *pColor = (openni::RGB888Pixel*)colorFrame.getData();

    openni::VideoFrameRef  mDepthFrame;
    if(mDepthStream.readFrame(&mDepthFrame) == openni::STATUS_OK) {
        float fx,fy,fz;
        int i=0;
        //以米为单位
        double fScale = 0.001;
        openni::DepthPixel *pDepthArray = (openni::DepthPixel*)mDepthFrame.getData();
        for(int y = 0; y < mDepthFrame.getHeight(); y++) {
            for(int x = 0; x < mDepthFrame.getWidth(); x++) {
                int idx = x + y*mDepthFrame.getWidth();
                const openni::DepthPixel rDepth = pDepthArray[idx];
                openni::CoordinateConverter::convertDepthToWorld(mDepthStream,x,y,rDepth,&fx,&fy,&fz);
                fx = -fx;
                fy = -fy;
                cloud_XYZRGB->points[i].x = fx * fScale;
                cloud_XYZRGB->points[i].y = fy * fScale;
                cloud_XYZRGB->points[i].z = fz * fScale;
                cloud_XYZRGB->points[i].r = pColor[i].r;
                cloud_XYZRGB->points[i].g = pColor[i].g;
                cloud_XYZRGB->points[i].b = pColor[i].b;
                i++;
            }
        }
        return true;
    } else {
        std::cout << "getCloudXYZCoordinate: fail to read frame from depth stream" << std::endl;
        return false;
    }
}

int main(){ 
    //openni初始化、打开摄像头
    if(!init()) {
        std::cout << "Fail to init ..." << std::endl;
        return -1;
    }
    //openni创建图像流
    if(createColorStream() && createDepthStream())
        std::cout << "displayPointCloud: create color stream and depth stream ..." << std::endl;
    else{
        std::cout << "displayPointCloud: can not create color stream and depth stream ..." << std::endl;
        return -1;
    }
    //创建pcl云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_XYZRGB(new pcl::PointCloud<pcl::PointXYZRGB>());
    cloud_XYZRGB->width = 640;
    cloud_XYZRGB->height = 480;
    cloud_XYZRGB->points.resize(cloud_XYZRGB->width*cloud_XYZRGB->height);
    //pcl可视化
    pcl::visualization::PCLVisualizer::Ptr m_pViewer(new pcl::visualization::PCLVisualizer("Viewer"));
    m_pViewer->setCameraPosition(0, 0, -2, 0,-1, 0, 0);
    m_pViewer->addCoordinateSystem(0.3);
    while(!m_pViewer->wasStopped()) {
        getCloudXYZCoordinate(cloud_XYZRGB);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_XYZRGB);
        m_pViewer->addPointCloud<pcl::PointXYZRGB>(cloud_XYZRGB,rgb,"cloud");
        m_pViewer->spinOnce();
        m_pViewer->removeAllPointClouds();
    }
    mColorStream.destroy();
    mDepthStream.destroy();
    mDevice.close();
    openni::OpenNI::shutdown();

    return 0;
}

使用
pcl::visualization::PointCloudColorHandlerCustom GreenHandler(cloud_XYZRGB,0,255,0);
m_pViewer->addPointCloud(cloud_XYZRGB,GreenHandler,”cloud”);
可以将点云全部设置为绿色。

四、CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(pcl)

SET(OpenNI2_SO_DIR ${PROJECT_SOURCE_DIR}/lib)

find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (pcl ${PROJECT_SOURCE_DIR}/src/main.cpp)
target_link_libraries (pcl ${PCL_LIBRARIES} ${OpenNI2_SO_DIR}/libOpenNI2.so)

不同的相机需要的libOpenNI2.so不同，依个人情况修改。