OpenCV undistortPoints и triangulatePoint дают странные результаты (стерео)

Я пытаюсь получить 3D-координаты нескольких точек в пространстве, но получаю странные результаты как от undistortPoints(), так и от triangulatePoints().

Поскольку обе камеры имеют разное разрешение, я откалибровал их отдельно, получил среднеквадратичные ошибки 0,34 и 0,43, затем использовал stereoCalibrate() для получения дополнительных матриц, получил среднеквадратичное значение 0,708, а затем использовал stereoRectify() для получения оставшихся матриц. Имея это в руках, я начал работу по сбору координат, но получил странные результаты.

Например, ввод: (935, 262), а вывод undistortPoints() — (1228.709125, 342.79841) для одной точки, а для другой — (934, 176) и (1227.9016, 292.4686) соответственно. Что странно, ведь обе эти точки находятся очень близко к середине кадра, где искажения минимальны. Я не ожидал, что это переместит их на 300 пикселей.

При переходе к traingulatePoints() результаты становятся еще более странными - я измерил расстояние между тремя точками в реальной жизни (линейкой) и вычислил расстояние между пикселями на каждой картинке. Поскольку на этот раз точки находились на довольно плоской плоскости, эти две длины (пиксельная и реальная) совпадали, как в |AB|/|BC| в обоих случаях было около 4/9. Тем не менее, triangulatePoints() дает мне результаты с рельсов, с |AB|/|BC| быть 3/2 или 4/2.

Это мой код:

double pointsBok[2] = { bokList[j].toFloat()+xBok/2, bokList[j+1].toFloat()+yBok/2 };
cv::Mat imgPointsBokProper = cv::Mat(1,1, CV_64FC2, pointsBok);

double pointsTyl[2] = { tylList[j].toFloat()+xTyl/2, tylList[j+1].toFloat()+yTyl/2 };
//cv::Mat imgPointsTyl = cv::Mat(2,1, CV_64FC1, pointsTyl);
cv::Mat imgPointsTylProper = cv::Mat(1,1, CV_64FC2, pointsTyl);

cv::undistortPoints(imgPointsBokProper, imgPointsBokProper, 
      intrinsicOne, distCoeffsOne, R1, P1);
cv::undistortPoints(imgPointsTylProper, imgPointsTylProper, 
      intrinsicTwo, distCoeffsTwo, R2, P2);

cv::triangulatePoints(P1, P2, imgWutBok, imgWutTyl, point4D);

double wResult = point4D.at<double>(3,0);
double realX = point4D.at<double>(0,0)/wResult;
double realY = point4D.at<double>(1,0)/wResult;
double realZ = point4D.at<double>(2,0)/wResult;

Углы между точками вроде как хороши, но обычно это не так:

`7,16816    168,389 4,44275` vs `5,85232    170,422 3,72561` (degrees)
`8,44743    166,835 4,71715` vs `12,4064    158,132 9,46158`
`9,34182    165,388 5,26994` vs `19,0785    150,883 10,0389`

Я пытался использовать undistort() для всего кадра, но результаты были такими же странными. Расстояние между точками B и C должно практически не меняться все время, и все же это то, что я получаю:

7502,42     
4876,46 
3230,13 
2740,67 
2239,95 

Покадровый.

Расстояние в пикселях (внизу) и реальное расстояние (вверху) — должно быть очень похоже:

Угол:

Кроме того, разве undistortPoints() и undistort() не должны давать одинаковые результаты (другой набор видео здесь)?