openni_cam.cpp

Go to the documentation of this file.

#include "openni_cam.h"

#include <XnOS.h>
#include <math.h>
#include "geom/nmmatrix.h"

#include <XnCppWrapper.h>

namespace openni_cam{

using namespace xn;

//---------------------------------------------------------------------------
// Globals
//---------------------------------------------------------------------------
XnRGB24Pixel* g_pTexMap = NULL;
unsigned int g_nTexMapX = 0;
unsigned int g_nTexMapY = 0;

Context g_context;
ScriptNode g_scriptNode;
DepthGenerator g_depth;
ImageGenerator g_image;
DepthMetaData g_depthMD;
ImageMetaData g_imageMD;

void openni_cam::init() {

    XnStatus rc;

    EnumerationErrors errors;
    rc = g_context.InitFromXmlFile("../config/SamplesConfig.xml", g_scriptNode, &errors);
    if (rc == XN_STATUS_NO_NODE_PRESENT) {
        XnChar strError[1024];
        errors.ToString(strError, 1024);
        printf("%s\n", strError);
        exit(1);
    } else if (rc != XN_STATUS_OK) {
        printf("Open failed: %s\n", xnGetStatusString(rc));
        exit(1);
    }

    rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
    if (rc != XN_STATUS_OK) {
        printf("No depth node exists! Check your XML.");
        exit(1);
    }

    rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
    if (rc != XN_STATUS_OK) {
        printf("No image node exists! Check your XML.");
        exit(1);
    }

    g_depth.GetMetaData(g_depthMD);
    g_image.GetMetaData(g_imageMD);

    // RGB is the only image format supported.
    if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24) {
        printf("The device image format must be RGB24\n");
        exit(1);
    };

    int w_rgb = g_imageMD.XRes();
    int h_rgb = g_imageMD.YRes();
    int w_depth = g_depthMD.XRes();
    int h_depth = g_depthMD.YRes();

    rgb.resize(exttype::mint3(3,w_rgb,h_rgb));
    depth.resize(exttype::mint2(w_depth,h_depth));
    g_depth.GetAlternativeViewPointCap().SetViewPoint(g_image);

    // get kinect inv proj matrix
    geom::matrix3 Kinv;
    Kinv <<0;
    XnPoint3D Point2D, Point3D;
    Point2D.X = 0;
    Point2D.Y = 0;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,2) = Point3D.X;
    Kinv(1,2) = Point3D.Y;
    Kinv(2,2) = 1;
    Point2D.X = 1;
    Point2D.Y = 0;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,0) = Point3D.X-Kinv(0,2);
    Kinv(1,0) = Point3D.Y-Kinv(1,2);
    Point2D.X = 0;
    Point2D.Y = 1;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,1) = Point3D.X-Kinv(0,2);
    Kinv(1,1) = Point3D.Y-Kinv(1,2);
    //
    //K = Kinv.invert();
/*
    K << 0;
    Point3D.X = 0;
    Point3D.Y = 0;
    Point3D.Z = 1;
    g_depth.ConvertRealWorldToProjective(1, &Point3D, &Point2D);
    Point2D.X = Point2D.X/
    K(0,2) = Point2D.X;
    K(1,2) = Point2D.Y;
    K(2,2) = Point2D.Z;
    Point3D.X = 1;
    Point3D.Y = 0;
    Point3D.Z = 1;
    g_depth.ConvertRealWorldToProjective(1, &Point3D, &Point2D);
    K(0,0) = Point2D.X-K(0,2);
    K(1,0) = Point2D.Y-K(1,2);
    Point3D.X = 0;
    Point3D.Y = 1;
    Point3D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point3D, &Point2D);
    K(0,1) = Point2D.X-K(0,2);
    K(1,1) = Point2D.Y-K(1,2);
*/
    std::ofstream f("../config/Kinv.txt");
    if(!f.is_open()) {
        throw debug_exception("cant create ../config/Kinv.txt");
    };
    for(int i=0; i<3; ++i) {
        for(int j=0; j<3; ++j) {
            f<<Kinv(i,j);
            if(j<3)f<<" ";
        };
        if(i<3)f<<"\n";
    };

    Kinv <<0;
    Point2D.X = 0;
    Point2D.Y = 0;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,2) = Point3D.X;
    Kinv(1,2) = -Point3D.Y;
    Kinv(2,2) = 1;
    Point2D.X = 1;
    Point2D.Y = 0;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,0) = Point3D.X-Kinv(0,2);
    Kinv(1,0) = -Point3D.Y-Kinv(1,2);
    Point2D.X = 0;
    Point2D.Y = 1;
    Point2D.Z = 1;
    g_depth.ConvertProjectiveToRealWorld(1, &Point2D, &Point3D);
    Kinv(0,1) = Point3D.X-Kinv(0,2);
    Kinv(1,1) = -Point3D.Y-Kinv(1,2);
    K = Kinv.invert();
};


bool openni_cam::get_image() {
    XnStatus rc = XN_STATUS_OK;
    // Read a new frame
    rc = g_context.WaitAnyUpdateAll();
    if (rc != XN_STATUS_OK) {
        printf("Read failed: %s\n", xnGetStatusString(rc));
        return false;
    };

    g_depth.GetMetaData(g_depthMD);
    g_image.GetMetaData(g_imageMD);

    const XnDepthPixel* pDepth = g_depthMD.Data();
    const XnUInt8* pImage = g_imageMD.Data();

    memcpy(depth.begin(),pDepth,depth.length()*sizeof(unsigned short));
    memcpy(rgb.begin(),pImage,rgb.length());
    return true;
};
};