ROS kinect:OpenNI读取深度图像与彩色图像

1.创建你的包

　　catkin_create_pkg depth_rgb_image

　　将以下代码粘贴到src/depth_rgb_image.cpp

#include <stdlib.h>
#include <iostream>
#include <string>
//【1】
#include <XnCppWrapper.h>
#include "opencv/cv.h"
#include "opencv/highgui.h"

using namespace std;
using namespace cv;

void CheckOpenNIError( XnStatus result, string status )
{ 
    if( result != XN_STATUS_OK ) 
        cerr << status << " Error: " << xnGetStatusString( result ) << endl;
}

int main( int argc, char** argv )
{
    XnStatus result = XN_STATUS_OK;  
    xn::DepthMetaData depthMD;
    xn::ImageMetaData imageMD;

    //OpenCV
    IplImage*  imgDepth16u=cvCreateImage(cvSize(640,480),IPL_DEPTH_16U,1);
    IplImage* imgRGB8u=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
    IplImage*  depthShow=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
    IplImage* imageShow=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
    cvNamedWindow("depth",1);
    cvNamedWindow("image",1);
    char key=0;

    //【2】
    // context 
    xn::Context context; 
    result = context.Init(); 
    CheckOpenNIError( result, "initialize context" );  

    // creategenerator  
    xn::DepthGenerator depthGenerator;  
    result = depthGenerator.Create( context ); 
    CheckOpenNIError( result, "Create depth generator" );  
    xn::ImageGenerator imageGenerator;
    result = imageGenerator.Create( context ); 
    CheckOpenNIError( result, "Create image generator" );

    //【3】
    //map mode  
    XnMapOutputMode mapMode; 
    mapMode.nXRes = 640;  
    mapMode.nYRes = 480; 
    mapMode.nFPS = 30; 
    result = depthGenerator.SetMapOutputMode( mapMode );  
    result = imageGenerator.SetMapOutputMode( mapMode );  

    //【4】
    // correct view port  
    depthGenerator.GetAlternativeViewPointCap().SetViewPoint( imageGenerator ); 

    //【5】
    //read data
    result = context.StartGeneratingAll();  
    //【6】
    result = context.WaitNoneUpdateAll();  

    while( (key!=27) && !(result = context.WaitNoneUpdateAll( ))  ) 
    {  
        //get meta data
        depthGenerator.GetMetaData(depthMD); 
        imageGenerator.GetMetaData(imageMD);

        //【7】
        //OpenCV output
        memcpy(imgDepth16u->imageData,depthMD.Data(),640*480*2);
        cvConvertScale(imgDepth16u,depthShow,255/4096.0,0);
        memcpy(imgRGB8u->imageData,imageMD.Data(),640*480*3);
        cvCvtColor(imgRGB8u,imageShow,CV_RGB2BGR);
        cvShowImage("depth", depthShow);
        cvShowImage("image",imageShow);
        key=cvWaitKey(20);
    }

    //destroy
    cvDestroyWindow("depth");
    cvDestroyWindow("image");
    cvReleaseImage(&imgDepth16u);
    cvReleaseImage(&imgRGB8u);
    cvReleaseImage(&depthShow);
    cvReleaseImage(&imageShow);
    context.StopGeneratingAll();
    context.Shutdown();
    return 0;
}

3.代码解释

【1】<XnCppWrapper.h>便是OpenNI的文件头了，使用OpenNI的话，目前只要include这个就行。

【2】DepthGenerator和ImageGenerator，小斤称之为图像生成器，前者负责深度图像，后者负责彩色图像。创建一个生成器非常简单，首先我们要初始化一个Context上下文，然后把Context作为Create函数的参数，便可以创建生成器了。

【3】XnMapOutputMode是用来设定生成器的参数的，这边小斤设定了分辨率为640*480（标准），30fps采样。

【4】depthGenerator.GetAlternativeViewPointCap().SetViewPoint( imageGenerator)这句话也许会让大家疑惑，它是用来调整视角的。为什么要调整呢？因为Kinect的三只眼长在不同的地方，所以画幅一致的深度摄像头和彩色摄像头，它们看出来的景物是有偏差的，这里OpenNI提供了函数进行对齐。这里，小斤把深度生成器的视角，设定为彩色生成器的视角。

【5】调用StartGeneratingAll()后，生成器们便开始上班了，如果要结束，就StopGeneratingAll()函数。

【6】尽管生成器们在工作了，但他们一直忙着各读各的，没有人协调，自己不会乖乖把最新的资料给我们。我们调用getMetaData()方法前，需要使用WaitAnyUpdateAll()、WaitOneUpdateAll()、WaitNoneUpdateAll()和WiatAndUpdateAll()中的一种。功能如其名，这边小斤使用的是WaitNoneUpdateAll()函数，它比较暴力，不管生成器有没有读到新数据，我这边先更新了再说。大家可以试试其它三个，看看效果。

【7】这边使用OpenNI获得图像MetaData数据后，小斤通过一系列函数，转换为OpenCV的IplImage图像类型，然后输出。主要参考了这篇文章。

对于深度MetaData，这边使用cvConvertScale转换尺度，成为灰度值[0,255]的灰度图。对于彩色MetaData，使用cvCvtColor转换色彩空间即可。

4.打开package.xml,添加

<build_depend>XnCppWrapper</build_depend>

<run_depend>XnCppWrapper</run_depend>

<cpp cflags="-I/你的OPENNI路径"/>

5.打开CMakeLists.txt,添加

find_package( OpenCV REQUIRED )
include_directories(
  ${OpenCV_INCLUDE_DIRS}
  "/usr/include/ni"
)
add_executable(depth_rgb_image src/depth_rgb_image.cpp)
target_link_libraries(depth_rgb_image OpenNI ${OpenCV_LIBRARIES} )

6.编译代码

cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
catkin_make

7.运行代码

rosrun depth_rgb_image depth_rgb_image

如果以上代码不能运行,请新source你的setup.bash

8.退出ctrl+c

9.用rivz显示

rosmake rviz

rosrun depth_rbg_image depth_rbg_image
rosrun rviz rviz

　　3)修改界面上的部分参数即可看见图像