Реализация алгоритма iterative closest point

Старков Артем Дмитриевич

Реализация алгоритма iterative closest point

Работа направлена на практическое применение известных методов оптимизации и работы с данными в таких областях как робототехника. Целью было реализовать итеративный алгоритм ближайших точек (Iterative closest point, ICP), а также использовать его для совмещения облаков точек. В основе выбранного алгоритма совмещения лежат методы, актуальность которых растёт с каждым годом, особенно в сфере автоматизации. Издано множество работ по математическому программированию и по алгоритму ICP в частности, в которых совершенствуются методы поиска ближайших точек и методы совмещения. Алгоритм ICP поочерёдно производит поиск ближайших точек и оптимальное преобразование между двумя наборами данных для их совмещения. Основной трудностью данного подхода является чувствительность к выбросам и недостаточное количество данных. Большинство практических реализаций ICP алгоритма решают эту проблему с помощью коррекции весов. Однако эти методы могут быть ненадежными и трудными, что часто требует существенной ручной настройки. Основной задачей рассматриваемого алгоритма является поиск аргументов, которые минимизируют критерий совмещения. : выпускная бакалаврская работа по направлению подготовки : 15.03.06 - Мехатроника и робототехника

Машиностроение

Дипломы

Вуз: Дальневосточный федеральный университет (ДВФУ)

ID: 5b8edaed7966e1073081beba

UUID: 78c98d70-92a5-0136-bd89-525400005860

Язык: Русский

Опубликовано: около 6 лет назад

Просмотры: 568

Старков Артем Дмитриевич

Источник: Федеральное государственное автономное образовательное учреждение высшего образования «Дальневосточный федеральный университет»

Комментировать 0

Рецензировать 0

Скачать - 11,2 МБ

Поделиться работой

АННОТАЦИЯ Работа направлена на практическое применение известных методов оптимизации и работы с данными в таких областях как робототехника. Целью было реализовать итеративный алгоритм ближайших точек (Iterative closest point, ICP), а также использовать его для совмещения облаков точек. В основе выбранного алгоритма совмещения лежат методы, актуальность которых растёт с каждым годом, особенно в сфере автоматизации. Издано множество работ по математическому программированию и по алгоритму ICP в частности, в которых совершенствуются методы поиска ближайших точек и методы совмещения. Алгоритм ICP поочерёдно производит поиск ближайших точек и оптимальное преобразование между двумя наборами данных для их совмещения. Основной трудностью данного подхода является чувствительность к выбросам и недостаточное количество данных. Большинство практических реализаций ICP алгоритма решают эту проблему с помощью коррекции весов [6]. Однако эти методы могут быть ненадежными и трудными, что часто требует существенной ручной настройки. Основной задачей рассматриваемого алгоритма является поиск аргументов, которые минимизируют критерий совмещения, переводя множество точек облака 𝒮 в множество точек облака 𝒯. 5

ОБОЗНАЧЕНИЯ И СОКРАЩЕНИЯ САПР − система автоматизированного проектирования ICP – итеративный алгоритм ближайших точек (iterative closest point) KD дерево – дерево k-мерного пространства SVD – сингулярное разложение (singular value decomposition) PCL – библиотека облака точек (point cloud library) PCD – формат облака точек (point cloud data) 6

ВВЕДЕНИЕ Современная промышленность нуждается в новых методах и подходах автоматизации производства, способных работать в режиме реального времени с приемлемыми показателями качества. Одним из важнейших этапов изготовления изделия является получение его математической модели. Рано или поздно возникает необходимость описания реального физического объекта с помощью математического аппарата для последующей обработки в различных пакетах САПР. Для автоматизации и ускорения процесса перевода поверхности модели в цифровой формат используется трёхмерное сканирование. Во время построения цифровых моделей появляется необходимость сканирования объекта с нескольких ракурсов в связи с перекрытием некоторых частей детали и ограниченным диапазоном датчиков. Результат сканирования представляет собой поверхность объекта, которая описана с помощью облака точек. Полученный скан и модель должны быть выровнены в общую систему отсчёта для дальнейшей обработки, чтобы иметь возможность измерять и классифицировать возможные ошибки в производстве. В мобильной робототехнике сопоставление скана сцены с её моделью обычно используется для уточнения местоположения. В последнее время устройства 3D-сканирования (например, Microsoft Kinect) привели к растущему интересу в области надежных алгоритмов совмещения. Низкая стоимость этих устройств сбора данных обеспечивается за счет плохого качества сканирования, что требует алгоритмов, которые способны иметь дело с большими объемами шума и выбросов. Алгоритм ближайших точек имеет широкое применение в промышленной робототехнике. Он позволяет решать задачи позиционирования манипулятора во время обработки детали. Для того, чтобы выработать необходимый управляющий сигнал и обработать деталь по заданной траектории, необходима 7

её CAD модель. Однако модель находится в некоторой системе координат и вычислить траекторию обработки детали можно только в системе координат модели. Поэтому обработать деталь таким образом возможно лишь закрепив её в заранее известной точке. Лазерное сканирование позволяет решить эту проблему. Благодаря сканеру и итеративному алгоритму ближайших точек возможно вычислить траекторию движения схвата манипулятора в его системе координат и обработать произвольно закреплённую деталь. Описанная выше задача решается путём вычисления матрицы преобразования из системы координат робота в систему координат модели. Зная эту матрицу, можно трансформировать траекторию движения схвата в системе координат модели в аналогичную траекторию в системе координат робота и обработать реальную произвольно расположенную деталь. 8

1 ОБЩЕЕ ОПИСАНИЕ РАБОТЫ АЛГОРИТМА Пусть 𝒯 = {𝜏1 , 𝜏2 , ⋯ 𝜏𝑁 } и 𝒮 = {𝑠1 , 𝑠2 , ⋯ 𝑠𝑁 } − два множества точек в конечномерном вещественном векторном пространстве ℝ𝑛 . Необходимо найти преобразование множества точек 𝒮 во множество точек 𝒯 с помощью матрицы поворота 𝑅 и вектора смещения 𝑡, зависимость которых представлена в виде: 𝜏𝑖 = 𝑅𝑠𝑖 + 𝑡. (1) где 𝑖 = 0,1, ⋯ , 𝑛. 1.1 Основные этапы работы алгоритма Итеративный алгоритм ближайших точек можно условно разбить на четыре этапа [3]: a) Нахождение соответствующей ближайшей пары 𝜏𝑗 для точки 𝑠𝑖 , такой, что: 𝑆(𝜏) = ‖𝜏𝑗 − 𝑠𝑖 ‖2 , (2) (𝜏) = argmin 𝑆(𝜏). (3) 𝑠𝑖 𝜖𝒮, 𝜏𝑗 𝜖𝒯 Под 𝑠𝑖 здесь понимается заданная точка в пространстве ℝ3 , для которой необходимо найти кратчайшее расстояние до множества 𝒯. То есть, найти такую точку 𝜏𝑗 , которая будет доставлять наименьшее значение функционалу (2). 9

