Петля, закрывающая 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 и является открытым исходным кодом.

Другие вопросы по тегам