Оценка позы камеры по основной матрице

Question

Я пытаюсь оценить движение камеры по паре изображений. Я нашел существенную матрицу E и разложил ее на элементы вращения и перемещения. Вот код C++:

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

Функция infrontOfBothCameras проверяет, находятся ли точки перед камерой.

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;

}

После я хочу оценить новую позу камеры. Как я могу использовать T и R для этого? Например, у меня есть старая поза камеры: old_pose=(0,0,0), и я пытаюсь вычислить новую позу:

new_pose = old_pose + R * t

Это правильно?

Answer 1

Я считаю, что это должно быть:

new_pose = R*(old_pose-t);

Остальное выглядит хорошо, но я не проверял каждую мелочь.

Если вы хотите, чтобы ссылку для сравнения, вы можете посмотреть на: https://github.com/MasteringOpenCV/code/blob/master/Chapter4_StructureFromMotion/FindCameraMatrices.cpp

Конкретно функции DecomposeEtoRandT а также FindCameraMatrices