ReprojectImageTo3D() в OpenCV

Я пытаюсь вычислить реальные точки мира из карты несоответствия, используя функцию repjectImageTo3D(), предоставленную OpenCV, но результат кажется неправильным.

У меня есть параметры калибровки и вычисляю Q-матрицу, используя

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);

Я считаю, что этот первый шаг правильный, поскольку стереокадры исправляются правильно, и удаление искажений, которое я выполняю, также кажется правильным. Карта несоответствий вычисляется с помощью алгоритма сопоставления блоков OpenCV, и он тоже выглядит хорошо.

Трехмерные точки вычисляются следующим образом:

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

Но по какой-то причине они образуют какой-то конус и даже не близки к тому, что я ожидаю, учитывая карту несоответствия. Я узнал, что у других людей была аналогичная проблема с этой функцией, и мне было интересно, есть ли у кого-то решение.

Спасибо заранее!

[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]

Добавление кода, используемого для вычисления трехмерных коордов из-за несоответствия:

cv::Vec3f *StereoFrame::compute_3D_world_coordinates(int row, int col,
  shared_ptr<StereoParameters> stereo_params_sptr){

 cv::Mat Q_32F;

 stereo_params_sptr->Q_sptr->convertTo(Q_32F,CV_32F);
 cv::Mat_<float> vec(4,1);

 vec(0) = col;
 vec(1) = row;
 vec(2) = this->disparity_sptr->at<float>(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(2))>10) return NULL;

 cv::Vec3f *point3f = new cv::Vec3f();
 (*point3f)[0] = vec(0);
 (*point3f)[1] = vec(1);
 (*point3f)[2] = vec(2);

    return point3f;
}

Ответы

Ответ 1

Ваш код кажется мне хорошим. Это может быть ошибка с reprojectImageTo3D. Попробуйте заменить его следующим кодом (который имеет ту же роль):

cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols);   // Output point cloud
cv::Mat_<float> vec_tmp(4,1);
for(int y=0; y<disparity32F.rows; ++y) {
    for(int x=0; x<disparity32F.cols; ++x) {
        vec_tmp(0)=x; vec_tmp(1)=y; vec_tmp(2)=disparity32F.at<float>(y,x); vec_tmp(3)=1;
        vec_tmp = Q*vec_tmp;
        vec_tmp /= vec_tmp(3);
        cv::Vec3f &point = XYZ.at<cv::Vec3f>(y,x);
        point[0] = vec_tmp(0);
        point[1] = vec_tmp(1);
        point[2] = vec_tmp(2);
    }
}

Я никогда не использовал reprojectImageTo3D, однако я успешно использую код, похожий на фрагмент выше.

[Исходный ответ]

Как объясняется в документации для StereoBM, если вы запрашиваете карту несоответствий CV_16S, вам необходимо разделить каждое несоответствие значение на 16 перед их использованием.

Следовательно, перед его использованием вы должны преобразовать карту несоответствия следующим образом:

imgDisparity16S.convertTo( imgDisparity32F, CV_32F, 1./16);

Вы также можете напрямую запросить карту несоответствия CV_32F из структуры StereoBM, и в этом случае вы можете получить истинное несоответствие.