Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version


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-><float>(y, x);
    else // CV_16UC1
        distance = ptr-><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;
cv::Point3d ray = model.projectPixelTo3dRay(cv::Point2d(x,y));
cv::Point3d pt3d = ray * distance;