b) Вычисление вектора смещения 𝑡 и матрицы поворота 𝑅, которые доставляют минимум функционалу: 2 𝒥(𝑅, 𝑡) = ∑𝑁 𝑖=1‖(𝑅𝑠𝑖 + 𝑡) − 𝜏𝑖 ‖2 , (𝑅, 𝑡) = argmin 𝒥(𝑅, 𝑡). 𝑅∈𝑆𝑂(3),𝑡∈𝑅 3 (4) (5) Матрица 𝑅 представляет собой вещественную ортогональную матрицу 3 × 3 с определителем равным единице, то есть принадлежит специальной ортогональной группе вращений 𝑆𝑂(3); Вектор 𝑡 представляет собой смещение множества точек вдоль вектора [𝑥, 𝑦, 𝑧]𝑇 ; ‖∙‖22 − квадрат нормы 𝑙2 или квадрат Евклидового расстояния между двумя точками. c) Преобразование трансформируемого облака точек с помощью найденной матрицы поворота и вектора смещения в новое облако точек 𝑠𝑖 = 𝑅𝑠𝑖 + 𝑡. (6) d) Повторение всего итерационного процесса алгоритма пока 𝒥(𝑅, 𝑡) ≥ 𝜀, где в качестве трансформируемого облака точек теперь используется облако, полученное на предыдущем этапе. 10

1.2 Поиск ближайших точек Рассмотрим два идентичных облака точек, которые смещены друг относительно друга. Каждая точка пронумерована и верно, что две точки из различных облаков с одинаковым индексом имеют идентичное положение относительно совмещаемых объектов. Очевидно, что совместить облака точек, означает совместить все пары точек с равными индексами из различных множеств. Другими словами, две поверхности должны совпадать, а все точки из совмещаемого множества должны принадлежать поверхности целевого облака точек. Рассмотренная идеальная модель обычно не имеет ничего общего с практикой, потому что точки с равными индексами не соответствуют идентичному положению относительно совмещаемых объектов. Такую задачу можно решить, прибегая к геометрическому соображению, которое заключается в том, что расстояние между точкой и поверхностью определяется как кратчайшее расстояние между ними. Сократив это расстояние, точка будет принадлежать поверхности, что и является целью совмещения. Из всего выше сказанного следует вывод, что для того, чтобы совместить два облака точек, необходимо сокращать расстояния между всеми парами ближайших точек. Чтобы ускорить этот процесс необходимо отсортировать точки целевого облака с помощью геометрической интерпретации бинарного дерева − KD-дерева [5]. 11

KD-дерево представляет собой двоичное дерево поиска, где данные в каждом узле являются K-мерной точкой в пространстве. Смысл построения дерева заключается в пространственном разбиении структуры данных для организации точек в пространстве. Не листовой узел в дереве делит пространство на две части, называемые полупространствами. Точки левого полупространства представлены левым поддеревом этого узла, а точки правого полупространства представлены правым поддеревом. Другими словами, если необходимо найти ближайшую точку в дереве к некоторой заданной точке, расположенной в левом полупространстве, следует спуститься в левое поддерево, тем самым исключая необходимость перебора всех точек. Далее, аналогичным образом, сравнивая по второй координате, можно перейти в верхнее или нижнее полупространство левого полупространства. На каждом уровне дерева сравнение происходит попеременно по каждой координате (рисунок 1). Что позволяет произвести сортировку в любом возможном направлении, переходя на всё меньшую область, в которой содержится одна точка (лист). Для демонстрации построения 3D-дерева целесообразно построить 2Dдерево, по причине сложности графического представления и принципиальной схожести процесса. В трёхмерном случае все операции производятся аналогичным образом с добавлением координаты 𝑧. Рассмотрим построение двумерного дерева [2, 7] для набора следующих точек: (3, 6), (17, 15), (13, 15), (6, 12), (10, 19), (9, 1), (2, 7). 12

Алгоритм построения: а) Вставить первую точку (3, 6): поскольку дерево пустое, сделать её корнем; б) Вставить вторую точку (17, 15): сравнить координату 𝑥 этой точки с координатой 𝑥 точки в корне. Так как 17 > 3, то точка идёт в правое поддерево; в) Вставить точку (13, 15): сравнить координату 𝑥 этой точки с координатой 𝑥 точки в корне. Так как 13 > 3, то точка идёт в правое поддерево. Далее сравнить координату 𝑦 этой точки с координатой 𝑦 точки в правом поддереве. Так как 15 = 15, то точка идёт в правое поддерево текущего корневого узла; г) Вставить точку (6, 12): так как 6 > 3, то точка идёт в правое поддерево. Так как 12 < 15, то точка будет лежать в левом поддереве корня (17, 15); д) Повторить аналогичные действия и добавить оставшиеся точки. Результат представлен на рисунке 1. 1.3 Геометрическая интерпретация KD-дерева Представленный выше алгоритм сортировки имеет следующий геометрический смысл [7]. На первом этапе точка (3, 6) делит пространство вертикальной прямой на две части. Все точки, которые меньше (левее) по координате 𝑥, окажутся в левом поддереве. Все точки, которые больше (правее) – в правом (рисунок 2). 13

Рисунок 1− 2D-дерево Правое поддерево Рисунок 2 – Правое поддерево. Добавление второй точки 14

Правый наследник правого поддерева Корень правого поддерева Рисунок 3 – Наследник правого поддерева Очевидно, что вторая точка из списка (точка (17,15)) находится в красной области и станет узлом правого поддерева, в котором сравнение происходит по координате 𝑦 (рисунок 3). Точка (13,15) станет наследником правого поддерева, так как принадлежит красной области (рисунок 3). Точка (6, 12) станет левым наследником (рисунок 5), так как сравнение в корне правого поддерева происходит по второй координате. Следующая точка (рисунок 4) окажется листовым узлом и будет левым наследником правого наследника правого поддерева. Так как правый наследник находится на втором уровне дерева и сравнение с ним производится по второй координате, то сравнение с его наследниками будет происходить по первой координате. 15

Левый наследник правого наследника правого поддерева Рисунок 4 – Листовой узел Рисунок 5 – Левый наследник правого поддерева 16

Рисунок 6 – Добавление последней точки На рисунке 6 представлена последняя точка – точка (9, 1), которая является листовым узлом. Так же листовым узлом является точка (2, 7). В результате построения, дерево получилось несбалансированным, то есть количество точек в правом и левом поддереве различается. Для устранения данного эффекта можно использовать разделение по медиане или среднему значению точек по каждой из координат. 17

