Kinect彩色图和红外图获取

Kinect的深度图实际上是由IR相机（红外相机）形成的，因此，本质上做的是彩色图和红外图像的对齐。因此，需要先获取彩色图像和红外图像，图像的获取方法应该是多样的，比如通过ros利用freenect功能包启动kinect，通过image_view订阅相关话题显示彩色图和红外图像，问题是我在订阅红外图像的时候总是不显示，全是黑色一片，只订阅红外图像话题仍然存在这个问题，尝试过很多方法都不行，考虑到一个原因就是kinect v1不能同时显示彩色图像和红外图像，但是在rviz中显示就没有问题，很费解！

此方法行不通后，考虑自己写代码实现，参考了一些资料后，成功实现彩色图像和红外图像的采集保存。代码及相应的配置文件如下，为保证文章的连续性，代码的说明放在另一篇幅中（链接）。

main.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;

//图像的宽度和高度，注意顺序
#define Depth_Width 640
#define Depth_Height 480
#define Rgb_Width 640
#define Rgb_Height 480
#define Ir_Width 640
#define Ir_Height 480

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;
    xn::IRMetaData irMD;

    //OpenCV
    //IplImage* cvCreateImage(CvSize cvSize(int width, int height), 
    //int depth, int channels);
    //如果我们要创建一个宽为360,高为640的3通道图像（RGB图像），
    //可以采用如下语句：
    //IplImage* img=cvCreateImage( cvSize(360,640), IPL_DEPTH_8U,3 );

    IplImage*  imgDepth16u=cvCreateImage(cvSize(Depth_Width,Depth_Height),
    IPL_DEPTH_16U,1);
    IplImage* imgRGB8u=cvCreateImage(cvSize(Rgb_Width,Rgb_Height),
    IPL_DEPTH_8U,3);
    IplImage* imgIR16u = cvCreateImage(cvSize(Ir_Width,Ir_Height),
    IPL_DEPTH_16U,1);

    IplImage*  depthShow=cvCreateImage(cvSize(Depth_Width,Depth_Height),
    IPL_DEPTH_8U,1);
    IplImage* imageShow=cvCreateImage(cvSize(Rgb_Width,Rgb_Height),
    IPL_DEPTH_8U,3);
    IplImage* irShow = cvCreateImage(cvSize(Ir_Width,Ir_Height),
    IPL_DEPTH_8U,1);

    // create window
    cvNamedWindow("depth",1);
    cvNamedWindow("image",1);
    cvNamedWindow("ir",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" );
    xn::IRGenerator irGenerator;
    result = irGenerator.Create( context );
    CheckOpenNIError( result, "Create ir generator" );

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

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

    //【5】
    //read data
    result = context.StartGeneratingAll();
    //【6】
    result = context.WaitNoneUpdateAll();
    long currentFrame = 0;
    while( (key!=27) && !(result = context.WaitNoneUpdateAll( ))  )
    {
        //get meta data
        depthGenerator.GetMetaData(depthMD);
        imageGenerator.GetMetaData(imageMD);
        irGenerator.GetMetaData(irMD);
        stringstream str;
        str << "stamp" << currentFrame << ".jpg";
        //【7】
        //OpenCV output
        memcpy(imgDepth16u->imageData,depthMD.Data(),640*480*2);
        cvConvertScale(imgDepth16u,depthShow,255/4096.0,0);
        memcpy(imgIR16u->imageData,irMD.Data(),640*480*2);
        cvConvertScale(imgIR16u,irShow,1,0);;
        memcpy(imgRGB8u->imageData,imageMD.Data(),640*480*3);
        cvCvtColor(imgRGB8u,imageShow,CV_RGB2BGR);
        cvShowImage("depth", depthShow);
        cvShowImage("image",imageShow);
        cvShowImage("ir",irShow);
        cvShowImage("ir",irShow);
        if (currentFrame % 30 == 0)
        {
            cvSaveImage("rgb.jpg",imageShow);
            cvSaveImage("depth.png",depthShow);
            cvSaveImage("ir.png",irShow);
        }
        key=cvWaitKey(20);
        currentFrame++;
    }

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

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(hellokinect)
 # 寻找OpenCV库
 find_package( OpenCV REQUIRED )
 # 添加头文件
 include_directories( ${OpenCV_INCLUDE_DIRS} )
 # 包含OpenNI库
 include_directories ("/usr/include/ni/" )
 add_executable(hellokinect main.cpp)
 target_link_libraries( hellokinect OpenNI ${OpenCV_LIBS})

创建图像包含三个元素：

图像的大小
图像的深度
图像的通道

depth 图像元素的位深度，可以是下面的其中之一：

位深度	取值范围
IPL_DEPTH_8U - 无符号8位整型	0–255
IPL_DEPTH_8S - 有符号8位整型	-128–127
IPL_DEPTH_16U - 无符号16位整型	0–65535
IPL_DEPTH_16S - 有符号16位整型	-32768–32767
IPL_DEPTH_32S - 有符号32位整型	0–65535
IPL_DEPTH_32F - 单精度浮点数	0.0–1.0
IPL_DEPTH_64F - 双精度浮点数	0.0–1.0

位深度转换原理

如上，给出图像的位深度及其取值范围后，我们不难理解，要转换位深度本质上就是对原深度下的数据做线性变换，使原位深度下的最小值和最大值分别对应转换后位深度下的最小值和最大值。实现上述线性变换，我们可以用opencv库函数cvConvertScale。

图像的通道

比较通俗易懂的解释是：灰度图的通道数为1，彩色图的通道为3。基本上，描述一个像素点，如果是灰度，那么只需要一个数值来描述它，就是单通道。如果一个像素点，有RGB三种颜色来描述它，就是三通道。

Kinect彩色图和深度图对准

MATLAB对准方法

将得到的彩色图像和红外图像分为两组，然后利用MATLAB的stereoCameraCalibrator工具箱，输入两组图像，然后点击calibrate即可得到两个相机的坐标变换旋转矩阵$R$和平移矩阵$T$，MATLAB得到的矩阵$\hat{R}$需要转置后才是旋转矩阵$R$.stereoCameraCalibrator工具箱链接

Kinect彩色图和深度图对准理论方法

kinect RGB的内参:
$$K_{rgb} =\left[\begin{matrix}
f_{x_rgb} & 0 & c_{x_rgb}\\
0 & f_{y_rgb} & c_{y_rgb}\\
0 & 0 & 1
\end{matrix}\right] =\left[\begin{array}{ccc}
522.955354 & 0.000000 & 327.140468\\
0.000000 & 523.763483 & 218.630165\\
0.000000 & 0.000000 & 1.000000
\end{array}\right]$$
因此，对于各自相机坐标系下的齐次的三维点($P=[X,Y,Z,1]^T$)到各自图片上齐次表示的像素坐标($p=[u,v,1]^T$)的映射关系如下：

对彩色相机而言，有：

$$Z_{rgb}\cdot p_{rgb} = K_{rgb}\cdot [I|O]\cdot P_{rgb}$$
展开如下，

\begin{aligned}
Z_{rgb}\cdot \left[\begin{array}{c}
u_{rgb}\\v_{rgb}\\1
\end{array}\right]& = \left[
\begin{array}{c}
f_{x_rgb}\cdot X_{rgb}+c_{x_rgb}\cdot Z_{rgb}\\f_{y_rgb}\cdot Y_{rgb}+c_{y_rgb}\cdot Z_{rgb}\\Z_{rgb}
\end{array}\right] \\& =\left[
\begin{array}{cccc}
f_{x_rgb} & 0 & c_{x_rgb} & 0\\
0 & f_{y_rgb} & c_{y_rgb} & 0\\
0 & 0 & 1& 0
\end{array}\right]\cdot \left[
\begin{array}{c}
X_{rgb}\\Y_{rgb}\\Z_{rgb}\\1
\end{array}\right]
\end{aligned}

其中，齐次坐标$P_{rgb} = [X_{rgb},Y_{rgb},Z_{rgb} 1]^T$我们可以用非齐次坐标$\bar{P_{rgb}}=[X_{rgb},Y_{rgb},Z_{rgb}]$来表示，应有如下的形式：

$$Z_{rgb}\cdot p_{rgb} = K_{rgb}\cdot \bar{P_{rgb}}$$

kinect Depth(ir)的内参:
\begin{equation}
K_{ir} =\left[\begin{array}{ccc}
f_{x_ir} & 0 & c_{x_ir}\\
0 & f_{y_ir} & c_{y_ir}\\
0 & 0 & 1
\end{array}\right] =\left[\begin{array}{ccc}
605.290291 & 0.000000 & 333.064832\\
0.000000 & 606.360112 & 205.426105\\
0.000000 & 0.000000 & 1.000000
\end{array}\right]
\end{equation}

同理，可得到深度相机的映射公式：
$$Z_{ir}\cdot p_{ir} = K_{ir}\cdot \bar{P_{ir}}$$

针对于同一幅棋盘的外参(MATLAB的cameraCalibrator可以实现内/外参标定):

彩色相机：$R_{rgb}$和$T_{rgb}$

深度相机：$R_{ir}$和$T_{ir}$

因此，两个相机有如下刚体变换关系：
$$R_{ir2rgb} = R_{rgb}\cdot R_{ir}^{-1}$$
$$T_{ir2rgb} = T_{rgb} - R_{ir2rgb}\cdot T_{ir}$$

对于非齐次坐标表示的各自相机坐标系下的三维点$\bar{P_{rgb}}$和$\bar{P_{ir}}$来说，有如下关系：
$$\bar{P_{rgb}} = R_{ir2rgb}\cdot \bar{P_{ir}} + T_{ir2rgb}$$

最后，我们可以得到如下公式：
$$Z_{rgb}\cdot p_{rgb} = K_{rgb}\cdot R_{ir2rgb}\cdot K_{ir}^{-1}\cdot Z_{ir}\cdot p_{ir}+K_{rgb}\cdot T_{ir2rgb}$$

这样就把$p_{rgb}$和$p_{ir}$联系起来了。

为了简化表示，我们令：
$$R = K_{rgb}\cdot R_{ir2rgb}\cdot K_{ir}^{-1}$$
$$T = K_{rgb}\cdot T_{ir2rgb}$$

则有，
$$Z_{rgb}\cdot p_{rgb} = R\cdot Z_{ir}\cdot p_{ir}+T$$