NDT-based localization

Локализация с помощью NDT

Посвящается всем тем, кто хочет узнать, как работает алгоритм, не читая диссер на английском на 200 страниц [1].

Дисклеймер: некоторые определения или заявления могут быть не точными, для получения самой точной информации обратитесь к серьезным статьям

Оглавление

1. Об алгоритмах локализации с помощью 3D облаков точек, point cloud registration (введение)

2. Окунемся глубже в NDT (математика)

3. Когда хочется поэкспериментировать (NDT from c++ pcl)

4. Ну куда же без сравнения (финал)

5. Список источников

Об алгоритмах локализации с помощью 3D облаков точек, point cloud registration

Локализация - определение положения робота в пространстве. Алгоритмы локализации дают позицию и ориентацию робота на карте в каждый момент времени. Пусть карта дана в формате облака точек:

Пример карты-облака точек(data source: YellowScan)

Используемый источник информации о мире - лидар. При таких вводных задача локализации станет задачей поиска наилучшего соотвествия между облаком точек от лидара и облаком карты. Эта задача называется point‑cloud registration, и существует множество алгоритмов её решения. 

Сопоставление двух облаков точек. (data source: https://en.wikipedia.org/wiki/Point-set_registration )

Два самых часто встречающихся алгоритма point‑cloud registration при локализации робота - преобразование нормального распределения (NDT) и итеративное приближение точки (ICP). 

Основная идея ICP - итеративно искать соответствия между облаками, считая на каждом шаге расстояния между точками. Поиск заканчивается, когда расстояния соответствуют желаемому.

NDT использует нормальное распределение для моделирования облаков точек. Облако точек (карта) разбивается на кубики (или на квадраты в двумерном случае). Внутрь каждого кубика попадает какое-то количество точек облака, для них считается нормальное распределение. Для сходимости NDT нужна высокая точность начальной позы - положения и ориентации, от которого начинать сопоставлять облака. При получении лидарного скана считаются вероятности попадания в кубик для каждой точки облака от лидара, начиная от указанной начальной позиции. Алгоритм итеративно считает суммы этих вероятностей, каждый шаг алгоритма смещая лидарное облако в поисках максимальной суммы вероятностей. Процесс повторяется, пока не достигнет или желаемой точности, или не произведет заданное количество итераций.

Часто перед работой алгоритма карту и лидарное облако фильтруют воксельным фильтром, что помогает сократить количество точек. Меньше точек -> меньше считать.
Стол до фильтрации, точек настолько много, что он кажется сплошным:

Облако точек стола до применения воксельного фильтра. Стол состоит из 460400 точек. (data source: https://github.com/PointCloudLibrary/data/blob/master/tutorials/table_scene_lms400.pcd )

Прореженный стол:

Облако точек стола после использования voxel filter из pcl. Размер куба, на которые делилось пространство составил 0.01 м, количество точек сократилось до 41049.

Окунемся глубже в NDT (математика)

Первый шаг алгоритма - разделить карту на ячейки. Рассмотрим двумерный случай, то есть карта - разбросанные по плоскости точки:

Исходная карта (data source: Apex.AI)

Делим ее на квадраты:

Разбитая на квадраты карта (data source: Apex.AI)

Радуемся, что с первым шагом справились. Дальше немного (нет) сложнее. Второй шаг - посчитать нормальное распределение для точек внутри каждого квадрата. Для поиска распределения воспользуемся формулой многомерного нормального распределения:

Многомерное нормальное распределение µ - многомерный вектор, а Σ - неотрицательно определённая симметричная матрица, называемая ковариационной

Допустим, что в рассматриваемый нами в данный момент квадрат попало N точек. Мат ожидание и ковариация для этого квадрата:

Мат ожидание

Ковариация

Напоминание. Для плоскости нормальное распределение выглядит как эллипс. 

Иллюстрация двухмерного нормальное распределение (data source: Apex.AI)

То есть на втором шаге получается по эллипсу на каждый квадрат:

Заполнили квадраты двумерным нормальным распределением (data source: Apex.AI)

О, ура, нам пришли данные от лидара. Время найти себя в этом мире. Накладываем лидарный скан на обработанную карту, учитывая предполагаемую точку, в которой находится робот:

Лидарный скан как-то наложившийся на карту (data source: Apex.AI)

Начальное приближение получилось близким, но не очень, начинаем считать вероятности.

Функция плотности вероятности p(x) для квадрата С для рассматриваемой точки лидарного скана x связана с вероятностью попадания этой точки в квадрат C.

Если вероятность для точки получилась высокой, то она хорошо сопоставлена с квадратом. Но одной точки для выводов мало.

Сумма вероятностей точек быть в квадратах

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

Суммарная вероятность показывает: насколько хорошо робот себя нашел на карте по лидарному скану. Алгоритм стремится ее максимизировать. p - поза робота, то есть p = (x, y, θ) в двумерном случае, p = (x, y, z, φ, θ, ψ) в трехмерном.

- функция, которая переводит точку x_s из координатной системы датчика в координатную системы карты, используя позу p. 

Для двумерного случая функция перевода из координатных осей датчика в оси карты будет [1]:

В диссертации [1] берут сумму со знаком минус, чтобы вместо максимума искать минимум.

После этого шага можно выбирать любой метод оптимизации для поиска численного решения. Метод Ньютона [4] можно использовать для поиска позы p, которая оптимизируют score(p). Он итеративно решает уравнение H∆p = -g, где H - матрица Гаусса, g - вектор градиента score(p). Приращение ∆p добавляется к текущей оценке позы в каждой итерации, так что p ← p + ∆p [1].

Последний шаг алгоритма - минимизация score(p) путем поиска истинной позиции, следовательно, сдвигания точек лидарного облака :

Ищем позицию, которая оптимизирует score (data source: Apex.AI)

Тадааааам:

Облака выровнялись (data source: Apex.AI)

Как это же выглядит с 3D облаками точек при локализации автономного автомобиля:

Облака выровнялись 2.0 (data source: Apex.AI)

Синим цветом показано облако от лидара автомобиля, белое облако - карта. Картинка взята из презентации одной лекций курса от Apex.AI [3].

Вымученный алгоритм работы NDT в двумерном случае [1]. Для трехмерного случая алгоритм работает также.

Алгоритм NDT

Когда хочется поэкспериментировать (NDT from c++ pcl)

Самый простой и быстрый способ запустить NDT - использовать библиотеку pcl. Начинаем с фильтрации карты:

pcl::PCLPointCloud2::Ptr map (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr map_filtered (new pcl::PCLPointCloud2 ());
pcl::PCDReader reader;
std::string file_path = "map.pcd";
reader.read(file_path, *map); 
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (map);
float leafSize = 0.01;
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*map_filtered);

Создание и заполнение параметрами объекта для работы с NDT:

double ndt_resolution = 5.0;
double transform_epsilon = 0.01;
int ndt_max_iter = 100;
double ndt_step_size = 0.1;

boost::shared_ptr<pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>> ndt( new pcl::NormalDistributionsTransform <pcl::PointXYZI, pcl::PointXYZI>());

ndt->setResolution(ndt_resolution);
ndt->setTransformationEpsilon(transform_epsilon);
ndt->setMaximumIterations (ndt_max_iter);
ndt->setStepSize(ndt_step_size);
ndt->setInputSource (lidar_cloud);
ndt->setInputTarget (map_filtered);

Пройдемся по параметрам и узнаем их в описанном выше алгоритме. Первой по алгоритму идет разбиение на ячейки (кубы/квадраты), за размер ячейки отвечает параметр ndt_resolution. В документации к библиотеки пишут, что следует его выбрать настолько большим, чтобы в каждой ячейке было хотя бы 6 точек, но достаточно малым, чтобы среда была определена однозначно. Дальше параметры для ограничения времени работы. transform_epsilon определяет минимальное приращение позиции, когда приращение станет меньше, чем этот порог, алгоритм остановится. То есть это некая точность, ее можно воспринимать, как разность между точками облака карты и облака лидара, к которой стремится алгоритм, чем она меньше, тем дольше алгоритм будет сходиться. К transform_epsilon можно стремиться бесконечно и никогда не прийти, чтобы такого не произошло задается ndt_max_iter. ndt_max_iter говорит о том, сколько максимально итераций нужно выполнить на пути к заданной точности. Если не успели, то и ладно, надо остановиться, отдать то, что получилось. С каким шагом идти по пространству задет ndt_step_size. Согласно вышеупомянутой документации ndt_step_size - максимальная длина шага, алгоритм выбирает лучший шаг, не превышающий заданный. 

Запуск!

Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//с начальной позой
ndt->align (*output_cloud, init_guess);
//без нее 
ndt->align (*output_cloud);

Теперь осталось перевести полученный результат в положение робота:

Eigen::Matrix4f final_transformation = ndt->getFinalTransformation();
Eigen::Matrix3d rot_mat = final_transformation.block<3, 3>(0, 0).cast<double>();
Eigen::Quaterniond quat_eig(rot_mat);
position.x = static_cast<double>(final_transformation(0, 3));
position.y = static_cast<double>(final_transformation(1, 3));
position.z = static_cast<double>(final_transformation(2, 3));
pose.orientation = quat_msg;

Для предложенных туториалами pcl файлов с комнатами получились результаты:

Выравнивание без начальной позы

Выравнивание с начальной позой, параметры не менялись

Для быстро едущего и вращающегося робота обычного NDT из pcl может быть недостаточно. Ситуацию может спасти его многопоточная реализация - NDT_OMP, которую легко найти в открытом доступе [5].

Ну куда же без сравнения (финал)

Autoware.Auto разработали свою локализацию на основе NDT алгоритма, код находится в открытом доступе. При разработке они провели исследование алгоритма, используя статьи, дали сравнительную характеристику для подвидов NDT, сравнили NDT и ICP. Источники для каждого утверждения указаны, если любопытство не позволит остановиться в исследовании NDT. 

Выводы сравнения NDT и ICP:
на основании [1]:
NDT быстрее и стабильнее ICP
NDT несколько более надежен, чем ICP, но требует хорошей инициализации
Color-NDT лучше чем обычный NDT
на основании [8]:
NDT хорош в обработке переводов и является одним из самых быстрых методов
NDT так себе при обработке вращений

P2P и D2D:
на основании [6]:
D2D-NDT быстрее, чем P2D-NDT
на основании [7]:
P2D более надежен для неструктурированных сред и небольших перекрытий между сканированиями по сравнению с D2D
D2D быстрее и менее чувствителен к неверным первоначальным оценкам

Список источников

1. The Three-Dimensional Normal-Distributions Transform --- an Efficient Representation for Registration, Surface Analysis, and Loop Detection. Martin Magnusson.  Доступно

2. Comparative Analysis of 3D LiDAR Scan-Matching Methods for State Estimation of Autonomous Surface Vessel. Haichao Wang, Yong Yin and Qianfeng Jing. Доступно

3. Self-Driving Cars with ROS 2 & Autoware. Доступно

4. Метод Ньютона

5. Одна из реализаций ndt_omp

6. Fast and Accurate Scan Registration through Minimization of the Distance between Compact 3D NDT Representations. Todor StoyanovMartin MagnussonMartin MagnussonHenrik AndreassonHenrik AndreassonAchim J. LilienthalAchim J. Lilienthal

7. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms. Martin Magnusson, Narunas Vaskevicius, Todor Stoyanov, Kaustubh Pathak, and Andreas Birk

8. Point cloud registration from local feature correspondences—Evaluation on challenging datasets. Tomas Petricek ,Tomas Svoboda