2 РАСЧЕТ ПАРАМЕТРОВ ПРЕОБРАЗОВАНИЯ Расчет параметров представляет собой один из основных этапов работы алгоритма. Результатом расчета является матрица поворота и вектор смещения, которые переводят одно множество точек в другое. Применив данное преобразование, можно совместить облака точек. Чтобы совместить два облака точек, сначала необходимо совместить их центры масс. Затем, используя алгоритм оптимизации, вычислить матрицу поворота и совместить оставшиеся точки. В качестве алгоритма для расчета параметров поворота было использовано сингулярное разложение (singular value decomposition, SVD). Выбор данного метода обусловлен его распространённостью и широким применением в других областях науки и техники. Пусть 𝒯 = {𝜏1 , 𝜏2 , ⋯ 𝜏𝑁 } и 𝒮 = {𝑠1 , 𝑠2 , ⋯ 𝑠𝑁 } − два множества точек в конечномерном вещественном векторном пространстве ℝ3 . 2.1 Вычисление вектора смещения Чтобы найти смещение, минимизирующее критерий (4), необходимо вычислить его частную производную по аргументу 𝒕 и приравнять к нулю: 𝑁 𝜕𝒥 = 2 ∑(𝑅𝑠𝑖 + 𝑡 − 𝜏𝑖 ), 𝜕𝑡 𝑖=1 𝑁 𝑁 𝑁 ∑𝑅𝑠𝑖 + ∑ 𝑡 − ∑𝜏𝑖 = 0, 𝑖=1 𝑖=1 𝑁 𝑖=1 𝑁 ∑𝑅𝑠𝑖 + 𝑡𝑁 − ∑𝜏𝑖 = 0. 𝑖=1 𝑖=1 18

Для того, чтобы избавиться от параметра 𝑡, введём константы: ∑𝑁 𝑖=1𝑠𝑖 𝑠̅ = , 𝑁 𝜏̅ = ∑𝑁 𝑖=1𝜏𝑖 𝑁 . (7) Данная замена имеет геометрический смысл, который заключается в том, что точки с координатами 𝒔̅ и 𝝉̅ представляют собой положение центров масс для поверхностей 𝒮 и 𝒯 соответственно. Проведя аналогию с центром масс материальных точек в классической механике, введённые замены можно получить, приняв массы точек равными единице. Тогда: 𝑅𝑠̅𝑁 + 𝑡𝑁 − 𝜏̅𝑁 = 0, 𝑡 = 𝜏̅ − 𝑅𝑠̅. (8) Выражение (8) позволяет найти оптимальное смещение. Используем данное равенство в исходном критерии (4): 𝑁 𝑁 𝑁 ̅ ‖2 , ∑‖(𝑅𝑠𝑖 + 𝑡) − 𝜏𝑖 ‖22 = ∑‖𝑅𝑠𝑖 + 𝜏̅ − 𝑅𝑠̅ − 𝜏𝑖 ‖22 = ∑‖𝑅(𝑠𝑖 − 𝑠̅) − (𝜏𝑖 − 𝜏) 2 𝑖=1 𝑖=1 𝑖=1 И введём замену: 𝑎𝑖 = 𝑠𝑖 − 𝑠̅ , 𝑏𝑖 = 𝜏𝑖 − 𝜏̅, (9) Которая представляет собой координаты векторов, проведённых из центра масс к каждой точке соответствующей поверхности, называемые центроидами. Тогда исходная задача может быть переформулирована как поиск одного 19

аргумента − параметра 𝑹, так как центры масс уже совмещены с помощью найденного ранее оптимального смещения. Эти преобразования позволяют перейти к обновлённому критерию минимизации: 2 𝑅 = argmin ∑𝑁 𝑖=1‖𝑅𝑎𝑖 − 𝑏𝑖 ‖2 . (10) 𝑅∈𝑆𝑂(3) 2.2 Вычисление матрицы поворота По определению (7), норма 𝐿2 порождает метрику ℓ2 , которая представляет собой скалярное произведение векторов, что соответствует произведению вектора строки на вектор столбец: ‖𝑅𝑎𝑖 − 𝑏𝑖 ‖22 = (𝑅𝑎𝑖 − 𝑏𝑖 )𝑇 (𝑅𝑎𝑖 − 𝑏𝑖 ), (𝑅𝑎𝑖 − 𝑏𝑖 )𝑇 (𝑅𝑎𝑖 − 𝑏𝑖 ) = (𝑎𝑖 𝑇 𝑅𝑇 − 𝑏𝑖 𝑇 )(𝑅𝑎𝑖 − 𝑏𝑖 ) = = 𝑎𝑖 𝑇 𝑅𝑇 𝑅𝑎𝑖 − 𝑎𝑖 𝑇 𝑅𝑇 𝑏𝑖 − 𝑏𝑖 𝑇 𝑅𝑎𝑖 + 𝑏𝑖 𝑇 𝑏𝑖 , Так как матрица 𝑅 ортогональна, то 𝑅𝑇 𝑅 = 𝐼. Можно заметить, что слагаемое 𝑎𝑖 𝑇 𝑅𝑇 𝑏𝑖 – это скаляр, так как 𝑎𝑖 𝑇 имеет размер 1 × 3, 𝑅𝑇 имеет размер 3 × 3, 𝑏𝑖 − 3 × 1. Для произвольного скаляра верно 𝑎𝑇 = 𝑎, поэтому: 𝑎𝑖 𝑇 𝑅𝑇 𝑏𝑖 = (𝑎𝑖 𝑇 𝑅𝑇 𝑏𝑖 )𝑻 = 𝑏𝑖 𝑇 𝑅𝑎𝑖 , ‖𝑅𝑎𝑖 − 𝑏𝑖 ‖22 = 𝑎𝑖 𝑇 𝑅𝑇 𝑅𝑎𝑖 + 𝑏𝑖 𝑇 𝑏𝑖 − 2𝑏𝑖 𝑇 𝑅𝑎𝑖 = 𝑎𝑖 𝑇 𝑎𝑖 + 𝑏𝑖 𝑇 𝑏𝑖 − 2𝑏𝑖 𝑇 𝑅𝑎𝑖 . Вернёмся к задаче минимизации: 𝑁 𝑁 argmin ∑‖𝑅𝑎𝑖 − 𝑏𝑖 ‖22 = argmin ∑ 𝑎𝑖 𝑇 𝑎𝑖 + 𝑏𝑖 𝑇 𝑏𝑖 − 2𝑏𝑖 𝑇 𝑅𝑎𝑖 . 𝑅∈𝑆𝑂(3) 𝑖=1 𝑅∈𝑆𝑂(3) 20 𝑖=1

