Measuring with OpenCV and OpenNI 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 &lt;iostream&gt;

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 &lt;&lt; "Can not open a capture object." &lt;&lt; endl;
        return -1;
    }
    unsigned elapsed=clock()-t0;
    cout &lt;&lt; "initialized in "&lt;&lt; elapsed &lt;&lt;" s. ready!" &lt;&lt; endl;
    namedWindow( "depth", 1 );
    setMouseCallback( "depth", onMouse, 0 );
    for(;;){
        Mat depthMap;
        if( !capture.grab() ){
            cout &lt;&lt; "Can not grab images." &lt;&lt; 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&lt;unsigned char&gt;(startPt.x, startPt.y);
                unsigned int ep = valid.at&lt;unsigned char&gt;(endPt.x, endPt.y);
                if(sp == 255 &amp;&amp; ep == 255){
                    Vec3f s = real.at&lt;Vec3f&gt;(startPt.x, startPt.y);
                    Vec3f e = real.at&lt;Vec3f&gt;(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 ) &gt;= 0 )    break;
    }

}


but I have a few issues:


Unless I check for values in the valid depth mask first the program
crashes sometimes (I imagine due to bad data)
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 ?
Coordinates should be passed in y,x order, not x,y.
Full updated code:

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

#include &lt;iostream&gt;

using namespace cv;
using namespace std;

Mat world;
Point startPt(0,0);
Point endPt(0,0);
float dist;

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);
        Vec3f s = world.at&lt;Vec3f&gt;(startPt.y, startPt.x);
        Vec3f e = world.at&lt;Vec3f&gt;(endPt.y, endPt.x);
        float dx = e[0]-s[0];
        float dy = e[1]-s[1];
        float dz = e[2]-s[2];
        dist = sqrt(dx*dx + dy*dy + dz*dz);
    }
}

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 &lt;&lt; "Can not open a capture object." &lt;&lt; endl;
        return -1;
    }
    unsigned elapsed=clock()-t0;
    cout &lt;&lt; "initialized in "&lt;&lt; elapsed &lt;&lt;" s. ready!" &lt;&lt; endl;
    namedWindow( "depth", 1 );
    setMouseCallback( "depth", onMouse, 0 );
    for(;;){
        if( !capture.grab() ){
            cout &lt;&lt; "Can not grab images." &lt;&lt; endl;
            return -1;
        }else{
            Mat depthMap,show;
            capture.retrieve(world, CV_CAP_OPENNI_POINT_CLOUD_MAP);
            if( capture.retrieve( depthMap, CV_CAP_OPENNI_DEPTH_MAP ) ) depthMap.convertTo( show, CV_8UC1, 0.05f);
            line(show,startPt,endPt,Scalar(255));
            putText(show,format("distance: %f m",dist),Point(5,15),FONT_HERSHEY_PLAIN,1,Scalar(255));
            imshow("depth",show);
        }
        if( waitKey( 30 ) &gt;= 0 )    break;
    }
}