ROS kinect:OpenNI讀取深度圖像與彩色圖像

參考:小斤的博客

1.創建你的包

　　catkin_create_pkg depth_rgb_image 

2.

　　將以下代碼粘貼到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顯示

　　1)

rosmake rviz

　　2)

rosrun depth_rbg_image depth_rbg_image
rosrun rviz rviz

　　3)修改界面上的部分參數即可看見圖像