Очевидно, что первые два слагаемых не содержат аргумента минимизации и не имеют влияния на результат оптимизации, поэтому запись можно упростить: 𝑁 𝑅 = argmin ∑ −2𝑏𝑖 𝑇 𝑅𝑎𝑖 . 𝑅∈𝑆𝑂(3) 𝑖=1 По той же аналогии можно избавиться от константы и, переформулировав задачу в поиск аргумента максимизации, опустить знак вычитания: 𝑇 𝑇 𝑁 𝑅 = argmin ∑𝑁 𝑖=1 −2𝑏𝑖 𝑅𝑎𝑖 = argmax ∑𝑖=1 𝑏𝑖 𝑅𝑎𝑖 . 𝑅∈𝑆𝑂(3) (11) 𝑅∈𝑆𝑂(3) Критерий (12) можно представить в виде следа матрицы 𝐴, полученной умножением: 𝑏1 𝑏 𝐴 = 𝐵𝑇 𝑅𝐴 = [ 2 ] 𝑅[𝑎1 ⋮ 𝑏𝑛 𝑎2 𝑏1 𝑇 𝑅𝑎1 ∗ ⋯ 𝑎𝑛 ] = ∗ [ ∗ ∗ 𝑏2 𝑅𝑎2 ∗ ∗ 𝑇 𝑡𝑟(𝐴) = 𝑡𝑟(𝐵𝑇 𝑅𝐴) = ∑𝑁 𝑖=1 𝑏𝑖 𝑅𝑎𝑖 . 𝑇 ∗ ∗ ∗ ∗ , ⋱ ∗ ∗ 𝑏𝑛 𝑇 𝑅𝑎𝑛 ] (12) С учётом свойств следа, можно записать: 𝑡𝑟(𝐵𝑇 𝑅𝐴) = 𝑡𝑟(𝑅𝐴𝐵𝑇 ). Пусть: 𝐴𝐵𝑇 = 𝑆. 21 (13)

Необходимо найти сингулярное разложение: 𝑆 = 𝑈Σ𝑉 𝑇 . (14) Тогда, введя замену 𝑀 = 𝑉 𝑇 𝑅𝑈: 𝑡𝑟(𝑅𝑆) = 𝑡𝑟(𝑅𝑈𝛴𝑉 𝑇 ) = 𝑡𝑟(𝛴𝑀). Очевидно, что для того, чтобы максимизировать критерий (12), достаточно максимизировать 𝑀. Заметим, что матрицы 𝑉, 𝑅 и 𝑈 ортогональны, поэтому матрица 𝑀 так же ортогональна. Это означает, что столбцы матрицы 𝑀 образуют систему ортонормированных векторов, для которой верно 𝑚𝑗𝑇 𝑚𝑗 = 1. Из этого следует, что элементы матрицы |𝑚𝑖𝑗 | ≤ 1. Таким образом, чтобы максимизировать критерий (12), достаточно, чтобы элементы матрицы 𝑀 на главной диагонали были равны единице: 𝑀 = 𝑉 𝑇 𝑅𝑈 = 𝐼 ⟹ 𝑅 = 𝑉𝑈 𝑇 . (15) Суммируя всё выше сказанное, можно записать краткий алгоритм работы, который будет использоваться в программном коде для решения задачи совмещения. 22

2.3 Конечный алгоритм а) Вычисление центроидов для каждой поверхности: ∑𝑁 𝑖=1𝑠𝑖 𝑠̅ = , 𝑁 ∑𝑁 𝑖=1𝜏𝑖 𝜏̅ = . 𝑁 б) Вычисление центрированных векторов: 𝑎𝑖 = 𝑠𝑖 − 𝑠̅ , 𝑏𝑖 = 𝜏𝑖 − 𝜏̅. в) Вычисление матрицы: 𝑆 = 𝐴𝐵𝑇 . г) Вычисление сингулярного разложения матрицы 𝑆. д) Вычисление матрицы поворота: 𝑅 = 𝑉𝑈 𝑇 . е) Вычисление вектора смещения: 𝑡 = 𝜏̅ − 𝑅𝑠̅. 23

2.4 Полное сингулярное разложение матрицы Сингулярное разложение представляет собой мощный вычислительный инструмент для анализа матриц и задач с ними связанными, который имеет приложения в различных областях. Сингулярным разложением действительной 𝑚 × 𝑛 матрицы 𝐴 называется факторизация вида: 𝐴 = 𝑈Σ𝑉 𝑇 , (16) где 𝑈 − ортогональная матрица 𝑚 × 𝑚; 𝑉 − ортогональная матрица 𝑛 × 𝑛; Σ − диагональная матрица 𝑚 × 𝑛, у которой 𝜎𝑖𝑗 = 0 при 𝑖 ≠ 𝑗 и 𝜎𝑖𝑖 = 𝜎𝑖 ≥ 0. Величины 𝜎𝑖𝑖 называются сингулярными числами матрицы 𝐴, а столбцы матриц 𝑈 и 𝑉 – левыми и правыми сингулярными векторами. За сингулярным разложением скрывается такая идея: при надлежащем выборе матриц 𝑈 и 𝑉 можно обратить большинство элементов матрицы 𝛴 в нули; более того, можно даже сделать её диагональной матрицей с неотрицательными диагональными элементами. Сингулярное разложение матрицы обладает свойством, которое связывает задачу факторизации и нахождения собственных векторов. Собственным вектором 𝒙 матрицы 𝐴 называется такой вектор, при котором выполняется условие 𝐴𝒙 = 𝜆𝒙, где 𝜆 − собственное число. Из (17) и ортогональности матриц 𝑈 и 𝑉 следует, что: 𝐴𝐴𝑇 = 𝑈Σ𝑉 𝑇 𝑉Σ𝑈 𝑇 = 𝑈𝛴 2 𝑈 𝑇 , 𝐴𝑇 𝐴 = 𝑉Σ𝑈 𝑇 𝑈Σ𝑉 𝑇 = 𝑉𝛴 2 𝑉 𝑇 , 24

До множив на 𝑈 и 𝑉 соответственно: 𝐴𝐴𝑇 𝑈 = 𝑈𝛴 2 , (17) 𝐴𝑇 𝐴𝑉 = 𝑉𝛴 2 . (18) Откуда следует, что столбцы матриц 𝑈 и 𝑉 являются собственными векторами матриц 𝐴𝐴𝑇 и 𝐴𝑇 𝐴 соответственно, а собственными значениями являются квадраты сингулярных чисел 𝛴. Для вычисления матриц 𝑈 и 𝑉 необходимо найти соответствующие матрицы собственных векторов 𝑈 ∗ и 𝑉 ∗ , а затем провести их ортогонализацию с помощью алгоритма Грама-Шмидта (см. Приложение А). 2.5 Процесс ортогонализации Грама-Шмидта Пусть 𝑣 = (𝑣1 , 𝑣2 , ⋯ , 𝑣𝑛 ) − некоторый базис в 𝑛-мерном евклидовом пространстве, который можно модифицировать преобразованием его в новый ортонормированный базис 𝑒 = (𝑒1 , 𝑒2 , ⋯ , 𝑒𝑛 ). Алгоритм сводится к последовательному вычислению 𝑔𝑖 и 𝑒𝑖 : 𝑔1 = 𝑣1 , 𝑒1 = 𝑔1 , ‖𝑔1 ‖2 𝑔2 = 𝑣2 − (𝑣2 , 𝑒1 )𝑒1 , 𝑒2 = 𝑔2 , ‖𝑔2 ‖2 𝑔3 = 𝑣3 − (𝑣3 , 𝑒1 )𝑒1 − (𝑣3 , 𝑒2 )𝑒2 , 𝑒3 = 𝑔3 , ‖𝑔3 ‖2 𝑔𝑛 = 𝑣𝑛 − (𝑣𝑛 , 𝑒1 )𝑒1 − ⋯ − (𝑣𝑛 , 𝑒𝑛−1 )𝑒𝑛−1 , 𝑒𝑛 = 𝑔𝑛 . ‖𝑔𝑛 ‖2 В результате, для матриц 𝑈 ∗ и 𝑉 ∗ можно получить матрицы 𝑈 и 𝑉. 25

