SolvePNP через некоторое время возвращает плохие rvec и tvec

Question

Я пытаюсь оценить траекторию камеры, используя пару стереокамер из набора данных KITTI. В какой-то момент программа использует cv::SolvePNP(), и для первых 1500 кадров она возвращает довольно хорошие результаты, но после этого она становится совершенно дикой.

Вот что я сейчас делаю:

1. загрузить 2 кадра, назовем их P и Q (один кадр слева и справа от стереопары)
2. получить функции, используя SIFT/SURF/ORB (пробовал все это)
3. сопоставить особенности в P и совпадать между P.left и Q.left
4. на основе совпадений я отфильтровываю ключевые точки, которых нет в совпадениях.
5. Я триангулирую точки на кадре P (обратите внимание, что у меня уже есть матрица камеры, предоставленная набором данных KITTI)
6. Я конвертирую точки из Гомогена

Теперь перейдем к острой части, я пытаюсь реализовать свой собственный RANSAC, который будет использовать solvePNP, чтобы получить лучшие значения rvec и tvec из стандартного cv::solvePNP (обратите внимание, что я пытался использовать cv::solvePNPRansac, и он работал нормально, но начал не удалось через некоторое время, а) поэтому процесс Ransac выглядит так:

7.1 Я рандомизирую трехмерное пространство (триангулированные точки на P) и соответствующие ключевые точки на Q.left

7.2 Я помещаю их N точек (N - это мой параметр RANSAC) в cv::SolvePNP(), чтобы узнать, сколько камер перемещается между 2 кадрами (обратите внимание, что мои dist Coefs - нулевая матрица), и это возвращает мне некоторые значения tvec и rvec что я преобразую в матрицу с помощью cv:: Rodrigues ()

7.3 Я конкатенирую эти векторы

7.4 Я пытаюсь спроецировать все триангулированные точки (не только те, которые используются в solvePNP) обратно на 2D-изображение, используя следующую формулу:

https://i.imgur.com/aMlZw9z.png

(обратите внимание, что я не получаю 1 в левой части, но вместо этого я получаю некоторые значения x,y,z, поэтому я делю x и y на z).

7.5 Я нахожу разницу между проецируемыми точками и ключевыми точками на P.left

7.6 теперь у моего RANSAC есть какое-то пороговое значение, которое я использую, чтобы отфильтровать выбросы и не использовать их в следующей итерации RANSAC.

7.7 Я повторяю процесс с новыми (отфильтрованными) точками несколько раз K (K также является параметром RANSAC)

Теперь это работает для 80% фреймов, через некоторое время он просто терпит неудачу, потому что решение PNP получает слишком мало очков. Это происходит потому, что ни одна из проецируемых точек на самом деле не прошла порог расстояния. Так что проблема в rvec и tvec из кадра раньше.

Действительно не знаю, какую часть кода я покажу, запросю и напишу.

Я ожидал незначительную ошибку в rvec и tvec, но ошибка слишком высока.

Answer 1

По-видимому, это произошло из-за множества неправильно подобранных функций, которые сильно повлияли на последующий процесс триангуляции. Проблема может быть решена путем внедрения RANSAC или повышения порога совпадения.