void SimKinect::DrawRGBMap(const xn::ImageMetaData& imd) {
static bool bInitialized = false;
static int width;
static int height;
static int width_step;
static XnRGB24Pixel* g_pTexMap;
if(!bInitialized) {
width = video_size_width;
height = video_size_height;
g_pTexMap = (XnRGB24Pixel*)malloc(width * height * sizeof(XnRGB24Pixel));
width_step = width*3;
bInitialized = true;
}
const XnUInt8* pImage = imd.Data();
const XnRGB24Pixel* pImageRow = imd.RGB24Data();
XnRGB24Pixel* pTexRow = g_pTexMap + imd.YOffset() * width;
for (XnUInt y = 0; y < imd.YRes(); ++y)
{
const XnRGB24Pixel* pImage = pImageRow;
XnRGB24Pixel* pTex = pTexRow + imd.XOffset();
for (XnUInt x = 0; x < imd.XRes(); ++x, ++pImage, ++pTex)
{
*pTex = *pImage;
}
pImageRow += imd.XRes();
pTexRow += width;
}
//Draw rgb image
XnRGB24Pixel* pix_ptr = g_pTexMap;
for ( int i=0 ; i<height ; i++ ) {
unsigned char* row_pointer = color_frame+i*width_step;
for ( int j=0 ; j<width_step; j+=3,pix_ptr++ ) {
row_pointer[j] = pix_ptr->nBlue;
row_pointer[j+1]=pix_ptr->nGreen;
row_pointer[j+2]=pix_ptr->nRed;
}
}
//imshow("COLOR",color_img);
}
/**
* return RGB Image
*/
std::string getRGBImage(bool br) {
{
ReadLock r_lock(myLock);
stringstream emitter;
emitter << "{RGBMap," << g_imageMD.XRes() << ","<< g_imageMD.YRes() << "}[";
for(int i = 0; i < g_imageMD.XRes(); i++) {
if(i>0 && br == true) emitter << "<BR>";
for(int j = 0; j < g_imageMD.YRes(); j++, pPixelPoint++) {
emitter << (int)(pPixelPoint->nRed) << ",";
emitter << (int)(pPixelPoint->nGreen) << ",";
emitter << (int)(pPixelPoint->nBlue) << ";";
}
}
emitter << "]";
return emitter.str();
}
}
inline void getBGRImageFromMetaData( const xn::ImageMetaData& imageMetaData, cv::Mat& bgrImage )
{
if( imageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24 )
CV_Error( CV_StsUnsupportedFormat, "Unsupported format of grabbed image\n" );
cv::Mat rgbImage( imageMetaData.YRes(), imageMetaData.XRes(), CV_8UC3 );
const XnRGB24Pixel* pRgbImage = imageMetaData.RGB24Data();
// CV_Assert( 3*sizeof(uchar) == sizeof(XnRGB24Pixel) );
memcpy( rgbImage.data, pRgbImage, rgbImage.total()*sizeof(XnRGB24Pixel) );
cv::cvtColor( rgbImage, bgrImage, CV_RGB2BGR );
}
int main ( int argc, char ** argv )
{
//
// Initializing Calibration Related
//
// ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_FILE, ARTAG_POS_FILE );
ARTagHelper artagHelper ( colorImgWidth, colorImgHeight, ARTAG_CONFIG_A3_FILE, ARTAG_POS_A3_FILE );
ExtrCalibrator extrCalibrator ( 6, KINECT_INTR_FILE, KINECT_DIST_FILE );
// unsigned char * kinectImgBuf = new unsigned char[colorImgWidth * colorImgHeight * 3];
//
// Initializing OpenNI Settings
//
int ctlWndKey = -1;
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
//
// Initialize Context Object
//
nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, g_ScriptNode, &errors );
if ( nRetVal == XN_STATUS_NO_NODE_PRESENT )
{
XnChar strError[1024];
errors.ToString ( strError, 1024 );
printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError );
system ( "pause" );
return ( nRetVal );
}
else if ( nRetVal != XN_STATUS_OK )
{
printf ( "Open FAILED:\n%s\n", xnGetStatusString ( nRetVal ) );
system ( "pause" );
return ( nRetVal );
}
//
// Handle the Depth Generator Node.
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Depth Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
//
// Handle the Image Generator node
//
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen );
if ( nRetVal != XN_STATUS_OK )
{
printf ( "No Image Node Exists! Please Check your XML.\n" );
return ( nRetVal );
}
// g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen );
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 );
assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT );
//
// Create OpenCV Showing Window and Related Data Structures
//
cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::Mat depthImgMat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 );
cv::Mat depthImgShow ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC3 );
cv::Mat colorImgMat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 );
#define ARTAG_DEBUG
#ifdef ARTAG_DEBUG
cv::setMouseCallback ( IMAGE_WIN_NAME, ClickOnMouse, 0 );
#endif
bool flipColor = true;
//
// Start to Loop
//
while ( ctlWndKey != ESC_KEY_VALUE )
{
//
// Try to Get New Frame From Kinect
//
nRetVal = g_Context.WaitAnyUpdateAll ();
g_DepthGen.GetMetaData ( g_DepthMD );
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() && g_DepthMD.FullYRes() == g_DepthMD.YRes() );
assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() && g_ImageMD.FullYRes() == g_ImageMD.YRes() );
GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), colorImgMat );
#ifdef SHOW_DEPTH_WINDOW
GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), depthImgMat );
// GlobalUtility::ConvertDepthCvMat16uToYellowCvMat ( depthImgMat, depthImgShow );
GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( depthImgMat, depthImgShow );
cv::imshow ( DEPTH_WIN_NAME, depthImgShow );
#endif
ctlWndKey = cvWaitKey ( 15 );
if ( ctlWndKey == 'f' || ctlWndKey == 'F' )
{
artagHelper.Clear();
artagHelper.FindMarkerCorners ( (unsigned char *)(g_ImageMD.Data()) );
artagHelper.PrintMarkerCornersPos2dInCam ();
extrCalibrator.ExtrCalib ( artagHelper );
std::cout << "\nKinect Extr Matrix:" << std::endl;
extrCalibrator.PrintMatrix ( extrCalibrator.GetMatrix ( ExtrCalibrator::EXTR ) );
std::cout << "Reprojection ERROR = "
<< extrCalibrator.ComputeReprojectionErr ( artagHelper ) << std::endl
// << extrCalibrator.ComputeReprojectionErr ( artagHelper.m_MarkerCornerPosCam2d, artagHelper.m_MarkerCornerPos3d, 24 ) << std::endl
<< "Valid Marker Number = " << artagHelper.GetValidMarkerNumber() << std::endl
<< std::endl;
extrCalibrator.SaveMatrix ( ExtrCalibrator::EXTR, KINECT_EXTR_FILE );
}
if ( ctlWndKey == 's' || ctlWndKey == 'S' )
{
flipColor = !flipColor;
}
if ( flipColor ) {
cv::cvtColor ( colorImgMat, colorImgMat, CV_RGB2BGR );
}
artagHelper.DrawMarkersInCameraImage ( colorImgMat );
cv::imshow ( IMAGE_WIN_NAME, colorImgMat );
}
g_Context.Release ();
system ( "pause" );
exit ( EXIT_SUCCESS );
}
int main ( int argc, char * argv[] )
{
//
// Initialize OpenNI Settings
//
XnStatus nRetVal = XN_STATUS_OK;
xn::ScriptNode scriptNode;
xn::EnumerationErrors errors;
//
// Initialize Context Object
//
nRetVal = g_Context.InitFromXmlFile ( CONFIG_XML_PATH, scriptNode, &errors );
if ( nRetVal == XN_STATUS_NO_NODE_PRESENT ) {
XnChar strError[1024];
errors.ToString(strError, 1024);
printf ( "XN_STATUS_NO_NODE_PRESENT:\n%s\n", strError );
system ( "pause" );
return ( nRetVal );
}
else if ( nRetVal != XN_STATUS_OK ) {
printf ( "Open failed: %s\n", xnGetStatusString(nRetVal) );
system ( "pause" );
return ( nRetVal );
}
//
// Handle Image & Depth Generator Node
//
bool colorFlag = true;
bool depthFlag = true;
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_DEPTH, g_DepthGen );
if ( nRetVal != XN_STATUS_OK ) {
printf("No depth node exists!\n");
depthFlag = false;
}
nRetVal = g_Context.FindExistingNode ( XN_NODE_TYPE_IMAGE, g_ImageGen );
if ( nRetVal != XN_STATUS_OK ) {
printf("No image node exists!\n");
colorFlag = false;
}
// g_DepthGen.GetAlternativeViewPointCap().SetViewPoint( g_ImageGen );
if ( depthFlag ) {
g_DepthGen.GetMetaData ( g_DepthMD );
assert ( g_DepthMD.PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT );
}
if ( colorFlag ) {
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24 );
}
g_DepthImgShow = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_8UC1 );
g_DepthImgMat = cv::Mat ( g_DepthMD.YRes(), g_DepthMD.XRes(), CV_16UC1 );
g_ColorImgMat = cv::Mat ( g_ImageMD.YRes(), g_ImageMD.XRes(), CV_8UC3 );
//
// Start to Loop
//
bool flipColor = true;
int ctlWndKey = -1;
g_StartTickCount = GetTickCount();
g_HeadTrackingFrameCount = 0;
while ( ctlWndKey != ESC_KEY_VALUE )
{
nRetVal = g_Context.WaitOneUpdateAll ( g_DepthGen );
// nRetVal = g_Context.WaitAnyUpdateAll();
#ifdef HANDLING_IMAGE_DATA
if ( colorFlag )
{
g_ImageGen.GetMetaData ( g_ImageMD );
assert ( g_ImageMD.FullXRes() == g_ImageMD.XRes() );
assert ( g_ImageMD.FullYRes() == g_ImageMD.YRes() );
GlobalUtility::CopyColorRawBufToCvMat8uc3 ( (const XnRGB24Pixel *)(g_ImageMD.Data()), g_ColorImgMat );
if ( ctlWndKey == 's' || ctlWndKey == 'S' ) { // Switch
flipColor = !flipColor;
}
if ( flipColor ) {
cv::cvtColor ( g_ColorImgMat, g_ColorImgMat, CV_RGB2BGR );
}
cv::namedWindow ( IMAGE_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::imshow ( IMAGE_WIN_NAME, g_ColorImgMat );
}
#endif
#ifdef HANDLING_DEPTH_DATA
if ( depthFlag )
{
g_DepthGen.GetMetaData(g_DepthMD);
// assert ( g_DepthMD.FullXRes() == g_DepthMD.XRes() );
// assert ( g_DepthMD.FullYRes() == g_DepthMD.YRes() );
GlobalUtility::CopyDepthRawBufToCvMat16u ( (const XnDepthPixel *)(g_DepthMD.Data()), g_DepthImgMat );
GlobalUtility::ConvertDepthCvMat16uToGrayCvMat ( g_DepthImgMat, g_DepthImgShow );
/*
cv::putText(colorImgMat,
GlobalUtility::DoubleToString(g_ImageMD.FPS()) + " FPS",
cv::Point(10, 450),
cv::FONT_ITALIC,
0.7,
cv::Scalar(255, 255, 255, 0),
2,
8,
false);
*/
cv::namedWindow ( DEPTH_WIN_NAME, CV_WINDOW_AUTOSIZE );
cv::imshow ( DEPTH_WIN_NAME, g_DepthImgShow );
}
#endif
XnFieldOfView fov;
g_DepthGen.GetFieldOfView( fov );
std::cout << "HFov = " << fov.fHFOV << std::endl
<< "VFov = " << fov.fVFOV << std::endl;
ctlWndKey = cvWaitKey ( 5 );
g_HeadTrackingFrameCount++;
g_CurrTickCount = GetTickCount();
std::cout << "FPS = "
<< 1000 / ( ( double )( g_CurrTickCount - g_StartTickCount ) / ( double )( g_HeadTrackingFrameCount ) )
<< std::endl;
}
g_Context.Release ();
exit ( EXIT_SUCCESS );
}