Revision history [back]

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!