Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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