Геопривязка pointcloud по опорным точкам

Наш маленький стартап решил испытать силы в трёхмерном моделировании городской инфраструктуры. Стоит сразу оговориться, что подразумевается не высокоточное геодезическое картографирование, а оперативное получение трехмерной карты на движущемся транспорте. Для решения этой задачи подобрали бесхозный лидар (трехмерный сканер) и соорудили конструкцию для установки на автомобиль. Надо было быстро и дешево отработать прототип софта для сбора информации. В стек навигации входила инерциалочка и GPS-приёмник. После нескольких заездов по Санкт-Петербургу основной проблемой оказалась точная склейка трехмерных сканов по данным навигации. Чтобы обойти этот момент, мы решили испробовать SLAM-алгоритм чтобы карта строилась только по лидарной съемке. В результате SLAM у нас получилась карта в локальной системе координат, относительно старта маршрута. Для заказчика требовалось выдать трехмерную карту с географической привязкой.

После каждой поездки сохранялся набор облаков точек и трек маршрута. На выходе мы должны получить объединенный pointcloud, где каждая точка будет в глобальной географической системе координат.

Для подготовки и привязки мы использовали open source редактор Cloud compare.

Объединение облаков точек

SLAM сохраняет данные в виде набора PCD-файлов, которые необходимо предварительно объединить. Далее буду описывать шаги в виде инструкции-памятки.

1. Нажимаем File → Open и выбираем группу облаков для склейки. После подтверждения должно быть аналогично:

   

2. Выделяем с помощью Shift все облака для объединения.

   

3. Объединяем через Edit → Merge. На вопрос типа "Generate scalar fields..." отвечаем No. Получилось объединенное облако.

   

4. Нажимаем File → Save и выбираем удобный формат для сохранения.

Обрезка облака

При объединении попался неудачно отсканированный участок, давайте обрежем его.

1. Нажимаем File → Open и выбираем облако для обрезки. Либо работаете с открытым далее. Допустим, оно у вас как в примере и вы хотите убрать лишнюю нижнюю часть:

   

2. Выбираем инструмент Edit → Segment. Сверху справа появляется дополнительная панель и сверху по центру надпись текущего режима: Segmentation [ON]. Значит мышь теперь работает в режиме выделения контура для обрезки. Выделяем контур однократным нажатием ЛКМ, после окончания ПКМ. Должен получиться шатёр такого вида:

   

3. Если всё устроило, то нажимаем белый многоугольник на красном фоне с подсказкой Segment Out во временной панельке сверху.

   

4. Выделенная зона пропадает из видимости, надпись текущего режима: Segmentation [PAUSED]. Теперь надо нажать зеленую галочку на той же панели для применения удаления. После этой процедуры создаются два облака - одно с постфиксом .remained, другое с .segmented. Нажимая галочки в правой панели с названиями облаков можно отключить их видимость и посмотреть результат.

   

5. Нажимаем File → Save для инвестирующего облака и выбираем удобный формат для сохранения.

Процедура геопривязки

Геопривязка подразумевает преобразование координат облака из локальной системы к глобальной. Этот раздел сделан на основе видео. Общая идея следующая - чтобы произвести переход от одной системы координат к другой, необходимо получить матрицу трансформации 4x4, которая бы однозначно перевела бы все точки облака. Для получения такой матрицы надо построить связь между тремя точками облака и тремя точками на карте. Для примера взят файл облака с Богатырским проспектом:

Если посмотрите на параметры слева снизу, то размер коробки с моделью будет:

X: 1029.14
Y: 461.169
Z: 89.9612

Для более удобной работы немного раскрасим модель, для этого выбираем пункт Edit → Scalar fields → Add point indexes as SF:

Далее нам необходимо обнаружить этот же участок на карте, можно пользоваться картами google, чтобы получить координаты для любой точки по нажатию.

Теперь осталось определить такие точки, которые легко обнаруживаются на карте и легко найти в облаке. После анализа были выбраны такие точки:

1. Перекресток Богатырского и Туристской

2. Точка между двумя "рамками", где второй съезд с Богатырского

3. Точка под "рамкой" на Шуваловском проспекте

Теперь нужно получить координаты для этих точек. Для карты это широта, долгота, для облака это XYZ. Для получения координат точек в CloudCompare выбираем Tools → Point picking. Покажем на примере перекрестка:

Координаты точки: (391.702393;-27.628052;2.057297)

Теперь то же самое на картах, просто кликаем мышью:

Координаты точки: (60.006764,30.212293)

