Использование OpenCV для создания трехмерных точек (при условии фронтальной параллельной конфигурации)

В настоящее время я пытаюсь создать 3D-точки с учетом пары стереоизображений в OpenCV. Это было сделано совсем немного, насколько я могу искать.

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

Я рассчитал псевдо-приличную карту несоответствий, используя StereoSGBM, и вручную закодировал матрицу Q, следуя книге O'Reilly Learning OpenCV, в которой указано:

Q = [ 1 0 0      -c_x
      0 1 0      -c_y
      0 0 0      f
      0 0 -1/T_x (c_x - c_x')/T_x ]

Я возьму, что (c _x, c _y )— главная точка (, которую я указал в координатах изображения ), f — фокусное расстояние (, которое я описал в мм ), а T _x — перевод между двумя камерами или базовая линия (, которую я также описал в мм ).

int type = CV_STEREO_BM_BASIC;
double rescx = 0.25, rescy = 0.25;
Mat disparity, vdisparity, depthMap;

Mat frame1 = imread( "C:\\Users\\Administrator\\Desktop\\Flow\\IMG137.jpg", CV_LOAD_IMAGE_GRAYSCALE );
Mat frame1L = frame1( Range( 0, frame1.rows ), Range( 0, frame1.cols/2 ));
Mat frame1R = frame1( Range( 0, frame1.rows ), Range( frame1.cols/2, frame1.cols ));

resize( frame1L, frame1L, Size(), rescx, rescy );
resize( frame1R, frame1R, Size(), rescx, rescy );

int preFilterSize = 9, preFilterCap = 32, disparityRange = 4;
int minDisparity = 2, textureThreshold = 12, uniquenessRatio = 3;
int windowSize = 21, smoothP1 = 0, smoothP2 = 0, dispMaxDiff = 32;
int speckleRange = 0, speckleWindowSize = 0;

bool dynamicP = false;

StereoSGBM stereo( minDisparity*-16, disparityRange*16, windowSize,
    smoothP1, smoothP2, dispMaxDiff,
    preFilterCap, uniquenessRatio,
    speckleRange*16, speckleWindowSize, dynamicP );

stereo( frame1L, frame1R, disparity );

double m1[3][3] = { { 46, 0, frame1L.cols/2 }, { 0, 46, frame1L.rows/2 }, { 0, 0, 1 } };
double t1[3] = { 65, 0, 0 };
double q[4][4] = {{ 1, 0, 0, -frame1L.cols/2.0 }, { 0, 1, 0, -frame1L.rows/2.0 }, { 0, 0, 0, 46 }, { 0, 0, -1.0/65, 0 }};
Mat cm1( 3, 3, CV_64F, m1), cm2( 3, 3, CV_64F, m1), T( 3, 1, CV_64F, t1 );
Mat R1, R2, P1, P2;
Mat Q( 4, 4, CV_64F, q );

//stereoRectify( cm1, Mat::zeros( 5, 1, CV_64F ), cm2, Mat::zeros( 5, 1, CV_64F ),  frame1L.size(), Mat::eye( 3, 3, CV_64F ), T, R1, R2, P1, P2, Q ); 

normalize( disparity, vdisparity, 0, 256, NORM_MINMAX );
//convertScaleAbs( disparity, disparity, 1/16.0 );
reprojectImageTo3D( disparity, depthMap, Q, true );
imshow( "Disparity", vdisparity );
imshow( "3D", depthMap );

Поэтому я передаю полученную карту несоответствий из StereoSGBM и эту матрицу Q, чтобы получить 3D-точки, которые я записываю в файл слоя.

Но результат таков:http://i.stack.imgur.com/7eH9V.png

Интересно посмотреть, но не то, что мне нужно :(. Я читал в Интернете, что он получает лучшие результаты после деления карты несоответствия на 16, и действительно, он выглядел немного лучше (, на самом деле похоже, что это была камера, которая сделала снимок! ).

Это моя карта несоответствий, если вам интересно:http://i.stack.imgur.com/lNPkO.png

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

Какие-либо предложения?