I am trying to generate 3D world coordinates using stereo vision.
I am getting disparity map, here is how I am processing it.
left_ret, leftFrame = left_camera.read()
fixedLeft = cv2.remap(leftFrame, leftMapX, leftMapY, cv2.INTER_LINEAR)
fixedRight = cv2.remap(rightFrame, rightMapX, rightMapY, cv2.INTER_LINEAR)
grayLeft = cv2.cvtColor(fixedLeft, cv2.COLOR_BGR2GRAY)
grayRight = cv2.cvtColor(fixedRight, cv2.COLOR_BGR2GRAY)
disparity = stereoMatcher.compute(grayLeft, grayRight).astype(np.float32)/16.0
image_publisher.publish(bridge.cv2_to_imgmsg(disparity.astype(np.float32), "32FC1"))
And using the disparity map, I am using cv::reprojectImageTo3D to get 3D Coordinates.
pcl::PointXYZ point;
try {
disparity_map = cv_bridge::toCvShare(msg, "32FC1")->image;
cv::Mat depth_image = cv::Mat::zeros(disparity_map.size().height, disparity_map.size().width, CV_32FC3);
cv::reprojectImageTo3D(disparity_map, depth_image, Q);
for (int i = 0; i < depth_image.rows; i++) {
for (int j = 0; j < depth_image.cols; j++) {
auto intensity = depth_image.at<cv::Vec3f>(i, j);
std::cout << intensity << std::endl;
}
}
Problem
It returns
[-27.7079, 2.54654e+23, -0.469625]
[-27.7079, 2.54654e+23, -0.469625]
[-27.7079, 6.43e+24, -0.469625]
[-27.7079, 6.43e+24, -0.469625]
[-27.7079, 6.43e+24, -0.469625]
[-27.7079, 6.4247e+24, -0.469625]
[-27.7079, 6.4247e+24, -0.469625]
[-27.7079, 6.4247e+24, -0.469625]
[-27.7079, 6.41939e+24, -0.469625]
[-27.7079, 6.4247e+24, -0.469625]
[-27.7079, 6.43e+24, -0.469625]
[-27.7079, 2.54654e+23, -0.469625]
Now, the magnitude of these values doesn't makes sense. What exactly am I doing wrong?