Camera pose estimation from essential matrix - computer-vision

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