reprojectImageTo3D() in OpenCV

问题:

I've been trying to compute real world coordinates of points from a disparity map using the reprojectImageTo3D() function provided by OpenCV, but the output seems to be incorrect.

I have the calibration parameters, and compute the Q matrix using

stereoRectify(left_cam_matrix, left_dist_coeffs, right_cam_matrix, right_dist_coeffs, frame_size, stereo_params.R, stereo_params.T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 0, frame_size, 0, 0);

I believe this first step is correct, since the stereo frames are being rectified properly, and the distortion removal I'm performing also seems all right. The disparity map is being computed with OpenCV's block matching algorithm, and it looks good too.

The 3D points are being calculated as follows:

cv::Mat XYZ(disparity8U.size(),CV_32FC3); reprojectImageTo3D(disparity8U, XYZ, Q, false, CV_32F);

But for some reason they form some sort of cone, and are not even close to what I'd expect, considering the disparity map. I found out that other people had a similar problem with this function, and I was wondering if someone has the solution.

Thanks in advance!

[EDIT]

stereoRectify(left_cam_matrix, left_dist_coeffs, right_cam_matrix, right_dist_coeffs,frame_size, stereo_params.R, stereo_params.T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 0, frame_size, 0, 0);  initUndistortRectifyMap(left_cam_matrix, left_dist_coeffs, R1, P1, frame_size,CV_32FC1, left_undist_rect_map_x, left_undist_rect_map_y); initUndistortRectifyMap(right_cam_matrix, right_dist_coeffs, R2, P2, frame_size, CV_32FC1, right_undist_rect_map_x, right_undist_rect_map_y); cv::remap(left_frame, left_undist_rect, left_undist_rect_map_x, left_undist_rect_map_y, CV_INTER_CUBIC, BORDER_CONSTANT, 0); cv::remap(right_frame, right_undist_rect, right_undist_rect_map_x, right_undist_rect_map_y, CV_INTER_CUBIC, BORDER_CONSTANT, 0);  cv::Mat imgDisparity32F = Mat( left_undist_rect.rows, left_undist_rect.cols, CV_32F );   StereoBM sbm(StereoBM::BASIC_PRESET,80,5); sbm.state->preFilterSize  = 15; sbm.state->preFilterCap   = 20; sbm.state->SADWindowSize  = 11; sbm.state->minDisparity   = 0; sbm.state->numberOfDisparities = 80; sbm.state->textureThreshold = 0; sbm.state->uniquenessRatio = 8; sbm.state->speckleWindowSize = 0; sbm.state->speckleRange = 0;  // Compute disparity sbm(left_undist_rect, right_undist_rect, imgDisparity32F, CV_32F );  // Compute world coordinates from the disparity image cv::Mat XYZ(disparity32F.size(),CV_32FC3); reprojectImageTo3D(disparity32F, XYZ, Q, false, CV_32F); print_3D_points(disparity32F, XYZ);

[EDIT]

Adding the code used to compute 3D coords from disparity:

cv::Vec3f *StereoFrame::compute_3D_world_coordinates(int row, int col,   shared_ptr stereo_params_sptr){   cv::Mat Q_32F;   stereo_params_sptr->Q_sptr->convertTo(Q_32F,CV_32F);  cv::Mat_ vec(4,1);   vec(0) = col;  vec(1) = row;  vec(2) = this->disparity_sptr->at(row,col);   // Discard points with 0 disparity      if(vec(2)==0) return NULL;  vec(3)=1;                vec = Q_32F*vec;  vec /= vec(3);  // Discard points that are too far from the camera, and thus are highly  // unreliable  if(abs(vec(0))>10 || abs(vec(1))>10 || abs(vec(