透过 OpenNI 建立 Kinect 3D Point Cloud

在可以透过 OpenNI 读取到 Kinect 的深度、色彩信息之后，其实就可以试着用这些信息，来重建 3D 的环境做显示了～不过实际上，在前面的范例中所读到的深度信息，都算是原始数据，而且坐标轴也都是传感器二维影像的坐标系统，如果要重建 3D 场景的话，这些信息都还是需要换算的；所幸，OpenNI 在 Depth Generator 已经有提供 ConvertProjectiveToRealWorld() 和ConvertRealWorldToProjective() 这两个函式，可以帮助程序开发者快速地进行坐标转换了！

而如果把直接把这些 3D 的点位附加颜色、用 OpenGL 画出来呢，就大概会是下面影片的样子吧～

当然，point cloud 不见得是最好的显示方式，有需要的话也可以重建出多边形再画，不过多边形的重建已经算是另一个主题了，所以 Heresy 也不打算在这边讨论；另外，Heresy 在这篇也不会提及 OpenGL 显示的部分，只会提供简单的范例，示范如何建立出这些 pointcloud 而已。

 

而为了储存这些点的位置以及颜色信息，这边先定义了一个简单的结构、SColorPoint3D：

structSColorPoint3D

{

  float  X;

  float  Y;

  float  Z;

  float  R;

  float  G;

  float  B;

 

  SColorPoint3D( XnPoint3D pos, XnRGB24Pixelcolor )

  {

    X = pos.X;

    Y = pos.Y;

    Z = pos.Z;

    R = (float)color.nRed / 255;

    G = (float)color.nGreen / 255;

    B = (float)color.nBlue / 255;

  }

};

这个结构只是单纯的六个福点数，分别记录这个点的位置、以及颜色；而建构子的部分，则是传入 OpenNI 定义的结构的变量：代表位置的 XnPoint3D  以及代表 RGB 颜色的 XnRGB24Pixel。

而为了方便起见，Heresy 把坐标转换的部分写成一个函式 GeneratePointCloud()，其内容如下：

voidGeneratePointCloud( xn::DepthGenerator&rDepthGen,

                         constXnDepthPixel* pDepth,

                         constXnRGB24Pixel* pImage,

                         vector<SColorPoint3D>& vPointCloud )

{

  // 1. number of pointis the number of 2D image pixel

  xn::DepthMetaData mDepthMD;

  rDepthGen.GetMetaData( mDepthMD );

  unsigned intuPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();

 

  // 2. build the datastructure for convert

  XnPoint3D* pDepthPointSet = new XnPoint3D[ uPointNum ];

  unsigned int i,j, idxShift, idx;

  for( j = 0; j < mDepthMD.FullYRes(); ++j )

  {

    idxShift = j * mDepthMD.FullXRes();

    for( i = 0; i < mDepthMD.FullXRes(); ++i )

    {

      idx = idxShift + i;

      pDepthPointSet[idx].X = i;

      pDepthPointSet[idx].Y = j;

      pDepthPointSet[idx].Z = pDepth[idx];

    }

  }

 

  // 3. un-projectpoints to real world

  XnPoint3D* p3DPointSet = newXnPoint3D[ uPointNum ];

  rDepthGen.ConvertProjectiveToRealWorld(uPointNum, pDepthPointSet, p3DPointSet );

  delete[] pDepthPointSet;

 

  // 4. build pointcloud

  for( i = 0; i < uPointNum; ++ i )

  {

    // skip the depth 0points

    if( p3DPointSet[i].Z == 0 )

      continue;

 

    vPointCloud.push_back( SColorPoint3D(p3DPointSet[i], pImage[i] ) );

  }

   delete[] p3DPointSet;

}

这个函示要把 xn::DepthGenerator 以及读到的深度影像和彩色影像传进来，用来当作数据源；同时也传入一个vector<SColorPoint3D>，作为储存转换完成后的 3D 点位数据。

其中，深度影像的格式还是一样用 XnDepthPixel 的 const 指标，不过在彩色影像的部分，Heresy 则是改用把 RGB 封包好的 XnRGB24Pixel，这样可以减少一些索引值的计算；而因为这样修改，之前读取彩色影像的程序也要由

const XnUInt8* pImageMap = mImageGenerator.GetImageMap();

修改为

const XnRGB24Pixel* pImageMap = mImageGenerator.GetRGB24ImageMap();

而在函式内容的部分，第一段的部分主要是透过取得 depth generator 的 meta-data：xn::DepthMetaData 来做简单的大小、索引计算；如果不想这样用的话，其实也是可以直接用 640 x 480 这样固定的值来做计算，不过就是要和之前在 SetMapOutputMode() 所设定的分辨率一致就是了。

第二部分「build the data structure for convert」，则是将深度影像的 640 x 480 个点，都转换为 XnPoint3D 形式的一为数组，已准备进行之后的坐标转换。

第三部分「un-project points to real world」则就是实际进行转换的部分了。这边要把坐标由影像的坐标系统转换到 3D 坐标系统，主要是用 Depth Generator 的 ConvertProjectiveToRealWorld() 这个函式；而它的使用方法也很简单，只要告诉他要转换的点的数量（uPointNum）、把要转换的点用数组的形式传（const XnPoint3D*）进去，并给他一块已经 allocate好的 XnPoint3D 数组（p3DPointSet），就可以自动进行转换了～

第四部份 Heresy 则是再用一个循环去扫过全部的点，并把深度为 0 的点给去掉（因为这些点是代表是 Kinect 没有办法判定深度的部分）、并和颜色的信息一起转换为 SColorPoint3D 的形式，丢到 vPointCloud 里储存下来了。

（这个动作其实也可以在第二步的时候先做掉，但是在那边做颜色的部分会比较麻烦就是了。）

而回到主程序的部分，本来读取数据的程序是：

// 8. read data

eResult = mContext.WaitNoneUpdateAll();

if( eResult == XN_STATUS_OK )

{

  // 9a. get the depth map

  const XnDepthPixel*  pDepthMap =mDepthGenerator.GetDepthMap();

  // 9b. get the image map

  const XnUInt8*    pImageMap =mImageGenerator.GetImageMap();

}

前面也提过了，Heresy 这边不打算提及用 OpenGL 显示的部分，所以这边为了不停地更新数据，所以改用一个无穷循环的形式来不停地更新数据、并进行坐标转换；而转换后的结果，也很简单地只输出它的点的数目了。

//8. read data

vector<SColorPoint3D> vPointCloud;

while( true )

{

  eResult = mContext.WaitNoneUpdateAll();

  // 9a. get the depthmap

  const XnDepthPixel* pDepthMap = mDepthGenerator.GetDepthMap();

 

  // 9b. get the imagemap

  constXnRGB24Pixel* pImageMap = mImageGenerator.GetRGB24ImageMap();

 

  // 10 generate pointcloud

  vPointCloud.clear();

  GeneratePointCloud( mDepthGenerator, pDepthMap, pImageMap, vPointCloud );

  cout << "Pointnumber: " << vPointCloud.size() <<endl;

}

如果是要用 OpenGL 画出来的话，基本上就是不要使用无穷循环，而是在每次要画之前，再去读取 Kinect 的数据、并透过 GeneratePointCloud() 做转换了～而如果不打算重建多边形、而是像 Heresy 直接一点一点画出来的话，结果大概就会像上面的影片一样了～