How can I measure distances with OpenNI ?

edit

Hello, I'm playing with the built in OpenNI access within OpenCV 2.4.0 and I'm trying to measure the distance between two points in the depth map. I've tried this so far:

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <iostream>

using namespace cv;
using namespace std;

Point startPt(0,0);
Point endPt(0,0);

void onMouse( int event, int x, int y, int flags, void* )
{
    if( event == CV_EVENT_LBUTTONUP) startPt = Point(x,y);
    if( event == CV_EVENT_RBUTTONUP) endPt   = Point(x,y);
}

int main( int argc, char* argv[] ){
    VideoCapture capture;
    capture.open(CV_CAP_OPENNI);
    //capture.set( CV_CAP_PROP_OPENNI_REGISTRATION , 0);
    unsigned t0=clock();
    if( !capture.isOpened() ){
        cout << "Can not open a capture object." << endl;
        return -1;
    }
    unsigned elapsed=clock()-t0;
    cout << "initialized in "<< elapsed <<" s. ready!" << endl;
    namedWindow( "depth", 1 );
    setMouseCallback( "depth", onMouse, 0 );
    for(;;){
        Mat depthMap;
        if( !capture.grab() ){
            cout << "Can not grab images." << endl;
            return -1;
        }else{
            Mat show,real,valid;
            if( capture.retrieve( depthMap, CV_CAP_OPENNI_DEPTH_MAP ) ){
                depthMap.convertTo( show, CV_8UC1, 0.05f);
            }
            capture.retrieve(valid,CV_CAP_OPENNI_VALID_DEPTH_MASK);
            if( capture.retrieve(real, CV_CAP_OPENNI_POINT_CLOUD_MAP)){
                unsigned int sp = valid.at<unsigned char>(startPt.x, startPt.y);
                unsigned int ep = valid.at<unsigned char>(endPt.x, endPt.y);
                if(sp == 255 && ep == 255){
                    Vec3f s = real.at<Vec3f>(startPt.x, startPt.y);
                    Vec3f e = real.at<Vec3f>(endPt.x, endPt.y);
                    float dx = e[0]-s[0];
                    float dy = e[1]-s[1];
                    float dz = e[2]-s[2];
                    float dist = sqrt(dx*dx + dy*dy + dz*dz);
                    putText(show,format("distance: %f m\n",dist),Point(10,10),FONT_HERSHEY_PLAIN,1,Scalar(255));
                }
            }
            circle(show,startPt,3,Scalar(255),3);
            circle(show,endPt,3,Scalar(192),3);
            line(show,startPt,endPt,Scalar(128));
            imshow("depth",show);
        }
        if( waitKey( 30 ) >= 0 )    break;
    }

}

but I have a few issues:

1. Unless I check for values in the valid depth mask first the program crashes sometimes (I imagine due to bad data)
2. As far as I understand the point cloud map returns XYZ data in meters (CV_32FC3), but the numbers I see on screen look wrong.

Am I retrieving the xyz values and doing the distance calculations correctly ? What am I missing out ?

Thanks, George