3 ПРИМЕНЕНИЕ АЛГОРИТМА Реализация работы алгоритма требует изучения документации различных библиотек и построения структуры программы. Первый и самый затруднительный этап алгоритма − это поиск ближайших точек. Наиболее перспективные и быстрые библиотеки, реализующие построение и поиск в KDдеревьях − это библиотеки flann и nanoflann. В качестве рабочей библиотеки была использована flann. Также необходимо произвести некоторые матричные преобразования и использовать сингулярное разложение матрицы. В настоящее время создано большое количество пакетов для матричных преобразований. В реализованном алгоритме используется библиотека Eigen. Она довольно просто позволяет производить матричные операции, что особенно удобно при работе с большими объёмами данных, такими как облака точек. 3.1 Структура программы Программа состоит из нескольких файлов: main.cpp IO.h Optimization.cpp Optimization.h Registration.cpp Registration.h Файл IO.h содержит объявления классов для чтения и визуализации данных. Файл Optimization.h содержит объявление класса для преобразования и оптимизации облака точек, в Optimization.cpp представлена соответствующая реализация методов. 26

Файл Registration.h содержит абстрактный класс для совмещения облаков точек и класс-наследник ICP. Registration.cpp содержит соответствующую реализацию. Диаграмма классов представлена на рисунке 7. Рисунок 7 − Диаграмма классов 27

3.2 Алгоритм работы Основные этапы программы, содержащиеся в main.cpp можно разбить на: 1. Target = Read("cat.pcd"); 2. Source = Transform(Target); 3. Transformation = PointToPoint (Target, Source); 4. GetView (Target, Source); На первом этапе происходит чтение файла. В качестве входных данных использованы облака точек в формате point cloud data (PCD), представляющие собой матрицу 3 × 𝑁. Далее задаётся преобразование для облака точек и выполняется вычисление параметров для его устранения. На последнем этапе отображаются совмещённые облака точек. Первый и второй пункты реализованы с помощью PCL библиотеки, их реализация представлена в файлах IO.h Optimization.cpp соответственно (см. Приложение Б). В методе PointToPoint вычисляется и применяется преобразование, минимизирующее критерий оптимизации на каждой итерации. Метод возвращает оптимальное преобразование в формате Matrix4f из библиотеки Eigen в следующем виде: 𝑅 𝑅𝑒𝑠𝑢𝑙𝑡 = [0 0 28 0 𝑡𝑥 𝑡𝑦 . 𝑡𝑧 1] (19)

Общий алгоритм работы PointToPoint следующий: 1. EigenTarget = getEigenMatrix(Target); 2. setInputCloud(Target); 3. nearestKSearch(Source[index]); 4. Result = ForTest.RigidTransform(NNMatrix, EigenSource); 5. transformPointCloud(*Source, *Source, Result); Для работы с SVD разложением и библиотекой Eigen необходим другой формат данных, отличный от PCD. Поэтому на первом этапе происходит преобразование облака точек в формат матриц Eigen для последующей работы. Далее необходимо построить 3D-дерево для целевого облака точек и найти в нём пару ближайших точек для каждой точки из совмещаемого облака (этап 3) и заполнить матрицу ближайших точек NNMatrix, в которую повекторно из EigenTarget копируются строки. Таким образом могут быть получены пары индексов ближайших точек. Далее с помощью сингулярного разложения (JacobiSVD) вычисляется результат в формате (19), который затем применяется к совмещаемому облаку точек Source. После этого весь процесс повторяется до тех пор, пока не завершится цикл работы ICP алгоритма либо не будет достигнуто приемлемое значение критерия оптимизации. 29

3.3 Результат совмещения В качестве входных данных была использована тестовая модель в формате PCD (Point cloud data), содержащая 3400 точек. Рисунок 8 − Тестовые облака точек 30

Рисунок 9 − Результат первого теста Для целевого облака точек (точки чёрного цвета) в первом тесте (рисунок 8) был задан произвольный поворот по оси 𝑍 (60° ); смещение отсутствует. Результат сходимости представлен на рисунке 10. Критерий останова не использовался. Как видно из графика, было достаточно 15-20 итераций, чтобы совместить тестовые облака точек. Всё зависит от требуемой точности совмещения, то есть требуемой близости к нулю критерия оптимизации. В результате применения найденного преобразования к облаку с красными точками, можно получить результат, представленный на рисунке 9. 31

2,50E+06 𝑐𝑟𝑖𝑡𝑒𝑟𝑖𝑎 𝑣𝑎𝑙𝑢𝑒 2,00E+06 1,50E+06 1,00E+06 5,00E+05 0,00E+00 0 5 10 15 𝑖𝑡𝑒𝑟𝑎𝑡𝑖𝑜𝑛 20 25 Рисунок 10 − Сходимость первого теста Для целевого облака точек во втором тесте (рисунок 11) был задан произвольный поворот по оси 𝑍 (60° ) и смещение. Результат сходимости представлен на рисунке 13. В результате применения найденного преобразования к облаку с красными точками, можно получить результат, представленный на рисунке 13. Визуально облака точек не совмещены, потому что оказалось недостаточно 25-ти заданных итераций. Очевидно, что второй тест сходится медленнее. 32

Рисунок 11 − Второй тест Основной проблемой совмещения является формирование соответствующих пар точек. Как следствие, увеличивается не только время сходимости, но и качество совмещения. Очевидно, что при более сложных положениях облаков точек друг относительно друга, времени для совмещения понадобится больше, иногда и вовсе критерий может не сойтись к минимуму (при больших поворотах или при симметричных объектах совмещения). 33

Рисунок 12− Результат второго теста 7,00E+06 5,00E+06 4,00E+06 3,00E+06 𝑐𝑟𝑖𝑡𝑒𝑟𝑖𝑎 𝑣𝑎𝑙𝑢𝑒 6,00E+06 2,00E+06 1,00E+06 0,00E+00 0 5 10 15 Рисунок 13 − Сходимость второго теста 34 𝑖𝑡𝑒𝑟𝑎𝑡𝑖𝑜𝑛 20 25

Рисунок 14 – Третий тест Для целевого облака точек в третьем тесте (рисунок 14) был задан произвольный поворот по оси 𝑋 и 𝑌, а также некоторое смещение. Результат сходимости представлен на рисунке 16. Как видно из графика, было достаточно 15-20 итераций, чтобы совместить тестовые облака точек. В результате применения найденного преобразования к 35

облаку с красными точками, можно получить результат, представленный на рисунке 15. Рисунок 15 − Результат третьего теста Также для совмещения облаков точек из второго теста была использована библиотека PCL. Она содержит различные методы для реконструкции поверхностей и трёхмерного совмещения. Для сравнения было задано то же количество итераций, что и во втором тесте. Совмещение представлено на рисунке 17. Очевидно, что результаты схожи. 36