Для удобства представления и конвертации выбрали проекцию Меркатора (UTM). Можно воспользоваться бесплатным сервисом latlong.net. Аналогично с остальными двумя местами. У вас должна получиться табличка такого типа (значения немного уточнены):

Теперь надо сделать маленькую модельку с координатами UTM, для этого создаёте текстовый файл. Для примера georef.asc с содержимым:

CENT,344573.39,6655440.61,0.0,255,0,0
STO2,343953.94,6655621.66,0.0,0,255,0
LSTO,343763.61,6655468.82,0.0,0,0,255

Первая колонка - название точки, затем координаты (z=0), компоненты цвета r,g,b для удобства. Загружаем файл в CloudCompare, в окне выбираем Apply, затем на вопрос Rescale отвечаем No, иначе координаты все потеряются. После загрузки облака сильно разнесены друг от друга.

Теперь начнем совмещение. Выбираем оба облака. Затем Tools → Registration → Alighn(points pairs picking) В появившемся окне выбираем перемещаемое облако (bogatirskii_small). Появляется сверху справа табличка, в которой в верхней половине вводим координаты модели, а в нижней координаты UTM опорных точек. Надо внимательно переносить чтобы последовательность точек была одинаковая.

В правой колонке указана величина ошибки, в нашем случае максимальная ошибка 14 метров при совмещении. Можно поискать другие опорные точки, но для примера подойдет. Нажимаем align, рассчитывается матрица и выполняется трансформация.

На скриншоте видно как совместились точки. Если всё устраивает, то нажимаем зеленую галочку. В окне отобразится трансформационная матрица и средняя ошибка (10.446). Всё, теперь готово, проверяем смещение координат. Нажимаем на bogatirskii_small слева и смотрим Global Box Center:

X: 344176.843750
Y: 6655525.500000
Z: -0.003010

Далее сохраняем в удобном формате. Мы выбрали LAS, так как он является определенным стандартом для работы с pointcloud. Однако, для корректного перехода в LAS предлагаю сохранение в формате Ascii Cloud с префиксом .xyz (bogatirskii_georef.xyz). Тогда начало файла будет вида:

344050.87500000 6655427.50000000 -2.37526727
344050.96875000 6655427.50000000 -2.09358692
344050.90625000 6655427.50000000 -1.77294660
344063.78125000 6655432.00000000 14.17946529
344083.50000000 6655438.00000000 -2.25361919
344083.53125000 6655438.50000000 -2.18411636
344069.46875000 6655448.50000000 -4.81668091
344073.34375000 6655450.00000000 -4.93998623
344142.00000000 6655462.50000000 -1.68570638
344427.09375000 6655324.00000000 0.06759101
344479.12500000 6655306.50000000 -2.23689747

Теперь можно через lastools сконвертировать в LAS с указанием зоны:

>txt2las -i bogatirskii_georef.xyz -o bogat_geo.las -parse xyz -utm 36V

Как не потерять точность

При трансформации система может выдать сообщение следующего вида:

Это значит что при применении трансформации, будет потеряна точность в координатах и часть точек сольется, получится сетчатая структура (это ограничения самого CloudCompare). Чтобы этого не произошло, предлагается использовать отрицательный сдвиг начала координат, а при сохранении его учесть. Нажимаем YES. Получаются результаты такого вида:

Shifted box center:
X: 60.3544
Y: 31.0959
Z: 72.9026
Global box center:
X: 343560.354370
Y: 6654031.095917
Z: 72.902580

Всё нормально. Теперь можно сохранить, например в LAS/LAZ. Чтобы добавить информацию по проекции надо воспользоваться утилитой las2las из комплекта LasTools:

las2las -i my_file.laz -o my_file_utm.laz -utm 36V

Файл геопривязан и есть сведения о проекции.

Проверка расположения в GoogleEarth

Можно приблизительно оценить регион геопривязки. Получаем shape-файл из LAS:

las2shp.exe bogat_fp2_geo.laz

В каталоге сгенерируется файл bogat_fp2_geo.shp. Теперь открываем его GoogleEarth. Если он очень большой можно загрузить часть. Выделяем галочкой и смотрим на маячки.

Выводы

В результате получилось геопривязанное облако, приемлемого качества. Во всяком случае, заказчику понравилось. Конечно, минус такого подхода - использование разных, не совсем удобных утилит. Конечно, хотелось бы работать в одной мощной среде. Основное преимущество - не надо платить за дорогое коммерческое решение для редких задач привязки облаков.

Ссылки

• Cloud compare

• LasTools

• Сервис latlong.net

• Видео с инструкцией по геопривязке CloudCompare