Петля, закрывающая 3D Point Clouds
Я хочу понять, как реализовать простой алгоритм закрытия цикла.
Вот моя ситуация: у меня есть x чисел облаков точек, и я использовал алгоритм регистрации в 3d, который дал мне позу всех этих облаков точек.
В конце концов, я более или менее оказываюсь в той же точке на моей карте, но со смещением. Я могу использовать свой алгоритм регистрации, чтобы увидеть, где находится мое действительное облако конечных точек относительно моего начального.
Зная это, я хотел бы глобально оптимизировать остальные мои облака точек вплоть до моего первоначального, основываясь на "дрейфе", который я рассчитал.
Мне удалось быстро написать что-то в отношении перевода, что кажется правильным, но вращение проблематично, так как точность / совмещение элементов (стен и т. Д.) Снижается.
То, на что я обращал внимание: библиотеки g2o, GTSAM, ISAM, все стремятся оптимизировать, но я чувствую, что их реализация требует огромных затрат, все они требуют множества ограничений, установки огромного количества параметров и т. Д.
Я даже не хочу автоматически обнаруживать петли (я сделаю это позже), я просто хотел бы сделать: эти два облака точек представляют петлю, распространяют (правильно) дрейф в перемещении и вращении (которое я вычисляю) между ними ко всем точечным облакам между двумя.
Заранее спасибо,
2 ответа
Я ждал этого вопроса. В моей магистерской программе я разработал именно эту функцию глобальной оптимизации для уточнения поворотов в случаях замыкания петель. Матрица вращения loopcloruse должна быть тождеством SO(3), правильно? Но из-за ошибок дело не в этом, а что-то чуть-чуть рядом. Не будем называть это «вращение замкнутой петли» и будем представлять его кватернионом, называемым
q_loop
Просто интерполируйте, используя технику SLERP, между вращениями поз и этими вращениями, умноженными на
q_loop^(-1)
. Уловка состоит в том, чтобы интерполировать с интервалом, пропорциональным каждой позе в схеме. Поскольку каждая поза накапливает разную величину ошибок, этот интервал не может быть постоянным, верно? Чему пропорциональна эта ошибка? Количество накопленных трансформаций, использованное для расчета этой позы, так же верно и для этих интервалов.
Если у вас есть
n
облака, тогда тебе нужно
n-1
позы положить все в глобальное происхождение. Первая поза
P1 = (R1,t1)
, чтобы оптимизировать
R1
, сначала преобразуйте его в кватернион
q1
, затем умножьте его на
q_loop^-1
, назовем это
p1 = q1*q_loop^-1
. Первое оптимальное вращение
q1_optimal = SLERP(q1, p1, 1/n)
. Второй оптимальный поворот:
q2_optimal = SLERP(q2, p2, 2/n)
.
Согласно этому паттерну, если у вас есть облака, оптимальные позы вращения будут
q1 = SLERP(q1, p1, 1/4)
q2 = SLERP(q2, p2, 2/4)
q3 = SLERP(q3, p3, 3/4)
[1/4, 2/4, 3/4]
интервалы интерполяции, которые варьируются [0,1]
Эти интервалы распределяют ошибку от финальной позы к первой, но также уменьшают общую ошибку. Их можно продемонстрировать, применяя метод наименьших квадратов в экспонентах технологии SLERP с ограничением, что кватернион замыкания петли должен быть идентичностью. Когда мы делаем
qn*q_loop^(-1)
и получил,
pn
- вращение, составленное в обратном направлении в цепи. Но к первому повороту позы такой путь накапливает больше ошибок, поэтому первое вращение интерполируется рядом с
0
, что объясняет
1/4
. Последние повороты позы будут интерполированы около
1
, потому что обратный путь накапливает меньше ошибок, что объясняет
3/4
позировать P3.
С
n=4
облака, один из поворотов интерполируется ровно посередине,
2/4 = 0.5
, потому что, идя так или иначе, эта поза будет накапливать два поворота. Этот метод предполагает равномерное распределение ошибок при вращении каждого номинала, это неверно, но модель была протестирована, и результаты отличные в облаках точек от TLS и от роботов (велодин LIDAR). Работа еще не опубликована.
Цикл замыкания в целом обычно разбивается на следующие этапы:
1- Обнаружение петли 2- Закрытие петли 3- Коррекция петли
Вам не нужно автоматическое обнаружение петли прямо сейчас, я пропущу это.
Закрытие петли является проблемой получения правильной позы одного конца, чтобы соответствовать другому концу. Поза - это преобразование SE3 (трехмерное рототрассирование), или, если у вас дрейфующий масштаб (например, монокулярный визуальный удар), поза - это преобразование Sim3 (трехмерное сходство).
В этой позе коррекция петли - это проблема исправления всех точек и ключевых кадров для обеспечения согласованности всей карты.
Scale Drift-Aware Large Scale Monocular SLAM - статья Страсдата и др., В которой описывается метод исправления петель (третий шаг).
ORB-SLAM2 реализует его в Optimizer::OptimizeEssentialGraph и является открытым исходным кодом.