Using RTABMap and Kinect, how do you get distance at a certain point?

asked 2016-04-18 12:34:50 -0500

michaelrosario
1 ●1 ●1 ●1

Hi.

I appreciate all the efforts and amazing work in the ROS community. In my situation, I am trying to estimate a distance to an object using RTABMap, ROS, and a Kinect 360. For my implementation, we are using ROS indigo and libfreenect .

In our system, we are able to display the field of view of the robot to a web page using roslibjs. When the user clicks on a certain (x,y) coordinate on that video stream, we want to obtain an estimated distance to the object.

Is there an API in RTABMap/ROS that can help me obtain this information easily?

It looks like the information should be available in the following topic: /camera/depth_registered/image_raw - Raw image from device. Contains uint16 depths in mm.

I'm not sure how I might extract/read the information.

I do appreciate your help and pointers.

All the best!

answered 2016-04-20 07:49:17 -0500

matlabbe

6409 ●11 ●96 ●80

Hi,

You can use cv_bridge to get the image in cv::Mat format, then you get directly the distance at the clicked pixel:

void callback(
    const sensor_msgs::ImageConstPtr& depth,
    const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
    cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(depth);

    float distance = 0.0f; // meters
    if(image.type() == CV_32FC1)
    {
        distance = ptr->image.at<float>(y, x);
    }
    else // CV_16UC1
    {
        distance = ptr->image.at<unsigned short>(y, x)/1000.0f; // convert from mm to meters
    }
    ...

If you want the 3D point in Camera referential, you can look at image_geometry::PinholeCameraModel class:

image_geometry::PinholeCameraModel model;
model.fromCameraInfo(cameraInfo);
cv::Point3d ray = model.projectPixelTo3dRay(cv::Point2d(x,y));
cv::Point3d pt3d = ray * distance;

cheers

edit flag offensive delete link

Comments

Hi. I do appreciate the answer! Thank you for your time!!!

michaelrosario ( 2016-04-20 08:38:18 -0500 )edit

add a comment

Using RTABMap and Kinect, how do you get distance at a certain point?

1 Answer

Comments

Question Tools

Stats

Related questions

Using RTABMap and Kinect, how do you get distance at a certain point? edit

1 Answer

Comments

Question Tools

Stats

Related questions

Using RTABMap and Kinect, how do you get distance at a certain point?