Ask Your Question
0

Get 3d point from clicked depth map pixel?

asked 2019-11-28 11:02:21 -0600

antithing gravatar image

I have a depth map, from the Zed stereo camera. I need a function that gives me the 3d point relative to the camera, at a mouse clicked location.

I have a cv::setMouseCallback to get the clicked pixel, and then I run the following: (depth is the depth map, stored outside the function).

static void onMouse(int event, int x, int y, int, void*)
{
    if (event != cv::EVENT_LBUTTONDOWN)
        return;

    cv::Point seed = cv::Point(x, y);
    cv::Point3d pnt = RGBDtoPoint(depth, seed);

    std::cout << pnt << std::endl;
}

cv::Point3d RGBDtoPoint(cv::Mat depth_image, cv::Point2d pnt)
{

    float fx = 700.251;
    float fy = 700.251;
    float cx = 645.736;
    float cy = 344.305;

    depth_image.convertTo(depth_image, CV_32F); // convert the image data to float type 

    if (!depth_image.data) {
        std::cerr << "No depth data!!!" << std::endl;
        exit(EXIT_FAILURE);
    }       
        float Z = depth_image.at<float>(pnt.y, pnt.x);
        cv::Point3d p;
        if (Z != NULL)
        {       
            p.z = Z;
            p.x = (pnt.x - cx) * Z / fx;
            p.y = (pnt.y - cy) * Z / fy;

            p.z = p.z;
            p.x = p.x;
            p.y = p.y;
        }
        else
        {
            std::cout << "NOT VALID POINT " << std::endl;
        }       

    return p;
}

This runs and works, but the values I get are incorrect, the Z or distance value is just the 0 - 255 pixel value, it seems. Where am I going wrong?

Thanks!

edit retag flag offensive close merge delete

1 answer

Sort by ยป oldest newest most voted
1

answered 2019-11-29 06:46:56 -0600

kbarni gravatar image

Your depth_image is already normalized to 0-255 range. Converting it to CV_32F won't change the values.

You probably need to check the capturing part and the data format of the depth image. I don't know the Zed stereo cameras, but probably you have to set the depth resolution to millimeters before starting the capture.

Convert the captured buffer to a 16 bit cv::Mat (probably CV_16U type), let's call it depth_data. Then normalize to a depth_image, which will be used for display. In the RGBDtoPoint use the depth_data to get the real Z distance (in mm).

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2019-11-28 11:02:21 -0600

Seen: 1,055 times

Last updated: Nov 29 '19