Get 3d point from clicked depth map pixel?
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!