Я решаю задачу VSLAM, используя алгоритм 2d-3d на основе библиотеки OpenCV. Сейчас пытаюсь сделать геопривязку по данным GPS. Я преобразовываю R, t каждой камеры, а затем триангулирую совпадающие точки, используя тривиальную функцию
Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) {
cv::Mat A(4,4,CV_32F);
A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
cv::Mat u,w,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at<float>(3); }
, где kp1 и kp2 - характерные точки на левом-правом изображении, P1, P2 - проекционные матрицы
Я столкнулся со странной проблемой: когда я делаю простое смещение центров камер на какую-то огромную константу, у меня возникают большие ошибки перепроецирования на старых подходящих триангулированных точках. Чувствительна ли декомпозиция SVD для триангуляции точек к масштабу центров камер?
