Camera pose estimation from essential matrix

Question

I try to estimate the camera motion from pair of images. I found essential matrix E and decomposed it into the rotation and translation elements. Here is the C++ code:

cv::SVD svd(E);
cv::Matx33d W{0, -1, 0, 1, 0 , 0, 0, 0, 1};
cv::Mat_<double> R = svd.u * cv::Mat(W) * svd.vt;
cv::Mat_<double> t = svd.u.col(2);

if (!infrontOfBothCameras(inliers[0], inliers[1], R, t)) {                  
    t = -svd.u.col(2);
    if (!posEstimator.infrontOfBothCameras(inliers[0], inliers[1], R, t)) {
        R = svd.u * cv::Mat(W.t()) * svd.vt;
        t = svd.u.col(2);
        if (!infrontOfBothCameras(inliers[0], inliers[1], R, t)) {
            t = -svd.u.col(2);
            if (!infrontOfBothCameras(inliers[0], inliers[1], R, t)) {
                std::cout << "Incorrect SVD decomposition" << std::endl;
            }
        }
    }
}

function infrontOfBothCameras check if points are in front of the camera.

bool infrontOfBothCameras(std::vector<cv::Point2f>& points1, std::vector<cv::Point2f>& points2, cv::Mat_<double>& R, cv::Mat_<double>& t) {
cv::Mat r1 = R.row(0);
cv::Mat r2 = R.row(1);
cv::Mat r3 = R.row(2);

for (size_t i = 0; i < points1.size(); ++i) {
    cv::Matx13d uv{ points2[i].x, points2[i].y, 1 };

    double z = (r1 - points2[i].x * r3).dot(t.t()) / ((r1 - points2[i].x * r3).dot(cv::Mat_<double>(uv)));

    cv::Matx31d point3d_first{points1[i].x * z, points1[i].y * z, z};
    cv::Mat_<double> point3d_second = R.t() * (cv::Mat_<double>(point3d_first) - t);
    if (point3d_first(2) < 0 || point3d_second(2) < 0) {
        return false;
    }
}
return true;

}

After I wish to estimate new pose of camera. How I can use t and R for it? For example, i have old pose of camera: old_pose=(0,0,0) and i try to calculate new pose:

new_pose = old_pose + R * t

Is it correct?

Answer 1

I believe it should be:

new_pose = R*(old_pose-t);

The rest looks ok, but I haven't checked every little detail.

If you want a reference to compare to, you can look at: https://github.com/MasteringOpenCV/code/blob/master/Chapter4_StructureFromMotion/FindCameraMatrices.cpp

Specifically functions DecomposeEtoRandT and FindCameraMatrices