#include #include #include #include #include #include #include // ポイントクラウド変換クラス class DepthParam { public: XnUInt64 focalLength; XnDouble pixelSize; int uSize, vSize; int uCenter; int vCenter; void setParam( const xn::DepthGenerator &depth ) { // get the focal length in mm (ZPS = zero plane distance) depth.GetIntProperty ("ZPD", focalLength ); // get the pixel size in mm ("ZPPS" = pixel size at zero plane) depth.GetRealProperty ("ZPPS", pixelSize ); } void setParam( const xn::DepthMetaData &depthMD ) { uSize = depthMD.XRes(); vSize = depthMD.YRes(); uCenter = uSize / 2; vCenter = vSize / 2; } bool depth2point( const XnDepthPixel* depthMap, XnPoint3D *points ) { float P2R; // 変換定数 P2R = pixelSize * ( 1280 / uSize ) / focalLength; // 変換定数の計算 //実際の素子が1280x1024を640x480に変換している為、使うときは2倍しないといけない float dist; for (int v = 0; v < vSize; ++v) { for (int u = 0; u < uSize; ++u) { dist = (*depthMap) * 0.001f; //mm -> m #if 0 // カメラ座標系での座標変換 // x right // y bottom // z front points->X = dist * P2R * ( u - uCenter ); points->Y = dist * P2R * ( v - vCenter ); points->Z = dist; #else // ロボット座標系での座標変換 // x front // y left // z top points->X = dist; points->Y = dist * P2R * ( uCenter - u ); points->Z = dist * P2R * ( vCenter - v ); #endif ++depthMap; ++points; } } return true; } }; #if 0 // サンプルのメイン関数 // こんな感じで使う // 公開用に書き直したので動くか微妙です int main(int argc, char **argv) { XnStatus nRetVal = XN_STATUS_OK; Context context; EnumerationErrors errors; cout << "sensor open" << endl; nRetVal = context.InitFromXmlFile(DEFAULT_KINECT_SCENE_XML_PATH, &errors); if (nRetVal == XN_STATUS_NO_NODE_PRESENT) { XnChar strError[1024]; errors.ToString(strError, 1024); printf("%s\n", strError); return (nRetVal); } else if (nRetVal != XN_STATUS_OK) { printf("Open failed: %s\n", xnGetStatusString(nRetVal)); return (nRetVal); } //context.SetGlobalMirror( false ); // ジェネレータの起動 DepthGenerator depth; ImageGenerator image; nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); CHECK_RC(nRetVal, "Find depth generator"); nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image); CHECK_RC(nRetVal, "Find image generator"); nRetVal = context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator); CHECK_RC(nRetVal, "Find user generator"); depth.GetAlternativeViewPointCap().SetViewPoint(image); if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON) || !g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) { printf("User generator doesn't support either skeleton or pose detection.\n"); return XN_STATUS_ERROR; } DepthMetaData depthMD; ImageMetaData imageMD; depth.GetMetaData( depthMD ); image.GetMetaData( imageMD ); // Hybrid mode isn't supported in this sample if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes()) { cout << "The device depth and image resolution must be equal!" << endl; return 1; } if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) { cout << "The device image format must be RGB24" << endl; return 1; } User_init(); DepthParam depthParam; depthParam.setParam( depth ); depthParam.setParam( depthMD ); // 3次元座標の確保 XnPoint3D points[640 * 480]; // 開始 nRetVal = context.StartGeneratingAll(); CHECK_RC(nRetVal, "StartGenerating"); cout << "start" << endl; while ( !gShutOff ) { //nRetVal = context.WaitOneUpdateAll(depth); //nRetVal = context.WaitOneUpdateAll(image); nRetVal = context.WaitAnyUpdateAll(); if (nRetVal != XN_STATUS_OK) { printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal)); continue; } if( depth.IsDataNew() ) { depth.GetMetaData( depthMD ); // 3次元座標変換 depthParam.depth2point( depthMD.Data(), points ); } if( image.IsDataNew() ) { image.GetMetaData( imageMD ); } } context.Shutdown(); cout << "Shutdown." << endl; return 0; } #endif