XnStatus prepare(char useScene, char useDepth, char useHistogram)
{
//TODO handle possible failures!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
}
XnStatus prepare(char useScene, char useDepth, char useImage, char useIr, char useHistogram)
{
//TODO handle possible failures! Gotcha!
if (useDepth)
{
mDepthGen.GetMetaData(depthMD);
nXRes = depthMD.XRes();
nYRes = depthMD.YRes();
pDepth = depthMD.Data();
if (useHistogram)
{
calcHist();
// rewind the pointer
pDepth = depthMD.Data();
}
}
if (useScene)
{
mUserGen.GetUserPixels(0, sceneMD);
nXRes = sceneMD.XRes();
nYRes = sceneMD.YRes();
pLabels = sceneMD.Data();
}
if (useImage)
{
mImageGen.GetMetaData(imageMD);
nXRes = imageMD.XRes();
nYRes = imageMD.YRes();
pRGB = imageMD.RGB24Data();
// HISTOGRAM?????
}
if (useIr)
{
mIrGen.GetMetaData(irMD);
nXRes = irMD.XRes();
nYRes = irMD.YRes();
pIR = irMD.Data();
// HISTOGRAM????
}
}
void WorldRenderer::getHenshinData(XnUserID* pUserID, const XnLabel** ppLabel, XV3* pHeadCenter, XV3* pHeadDirection)
{
UserDetector* userDetector = m_ball_manager->user_detector();
XnUserID userID = *pUserID = userDetector->getTrackedUserID();
if (userID) {
SceneMetaData smd;
userDetector->getUserGenerator()->GetUserPixels(userID, smd);
*ppLabel = smd.Data();
XV3 ps[2];
//ps[0].assign(m_kkhStatus->center);
ps[0].assign(userDetector->getSkeletonJointPosition(XN_SKEL_HEAD));
ps[1].assign(userDetector->getSkeletonJointPosition(XN_SKEL_NECK));
m_depthGen->ConvertRealWorldToProjective(3, ps, ps);
normalizeProjective(&ps[0]);
normalizeProjective(&ps[1]);
//normalizeProjective(&ps[2]);
//*pLightCenter = ps[0];
*pHeadCenter = ps[0];
*pHeadDirection = ps[0] - ps[1];
}
}
//----------------------------------------------------
// イメージ描画
//----------------------------------------------------
void drawImage(void){
switch(g_nViewState){
case DISPLAY_MODE_OVERLAY: // ノーマル描画モード
case DISPLAY_MODE_DEPTH:
case DISPLAY_MODE_IMAGE:
glMatrixMode(GL_PROJECTION); // 射影変換の行列の設定
glLoadIdentity(); // スタックのクリア
gluOrtho2D(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0); // ワールド座標系を正規化デバイス座標系に平行投影(left, right, buttom, top, near, far)
// ★平行投影する事によって，ポイントクラウドも平面に投影でき，クロマキーに最適
// Kinectの距離は約500〜9000まで使える(設定は10000)
glMatrixMode(GL_MODELVIEW); // モデルビュー変換の行列の設定
glLoadIdentity();
glEnable(GL_TEXTURE_2D); // テクスチャマッピングの有効化
// テクスチャパラメータの設定と定義
glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, g_nTexMapX, g_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, g_pTexMap); // イメージデータ貼り付け
// Display the OpenGL texture map
glColor4f(1,1,1,1);
// イメージデータの貼り付け
glBegin(GL_QUADS); // 四角形の描画を行う
{
int nXRes = g_depthMD.FullXRes();
int nYRes = g_depthMD.FullYRes();
// 左上
glTexCoord2f(0, 0);
glVertex2f(0, 0); // 座標指定
// 右上
glTexCoord2f((float)nXRes/(float)g_nTexMapX, 0);
glVertex2f(GL_WIN_SIZE_X, 0); // 座標指定
// 右下
glTexCoord2f((float)nXRes/(float)g_nTexMapX, (float)nYRes/(float)g_nTexMapY);
glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); // 座標指定
// 左下
glTexCoord2f(0, (float)nYRes/(float)g_nTexMapY);
glVertex2f(0, GL_WIN_SIZE_Y); // 座標指定
}
glEnd();
glDisable(GL_TEXTURE_2D); // テクスチャマッピングの無効化
break;
case DISPLAY_MODE_CHROMA: // ポイントクラウド描画モード
case DISPLAY_MODE_POINT_CLOUD:
// 投影変換
glMatrixMode(GL_PROJECTION); // 射影変換の行列の設定
glLoadIdentity(); // スタックのクリア
glOrtho(0, KINECT_IMAGE_WIDTH,
KINECT_IMAGE_HEIGHT, 0,
-1.0, -KINECT_MAX_DEPTH - KINECT_VISIBLE_DELTA); // ワールド座標系を正規化デバイス座標系に平行投影(left, right, buttom, top, near, far)
// ★平行投影する事によって，ポイントクラウドも平面に投影でき，クロマキーに最適
// Kinectの距離は約500〜9000まで使える(設定は10000)
// 視野変換
gluLookAt(
g_lokEyeX, g_lokEyeY, g_lokEyeZ, // 視点の位置(初期位置：(0,0,-1))
g_lokDirX, g_lokDirY, g_lokDirZ, // 視点先の位置(初期位置：(0,0,-2))
0.0, 1.0, 0.0); // 向き
// モデリング変換
glMatrixMode(GL_MODELVIEW); // モデルビュー変換の行列の設定
glLoadIdentity(); // スタックのクリア
glEnable(GL_DEPTH_TEST); // 陰面処理の有効化
// ポイントクラウド表示
glPointSize(g_pointSize); // 点のサイズ
drawPointCloud(g_pBackTex, g_pBackDepth, g_pPoint); //背景画像表示
//drawPointCloud(g_imageMD.RGB24Data(), g_depthMD.Data(), 10, g_chromaThresh); // 人物抜き出し(距離の閾値)
drawPointCloudHuman(g_imageMD.RGB24Data(), g_depthMD.Data(), g_sceneMD.Data(), g_pPoint); // 人物抜き出し(動くものを検出)
glDisable(GL_DEPTH_TEST); // 陰面処理の無効化
break;
}
}
//----------------------------------------------------
// テクスチャの設定
//----------------------------------------------------
void setTexture(void){
xnOSMemSet(g_pTexMap, 0, g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel)); // g_pTexMapの全てに0を代入
// 描画モード1か3
if (g_nViewState == DISPLAY_MODE_OVERLAY || g_nViewState == DISPLAY_MODE_IMAGE){
const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data(); // g_imageMDのポインタ取得(画像データ取得)
XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX;
for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; ++ y){
const XnRGB24Pixel* pImage = pImageRow;
XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset();
for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; ++ x, ++ pImage, ++ pTex){
*pTex = *pImage;
}
pImageRow += g_imageMD.XRes();
pTexRow += g_nTexMapX;
}
}
// 描画モード1か2
if (g_nViewState == DISPLAY_MODE_OVERLAY || g_nViewState == DISPLAY_MODE_DEPTH){
const XnDepthPixel* pDepthRow = g_depthMD.Data();
XnRGB24Pixel* pTexRow = g_pTexMap + g_depthMD.YOffset() * g_nTexMapX;
const XnLabel* pLabel = g_sceneMD.Data();
for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; ++ y){
const XnDepthPixel* pDepth = pDepthRow;
XnRGB24Pixel* pTex = pTexRow + g_depthMD.XOffset();
for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; ++ x, ++ pDepth, ++ pTex, ++ pLabel){
int nHistValue = g_pDepthHist[*pDepth];
if(*pLabel){ // 人物なら
*pTex = userColor[*pLabel];
}else if (*pDepth != 0){
if(*pDepth < 1000){
*pTex = xnRGB24Pixel(nHistValue, 0, 0); // red
}else if(*pDepth < 2000){
*pTex = xnRGB24Pixel(0, nHistValue, 0); // green
}else if(*pDepth < 3000){
*pTex = xnRGB24Pixel(0, 0, nHistValue); // blue
}else if(*pDepth < 4000){
*pTex = xnRGB24Pixel(nHistValue, nHistValue, 0); // 水色
}else if(*pDepth < 5000){
*pTex = xnRGB24Pixel(0, nHistValue, nHistValue); // yellow
}else{
*pTex = xnRGB24Pixel(nHistValue, 0, nHistValue); // 紫
}
}
}
pDepthRow += g_depthMD.XRes();
pTexRow += g_nTexMapX;
}
}
// 描画モード4
//if (g_nViewState == DISPLAY_MODE_CHROMA){
// // イメージデータ(カメラ映像)貼り付け
// const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data(); // g_imageMDのポインタ取得(画像データ取得)
// XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX;
// for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; ++ y){ // 480
// const XnRGB24Pixel* pImage = pImageRow;
// XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset();
// for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; ++ x, ++ pImage, ++ pTex){ // 640
// *pTex = *pImage;
// }
// pImageRow += g_imageMD.XRes();
// pTexRow += g_nTexMapX;
// }
// // デプスデータを用いた人物抜き出し + 背景合成
// const XnDepthPixel* pDepthRow = g_depthMD.Data(); // デプスデータのポインタ取得
// pTexRow = g_pTexMap + g_depthMD.YOffset() * g_nTexMapX;
// GLuint g_backWidth = g_back.GetWidth(); // 背景の横幅の大きさ
// GLubyte* pBackData = g_back.GetData() + g_back.GetImageSize() - 3 * g_backWidth; // 背景のポインタ取得(最後から見ていく)
// for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; ++ y){ // 480
// const XnDepthPixel* pDepth = pDepthRow; // デプスデータのポインタ取得
// XnRGB24Pixel* pTex = pTexRow + g_depthMD.XOffset();
// for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; ++ x, ++ pDepth, ++ pTex){ // 640
// // 深さが0か閾値以上なら背景画像を描画（閾値以下ならその部分を残す）
// if (*pDepth == 0 || *pDepth >= g_chromaThresh){
// pTex->nRed = *pBackData;
// pTex->nGreen = *(pBackData + 1);
// pTex->nBlue = *(pBackData + 2);
// }
// pBackData += 3;
// }
// pDepthRow += g_depthMD.XRes();
// pTexRow += g_nTexMapX;
// pBackData -= 2 * 3 * g_backWidth;
// }
//}
}
void captureDepthMap(unsigned char* g_ucDepthBuffer)
{
SceneMetaData smd;
DepthMetaData dmd;
_depth.GetMetaData(dmd);
//printf("AS3OpenNI :: Frame %d Middle point is: %u. FPS: %f\n", dmd.FrameID(), dmd(dmd.XRes() / 2, dmd.YRes() / 2), xnFPSCalc(&xnFPS));
_depth.GetMetaData(dmd);
_userGenerator.GetUserPixels(0, smd);
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (_drawPixels)
{
XnUInt32 nIndex = 0;
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
g_ucDepthBuffer[0] = 0;
g_ucDepthBuffer[1] = 0;
g_ucDepthBuffer[2] = 0;
g_ucDepthBuffer[3] = 0x00;
if (_depthMapBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
if(_depthMapDetect)
{
g_ucDepthBuffer[0] = nHistValue * Colors[nColorID][0];
g_ucDepthBuffer[1] = nHistValue * Colors[nColorID][1];
g_ucDepthBuffer[2] = nHistValue * Colors[nColorID][2];
}
else
{
g_ucDepthBuffer[0] = nHistValue;
g_ucDepthBuffer[1] = nHistValue;
g_ucDepthBuffer[2] = nHistValue;
}
g_ucDepthBuffer[3] = 0xFF;
}
}
pDepth++;
pLabels++;
g_ucDepthBuffer+=4;
}
}
}
}
