Measuring distance between 2 points with OpenCV and OpenNI

Question

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 using the correct way to retrieve xyz values and compute distances ?

Any detail I might be missing ?

Answer 1

1. I ran the code but I couldn't make it crash even when I commented out if(sp == 255 && ep == 255). Does it happen often? I haven't worked with the OpenNI drivers, but in the Microsoft drivers for example there is also an invalid value (for example when the surface is specular). There are also different values for 'too near' or 'too far'. Could it be that OpenNI returns garbage in a case like that?

2. Try switching x's and y's:

   Vec3f s = real.at<Vec3f>(startPt.y, startPt.x);        
   Vec3f e = real.at<Vec3f>(endPt.y, endPt.x);        
   

(and consequently:)

unsigned int sp = valid.at<unsigned char>(startPt.y, startPt.x);
unsigned int ep = valid.at<unsigned char>(endPt.y, endPt.x);

The syntax is cv::Mat::at<_Tp>(int y, int x) or if it helps you better cv::Mat::at<_Tp>(int row, int column) I measured some distances and it worked fine for me.

Also, you actually calculate the 3D points and the distance for every frame that you get, is that on purpose? Because even though you choose two 2D points in the image once, the corresponding 3D points that you use to calculate the distance may change in every frame due to noise.