3,00E+07 2,00E+07 1,50E+07 𝑐𝑟𝑖𝑡𝑒𝑟𝑖𝑎 𝑣𝑎𝑙𝑢𝑒 2,50E+07 1,00E+07 5,00E+06 0,00E+00 0 5 10 15 Рисунок 16 − Сходимость третьего теста Рисунок 17 − Результат второго теста PCL 37 20 𝑖𝑡𝑒𝑟𝑎𝑡𝑖𝑜𝑛 25

4 ЗАКЛЮЧЕНИЕ Алгоритмы сингулярного разложения имеют широкое практическое применение в различных областях, что делает их незаменимым инструментом, который следует изучать. В частности, SVD используют такие современные алгоритмы ICP как sparse, что позволяет сделать шаг на пути к изучению самых актуальных и передовых методов в области сканирования и технического зрения. В процессе работы был изучен классический ICP алгоритм. В силу распространённости, а также удобства использования при работе с матрицами, выбран метод оптимизации с использованием сингулярного разложения матриц. Работа была направлена на практическое применение итеративного алгоритма ближайших точек. Результатом работы является рабочий программный код. В процессе тестирования программы, были получены совмещённые облака точек и показана скорость сходимости критерия оптимизации. Недостатками данного алгоритма является плохое качество совмещения при больших углах поворота облаков точек относительно друг друга. Основные проблемы изученного алгоритма, которые необходимо решить для его улучшения, лежат в области формирования соответствующих пар точек и времени их поиска. 38

СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ 1. Стрижов, В.В. Информационное моделирование [Электронный ресурс]: [Конспект лекций] / Стрижов, В.В. – Электрон. текстовые дан. – Москва: МФТИ, 2003. – Режим доступа: URL: http://strijov.com/teaching/ 2. A practical implementation of KD trees [Электронный ресурс]: Engineering blog – Режим доступа: URL: https://programmizm.sourceforge.io/blog/2011/apractical-implementation-of-kd-trees 3. Franco P.Preparata. Computational geometry an introduction / Franco P.Preparata, Michael lan Shamos // Springer-Verlag Berlin, Heidelberg, 1985. – С. 52-57. 4. Olga Sorkine-Hornung. Least-Squares Rigid Motion Using SVD / Olga SorkineHornung, Michael Rabinovich // Department of Computer Science, ETH Zurich January 16, 2016. 5. Bentley, Jon Louis . Multidimensional Binary Search Trees Used for Associative Searching / Jon Louis Bentley // Communications of the ACM, vol.18, 1975. – С. 509-517. 6. Paul J.Besl. A Method for Registration of 3-D Shapes / Paul J.Besl, Neil D.McKay // Transactions on pattern analysis and machine intelligence, vol.14, NO.2, FEBRUARY 1992. – С. 239-255. 7. K Dimensional Tree: Set 1 (Search and Insert) [Электронный ресурс]: Geeksforgeeks: A computer science portal for geeks – Режим доступа: URL: https://www.geeksforgeeks.org/k-dimensional-tree/ 8. Kirk Baker. Singular Value Decomposition Tutorial, March 29, 2005. 9. Sofien Bouaziz. Sparse Iterative Closest Point / Sofien Bouaziz, Andrea Tagliasacchi, Mark Pauly // Eurographics Symposium on Geometry Processing, vol.32, number 5, 2013. 39

СОДЕРЖАНИЕ АННОТАЦИЯ ОБОЗНАЧЕНИЯ И СОКРАЩЕНИЯ ВВЕДЕНИЕ 1 ОБЩЕЕ ОПИСАНИЕ РАБОТЫ АЛГОРИТМА 1.1 Основные этапы работы алгоритма 5 6 7 9 9 1.2 Поиск ближайших точек 11 1.3 Геометрическая интерпретация KD-дерева 13 2 3 РАСЧЕТ ПАРАМЕТРОВ ПРЕОБРАЗОВАНИЯ 2.1 Вычисление вектора смещения 18 18 2.2 Вычисление матрицы поворота 20 2.3 Конечный алгоритм 23 2.4 Полное сингулярное разложение матрицы 24 2.5 Процесс ортогонализации Грама-Шмидта 25 ПРИМЕНЕНИЕ АЛГОРИТМА 3.1 Структура программы 26 26 3.2 Алгоритм работы 28 3.3 Результат совмещения 30 4 ЗАКЛЮЧЕНИЕ СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ СОДЕРЖАНИЕ ПРИЛОЖЕНИЕ А ПРИЛОЖЕНИЕ Б Б.1. Файл main.cpp 38 39 40 41 48 48 Б.2. Файл IO.h 49 Б.3. Файл Optimization.cpp 51 Б.4. Файл Optimization.h 54 Б.5. Файл Registration.cpp 54 Б.6. Файл Registration.h 57 40

