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

Я пытаюсь оценить движение камеры по паре изображений. Я нашел основную матрицу 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

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


computer-vision pose-estimation
person anton.kryshchenko    schedule 10.04.2015    source источник

Ответы (1)


arrow_upward
0
arrow_downward

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

new_pose = R*(old_pose-t);

В остальном все нормально, но я не проверял каждую мелочь.

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

В частности, функции DecomposeEtoRandT и FindCameraMatrices

person Photon    schedule 12.04.2015
comment
Спасибо за ответ. Но при освоении книги OpenCV проверка того, находятся ли точки перед камерами, выполняется с помощью триангуляции. Выше я пытаюсь проверить это, используя этот подход ответы .opencv.org / question / 27155 / - person anton.kryshchenko; 14.04.2015
comment
Я провел несколько экспериментов и считаю, что моя функция infrontOfBothCameras некорректна, потому что она очень часто определяет неправильную декомпозицию SVD (точки позади камер во всех 4 случаях). Но я не знаю, как это исправить. @Фотон - person anton.kryshchenko; 14.04.2015
comment
Я не знаю способа без триангуляции. Если у вас все заработало, напишите здесь. - person Photon; 14.04.2015