KinectFusionのFrameTextureからPointCloudを取り出す

D3DとかMicrosoft方言は使ったことが無いとチンプンカンプンです困りますね。LockRectとか何の必要性があって存在するのかピンと来ません*1。それはさておき、KinectFusionで得られた結果の中から３次元位置を取得しましょう。

参考になるのは、本家のForumのこのページです。

http://social.msdn.microsoft.com/Forums/wpapps/ja-JP/d49cc1a8-324e-4069-8bc6-23afa2e0d9ed/extracting-point-cloud-data-using-kinect-fusion-api

PointCloudは位置と法線を持っているようなので、位置だけ欲しいときは明示的に取り出してやる必要があります。正しく取れているか、OpenCVの絵にしてみて見ましょう。

   if (!m_bCaptureColor)

    {

        hr = NuiFusionShadePointCloud(m_pPointCloud, &m_worldToCameraTransform, nullptr, m_pShadedSurface, nullptr);

        if (FAILED(hr))

        {

            SetStatusMessage("Kinect Fusion NuiFusionShadePointCloud call failed.");

            return;

        }

    }

//フレームから画像データの取得

INuiFrameTexture * pTexture = m_pPointCloud->pFrameTexture;

NUI_LOCKED_RECT LockedRect;

pTexture->LockRect( 0, &LockedRect, NULL, 0 );

PointCloudValue *frame = new PointCloudValue[m_pPointCloud->width * m_pPointCloud->height];

memcpy(frame, LockedRect.pBits, LockedRect.size);

pTexture->UnlockRect(0);

char filename[1024];
static int cnt = 0;
sprintf_s(filename, sizeof(filename), "pcd%04d.dat", cnt++);
std::ofstream fout;
fout.open(filename, std::ios::out|std::ios::binary);
fout.write((char*)frame, sizeof(PointCloudValue)*m_pPointCloud->width * m_pPointCloud->height);
fout.close();
 

cv::Mat jpg(cv::Size(width,height), CV_8UC3);

sprintf_s(filename, sizeof(filename), "test%04d.jpg", cnt++);

for(int i=0; i <jpg.rows; i++){

  for(int j=0; j<jpg.cols; j++){

    cv::Vec3b &b = jpg.at<cv::Vec3b>(i,j);

    float x = frame[i*jpg.cols+j].Position[0];

    float y = frame[i*jpg.cols+j].Position[1];

    float z = frame[i*jpg.cols+j].Position[2];

    b[0] = fabs(x) * 255 > 255 ? 255 : fabs(x) * 255;

    b[1] = fabs(y) * 255 > 255 ? 255 : fabs(y) * 255;

    b[2] = fabs(z) * 255 > 255 ? 255 : fabs(z) * 255;

  }

}

cv::imwrite(filename, jpg);

delete [] frame;

 fig.キネクトの座標原点

*1:念のため、共有メモリを使ってるから？