Huge reprojection errors triangulating appropriate points on stereopair with additional GPS data

Question

I'm solving VSLAM task using 2d-3d algo based on OpenCV library. Now I'm trying to make georeferencing using GPS data. I transform R, t of each camera and then triangulate matched points using trivial function

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

, where kp1 and kp2 - keypoint on left-right image, P1, P2 - projection matrices

I have faced strange problem: when I'm making simple shift for cameras centers by some huge constant, I've got big reprojection errors on old suitable triangulated points. Is SVD decomposition for points triangulation sensitive to cameras centers scale?

Answer 1

Just mistake in another part of code. Sorry.