ПРИЛОЖЕНИЕ А Сингулярное разложение матриц Пусть дана матрица A: 𝐴=[ 3 1 −1 3 1 ]. 1 Найдём 𝐴𝐴𝑇 : 3 −1 1 1 11 1 ] [1 3 ] = [ ]. 3 1 1 11 1 1 3 𝐴𝐴 = [ −1 𝑇 Далее необходимо найти собственные вектора 𝐴𝐴𝑇 . По определению: [ 𝑥1 11 1 𝑥1 ] [𝑥 ] = 𝜆 [𝑥 ]. 1 11 2 2 (А.1) Перепишем в виде системы двух равенств: { { 11𝑥1 + 𝑥2 = 𝜆𝑥1 , 𝑥1 + 11𝑥2 = 𝜆𝑥2 (11 − 𝜆)𝑥1 + 𝑥2 = 0 , 𝑥1 + (11 − 𝜆)𝑥2 = 0 И запишем характеристический многочлен: | (11 − 𝜆) 1 | = 0, (11 − 𝜆) 1 41 (А.2)

(11 − 𝜆)(11 − 𝜆) − 1 = 0, 𝜆2 − 11𝜆 − 11𝜆 + 121 − 1 = 0, 𝜆2 − 22𝜆 + 120 = 0. Решив его, можно найти собственные числа: 𝜆 = 10, 𝜆 = 12. Подставив найденные корни характеристического многочлена в систему (А.2), можно найти матрицу собственных векторов 𝑈 ∗ : Для 𝜆 = 12: 𝑥1 + (11 − 12)𝑥2 = 0, 𝑥1 = 𝑥2 . Для простоты примем, что 𝑥1 = 1, тогда 𝒗1 = [1 1]. Для 𝜆 = 10: (11 − 10)𝑥1 + 𝑥2 = 0, 𝑥1 = −𝑥2 . Для простоты примем, что 𝑥1 = 1, тогда 𝒗2 = [1 𝑈 ∗ = [ 𝒗1 𝒗2 ] = [1 1 ] 1 −1 42 −1]. В итоге получим: (А.3)

Найдём 𝐴𝑇 𝐴: 3 𝐴 𝐴 = [1 1 𝑇 −1 3 1 3 ][ −1 3 1 10 0 2 1 ] = [ 0 10 4]. 1 2 4 2 Собственные вектора найдём из: 𝑥1 10 0 2 𝑥1 [ 0 10 4] [𝑥2 ] = 𝜆 [𝑥2 ]. 𝑥3 2 4 2 𝑥3 (А.4) Перепишем в виде системы равенств: 10𝑥1 + 2𝑥3 = 𝜆𝑥1 { 10𝑥2 + 4𝑥3 = 𝜆𝑥2 , 2𝑥1 + 4𝑥2 + 2𝑥3 = 𝜆𝑥3 (10 − 𝜆)𝑥1 + 2𝑥3 = 0 { (10 − 𝜆)𝑥2 + 4𝑥3 = 0 . 2𝑥1 + 4𝑥2 + (2 − 𝜆)𝑥3 = 0 И запишем характеристический многочлен: (10 − 𝜆) 0 2 (10 − 𝜆) 0 4 | = 0. | (2 − 𝜆) 2 4 43 (А.5)

Разложим по первой строке: (10 − 𝜆) | (10 − 𝜆) 4 0 (10 − 𝜆)| = = (10 − 𝜆)[(10 − |+ 2| (2 − 𝜆) 4 2 4 𝜆)(2 − 𝜆) − 16] + 2[0 − 2(10 − 𝜆)] = 0. Решив, можно найти собственные числа: 𝜆 = 0, 𝜆 = 10, 𝜆 = 12. Подставив найденные корни характеристического многочлена в систему (А.5), можно найти матрицу собственных векторов 𝑉 ∗ . Для 𝜆 = 12: (10 − 12)𝑥1 + 2𝑥3 = 0, 𝑥1 = 𝑥3 , 𝑥1 = 1, 𝑥3 = 1. (10 − 12)𝑥2 + 4𝑥3 = 0, 𝑥2 = 2𝑥3 = 2, 𝒗1 = [1 2 1]. Для 𝜆 = 10: (10 − 10)𝑥1 + 2𝑥3 = 0, 𝑥3 = 0, 2𝑥1 + 4𝑥2 = 0, 𝑥1 = −2𝑥2 , 𝑥1 = 2, 𝑥2 = −1, 44

𝒗2 = [2 −1 0]. Для 𝜆 = 0: (10 − 0)𝑥1 + 2𝑥3 = 0, 𝑥3 = −5𝑥1 , 𝑥1 = 1, 𝑥3 = −5. 2𝑥1 + 4𝑥2 + (2 − 0)𝑥3 = 0, 𝑥2 = 2𝑥1 = 2, 𝒗3 = [1 2 −5]. Тогда матрица 𝑉 ∗ : 𝑉 = [ 𝒗1 ∗ 1 2 𝒗3 ] = [2 −1 1 0 𝒗2 1 2] −5 (А.6) Преобразуем матрицу (А.3) в ортогональную с помощью ортогонализации Грама-Шмидта: 𝒈1 = [1 1], 𝒆1 = 𝒈2 = [1 −1] − [1 −1] ∙ [ 1 [1 1] 1 1 =[ ], √2 √2 √12 + 12 1 1 1 ][ ] = [1 −1] − [0 √2 √2 √2 √2 45 0] = [1 −1],

𝒆2 = [1 −1] 1 1 − ]. =[ √2 √2 √12 + 12 1 1 𝑈 = [ 𝒆1 𝒆2 ] = √2 √2 . 1 1 − [√2 √2] Аналогичный процесс проделаем для матрицы (А.6): 𝒈1 = [1 2 𝒆1 = 𝒈2 = [2 −1 [1 2 1 ] 1 2 1 =[ ], √6 √6 √6 √12 + 22 + 12 0] − [2 −1 𝒆2 = 𝒈3 = [1 2 −5] − [1 2 −1 ∙[ √5 √5 𝒆3 = 1], 1 2 1 1 2 1 ][ ] = [2 −1 0], 0] ∙ [ √6 √ 6 √6 √6 √6 √6 [2 −1 0] √22 + 12 + 02 =[ 2 −1 √5 √5 0], 1 2 1 1 2 1 ][ ] − [1 2 2 −5] ∙ [ √6 √ 6 √6 √6 √6 √6 2 −1 0] [ 0] = [1 2 −5], √5 √5 [1 2 −5] 1 2 −5 =[ ], √30 √30 √30 √12 + 22 + 52 46 −5]

1 𝑉 = [ 𝒆1 𝒆2 𝒆3 ] = 1 √6 √5 √30 2 −1 2 √6 1 [√ 6 47 2 √5 0 √30 −5 √30] .

ПРИЛОЖЕНИЕ Б Листинг программы Б.1. Файл main.cpp #include "Registration.h" #include <ctime> int main() { // Pointers to Buffers for Target and Source Point Clouds pcl::PointCloud<pcl::PointXYZ>::Ptr Target; pcl::PointCloud<pcl::PointXYZ>::Ptr Source; Matrix4f Transformation; ICP Start; // Start Reading and Sparse ICP Algorithm // Read File Target = Start.Read.ReadPCDfile("cat.pcd"); // Transform Target Point Cloud into Source Point Cloud for test // We want to get two sets of Point Cloud (Initial and Transformed) for Registration // Now Source is transformed Target Point Cloud Source = Start.ForTest.Transform(Target); Transformation = Start.PointToPoint(Target, Source); //Transformation = Start.PCL(Target, Source); Start.Viewer.GetView(Target, Source); return 0; } 48

Б.2. Файл IO.h #include <iostream> #include <string> #include <Eigen/Dense> namespace IO { class ReadFile { public: ReadFile(); ~ReadFile(); }; class PCDreader : public ReadFile { public: inline const Eigen::MatrixXf getEigenMatrix(pcl::PointCloud<pcl::PointXYZ>::Ptr PCLcloud, int Dim) { Eigen::MatrixXf EigenCloud = PCLcloud>getMatrixXfMap(Dim, 4, 0); return EigenCloud; } inline pcl::PointCloud<pcl::PointXYZ>::Ptr PCDreader::ReadPCDfile(const std::string &filePath) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ>(filePath, *cloud) == -1) { PCL_ERROR("Couldn't Read File\n"); } else std::cout << "File Read Successfully!\n"; 49

return cloud; } inline void GetPCLpointConsole(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { for (size_t i = 0; i < cloud->points.size(); ++i) { std::cout << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << "\n"; } } inline void GetEigenPointConsole(Eigen::MatrixXf &Target, int iDim, int jDim) { for (int j = 0; j < jDim; j++) { for (int i = 0; i < iDim; i++) { std::cout << Target(i, j) << " "; } std::cout << "\n"; } } }; class Visualization { public: inline void GetView(pcl::PointCloud<pcl::PointXYZ>::Ptr Target, pcl::PointCloud<pcl::PointXYZ>::Ptr Source) { pcl::visualization::PCLVisualizer viewer("Registration"); pcl::visualization::PointCloudColorHandlerCustom<pcl::Poin tXYZ> target_handler(Target, 230, 20, 20); 50

viewer.addPointCloud(Target, target_handler, "Target"); pcl::visualization::PointCloudColorHandlerCustom<pcl::Poin tXYZ> source_handler(Source, 0, 0, 0); viewer.addPointCloud(Source, source_handler, "Source"); viewer.setBackgroundColor(255, 255, 255, 0); viewer.setPointCloudRenderingProperties(pcl::visualization ::PCL_VISUALIZER_POINT_SIZE, 1.5, "Source"); viewer.setPointCloudRenderingProperties(pcl::visualization ::PCL_VISUALIZER_POINT_SIZE, 1.5, "Target"); while (!viewer.wasStopped()) { viewer.spinOnce(); } } }; } Б.3. Файл Optimization.cpp #include "Optimization.h" using namespace Optimizer; pcl::PointCloud<pcl::PointXYZ>::Ptr Transformation::Transform(pcl::PointCloud<pcl::PointXYZ>::Ptr Target) { pcl::PointCloud<pcl::PointXYZ>::Ptr Source(new pcl::PointCloud<pcl::PointXYZ>); 51

Eigen::Affine3f Transformation = Eigen::Affine3f::Identity(); float theta = M_PI; Transformation.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitY())); //Transformation.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX())); Transformation.translation() << 70.0, 40.0, 90.0; pcl::transformPointCloud(*Target, *Source, Transformation); return Source; } Matrix4f Transformation::RigidTransform(const MatrixXf &Target, const MatrixXf &Source) { Vector3f CentroidForS(0,0,0); Vector3f CentroidForT(0,0,0); MatrixXf CenteredVectorsForS(Source.rows(),3); MatrixXf CenteredVectorsForT(Source.rows(),3); Matrix4f Transformation = MatrixXf::Identity(4, 4); //------------------------------------------------MatrixXf MatrixXf MatrixXf MatrixXf Matrix3f Vector3f U; S; V; Vt; R; t; //----------------------------------- Get Centroids //-- Get Sum of coordinates for (int i = 0; i < Target.rows(); i++) { CentroidForT += Target.block<1, 3>(i, 0).transpose(); CentroidForS += Source.block<1, 3>(i, 0).transpose(); 52

} //-CentroidForS /= Source.rows(); CentroidForT /= Source.rows(); //------------------------------------------------//---------------------------------- Get Centered Vectors for (int i = 0; i < Source.rows(); i++) { CenteredVectorsForS.block<1, 3>(i, 0) = Source.block<1, 3>(i, 0) - CentroidForS.transpose(); CenteredVectorsForT.block<1, 3>(i, 0) = Target.block<1, 3>(i, 0) - CentroidForT.transpose(); } //------------------------------------------------------S = CenteredVectorsForS.transpose()*CenteredVectorsForT; //---------------------------------- Singular Value Decomposition of S = AB^T JacobiSVD<MatrixXf> svd(S, ComputeFullU | ComputeFullV); U = svd.matrixU(); V = svd.matrixV(); //---------------------------------- Get Rotation R = V*U.transpose(); if (R.determinant() < 0) { V.transpose().block<1, 3>(2, 0) *= -1; R = V*U.transpose(); } //---------------------------------- Get Translation t = CentroidForT - R*CentroidForS; Transformation.block<3, 3>(0, 0) = R; Transformation.block<3, 1>(0, 3) = t; return Transformation; } 53

