Ask Your Question
0

How can I measure distances with OpenNI ?

asked 2012-08-14 04:27:23 -0500

George Profenza gravatar image

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

edit retag flag offensive close merge delete

1 answer

Sort by ยป oldest newest most voted
3

answered 2012-08-16 04:22:03 -0500

George Profenza gravatar image

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 <iostream>

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<Vec3f>(startPt.y, startPt.x);
        Vec3f e = world.at<Vec3f>(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 << "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(;;){
        if( !capture.grab() ){
            cout << "Can not grab images." << 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 ) >= 0 )    break;
    }
}
edit flag offensive delete link more
Login/Signup to Answer

Question Tools

Stats

Asked: 2012-08-14 04:27:23 -0500

Seen: 1,766 times

Last updated: Aug 16 '12