Б.4. Файл Optimization.h #pragma once #include <pcl/point_types.h> #include <pcl/common/transforms.h> #include <Eigen/Dense> using namespace Eigen; namespace Optimizer { class Transformation { public: pcl::PointCloud<pcl::PointXYZ>::Ptr Transform(pcl::PointCloud<pcl::PointXYZ>::Ptr Target); Matrix4f RigidTransform(const MatrixXf &Target, const MatrixXf &Source); }; } Б.5. Файл Registration.cpp #include #include #include #include <pcl/registration/icp.h> "Registration.h" "Optimization.h" <fstream> MyRegistration::MyRegistration() { } MyRegistration::~MyRegistration() { } 54

Matrix4f ICP::PointToPoint(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source) { EigenTarget = Read.getEigenMatrix(Target, 3); pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; kdtree.setInputCloud(Target); MatrixXf NNMatrix(3, EigenTarget.cols()); Matrix4f Result; MatrixXf Matrix(3, EigenTarget.cols()); ofstream file("file.txt"); vector<int> pointIdxNKNSearch(1); vector<float> pointNKNSquaredDistance(1); vector<float> value(100); float error = 0; for (int i = 0; i < 50; i++) { EigenSource = Read.getEigenMatrix(Source, 3); //--- kd Nearest Neighbour search for (int index = 0; index < Source->size(); index++) { if (kdtree.nearestKSearch(Source->points[index], 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { NNMatrix.block<3, 1>(0, index) = EigenTarget.block<3, 1>(0, pointIdxNKNSearch[0]); error = error + pointNKNSquaredDistance[0]; } } value[i] = error; file << value[i] << "\n"; cout << error << "\n"; //system("pause"); 55

//-------------------------------error = 0; Result = ForTest.RigidTransform(NNMatrix.transpose(), EigenSource.transpose()); //cout << Result; //cout << value; system("pause"); transformPointCloud(*Source, *Source, Result); } system("pause"); return Result; } Matrix4f ICP::PCL(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source) { IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setInputCloud(Source); icp.setInputTarget(Target); icp.setMaximumIterations(20); PointCloud<pcl::PointXYZ> Final; icp.align(Final); Matrix4f Result = icp.getFinalTransformation(); transformPointCloud(*Source, *Source, Result); return Result; } 56

Б.6. Файл Registration.h #pragma once #include "Optimization.h" #include "IO.h" #include #include #include #include <pcl/point_cloud.h> <pcl/kdtree/kdtree_flann.h> <vector> "nanoflann.hpp" #include <ctime> #include <cstdlib> using using using using namespace namespace namespace namespace std; pcl; Eigen; nanoflann; class MyRegistration { public: MyRegistration(); ~MyRegistration(); IO::PCDreader Read; IO::Visualization Viewer; Optimizer::Transformation ForTest; protected: // Composition for working with necessary class string File; int MaxIterations = 1; MatrixXf EigenTarget; MatrixXf EigenSource; virtual Matrix4f PointToPoint(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source) = 0; 57

virtual Matrix4f PCL(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source) = 0; }; class ICP : public MyRegistration { public: // Generalization of Constructor from Registration // Init FileName for Reading ICP() : MyRegistration(){} Matrix4f PointToPoint(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source); Matrix4f PCL(PointCloud<PointXYZ>::Ptr Target, PointCloud<PointXYZ>::Ptr Source); }; 58

Рецензии:

Авторизуйтесь, чтобы добавить рецензию

- у работы пока нет рецензий -

Отзывы:

Авторизуйтесь, чтобы оставить отзыв