Сегментация поверхности земли
Определение плоскости земли
Введение
В статье мы рассмотрим подходы к решению задачи определения плоскости земли с помощью данных лидара. Давайте изучим саму задачу подробнее.
Для начала определим входные данные и ожидаемый результат. Как показано на рисунке ниже, на входе мы получаем облако точек (точки объектов + точки поверхности, по которым перемещается робот, то есть транспортное средство с установленным лидаром). Нам нужно классифицировать данные точек на два класса, отделив точки объектов от точек поверхности.
Получившийся результат поможет нам при решении различных задач. К ним относятся:
- Нахождение объектов путем кластеризации облака точек
При использовании алгоритмов кластеризации с данными лидара без предварительной фильтрации из множества точек земли будет сформировано разное количество кластеров, которые потребуют дополнительной классификации. Для ее обхода перед кластеризацией отделяются точки земли, а это, в свою очередь, позволяет использовать результаты кластеризации как готовый набор объектов, снижает время исполнения алгоритмов за счет сокращения количества точек во входных данных.
- Использование в Lidar SLAM
На изображении выше этап выделения точек земли отмечен как Ground Segmentation. В рамках SLAM результаты нахождения земли можно использовать для уменьшения количества входных точек (что снижает время исполнения всего пайплайна) или получения наиболее релевантных точек для стадии мэтчинга.
- Формирование карты проходимости для робота
На рисунке мы видим пример карты занятости (проходимости), построенной путем обработки лидарного скана местности алгоритмом нахождения точек земли. С его помощью было получено разделение всех точек облака на свободные (отмечены красным цветом) и занятые (зеленым). Подобную карту занятости затем можно использовать в модуле построения маршрута движения робота.
- Поиск высоты объекта в задаче 3D object detection
В подходе Birdnet для обнаружения объектов используется BEV проекция (подробнее про виды представления данных можно прочитать здесь) облака точек, которая не позволяет предсказывать высоту объекта с высокой точностью. Для улучшения метрик по качеству авторы применяют ранее выделенные точки земли с целью вычисления высоты предсказанных bounding box.
Есть много разных подходов для определения плоскости земли. Их примеры представлены на рисунке ниже.
В этой статье мы рассмотрим два классических алгоритма использования нейронных сетей, основанных на подходе Ground modeling: СSF и Patchwork.
Алгоритм CSF (Cloth simulation fitler)
CSF метод базируется на идеи аппроксимации плоскости тканью — объектом с заданными физическими свойствами. Его основное преимущество заключается в возможности моделирования подобного объекта как множества физических частиц, соединенных между собой. Это позволяет аппроксимировать с высокой точностью даже самые сложные ландшафты с существенным перепадом высот.
Авторы определили идею аппроксимации как основу алгоритма CSF. Для реализации метода они обратились к результатам компьютерной графики с целью решения задачи симуляции ткани. В этой области есть много методов симуляции таких объектов. Авторы выбрали среди них модель ”Mass-Spring” (модель “Масс-Пружин”).
Модель “Mass-Spring”
Построение модели ткани начинается с определения ее структуры и элементов. Как правило, ее представляют в виде регулярной сетки размером nxm ячеек (структура), описываемых частицами и пружинами (элементы):
- Частицы отмечены на рисунке выше оранжевыми кружками, каждая индивидуальная частица имеет свою скорость смещения, позицию и величину массы;
- Пружины отмечены синими соединительными линиями, каждая из видов (struсtural, shear, flexion) пружин определяет изменение полученной сетки при приложении различных сил.
Опираясь на 2 закон Ньютона , зависимость между позицией частиц и прикладываемыми силами определяется следующим выражением:
\( m\dfrac{\partial X(t)}{\partial t^2} = F_{ext}(X,t) +F_{int}(X,t) \)
\( где\ m — значение\ массы \ частицы\ X-позиция\ частицы\ в\ момент\ времени\ t, \)
\( F_{ext}(X,t) — внешние\ силы, \ F_{int}(X,t) -внутренние\ силы\ \)
Внутренние силы — изменение положения частицы за счет физического растяжения (сужения) пружин сетки. Их можно представить таким образом:
\( \mathbf{F}_{\text{spring}} = K(L_0 — \| \mathrm{p} — \mathrm{q} \|) \frac{\mathrm{p} — \mathrm{q}}{\| \mathrm{p} — \mathrm{q} \|} \)
\( где\ K — параметр \ жесткости, L_0 — длина\ для\ struсtural\ пружин \)
\( p\ и\ q — индексы\ позиции\ частицы \)
К внешним силам относятся:
- сила гравитации
\( \mathbf{F}_{\text{gravity}} = \begin{pmatrix} 0\\ -mg\\ 0 \end{pmatrix} \)
\( g-значение \ гравитации,m — значение\ массы \ частицы \)
- сила затухания колебаний
\( \mathbf{F}_{\text{damp}} = -c_d\,\mathbf{v} \)
\( где\ c_d — константное\ значение > 0 \)
В зависимости от сложности модели количество внутренних и внешних сил может варьироваться.
Применение модели в алгоритме CSF
Итак, мы получили модель ткани данного метода. Теперь давайте перейдем к ее практическому применению. Авторы предлагают следующий подход.
На первом шаге инвертируется исходное облако точек. Происходит процесс наложения модели ткани:
Для вычисления финального расположения ткани применяется следующий подход:
То есть у нас есть три шага вычисления финального расположения ткани:
A. Инициализация модели регулярной сетки и ее расположение на заданной высоте;
B. Вычисление положения и скорости частиц при помощи формулы (внутренние силы равны нулю):
\( где\ t — текущий \ момент\ времени\ ∆t — величина\ смещения\ во\ времени, \)
\( m — величина\ массы\ частицы, \)
\( G — значение\ гравитации \)
Если частица сталкивается с точкой облака, то мы фиксируем ее позицию в пространстве, тогда ее движение прекращается. Мы отмечаем эту частицу (точку) исходной сетки как недвижимую.
C. Вычисление изменения положения (за счет внутренних сил) тех точек, которые мы не отметили на этапе B:
\( \vec{d} \) = \( \sum_{RT=1} \) \( \dfrac{1}{2}^{RT} b( \vec{p_{i}} − \vec{p_{0}} ) * \vec{n} \)
\( где\ b — бинарное\ значение\ состояния \ частицы обозначает\ движимая\ частица\ или\ нет, \)
\( p_{0} — текущее\ положение\ частицы ,\)
\( p_{i} — положение\ соседней\ соединенной\ частицы,\)
\( RT — коэффициент \ изменения\ вертикального\ положения\ частицы,\)
\( n=(0, 0, 1)^T \)
Параметр RT оказывает основное влияние на результат. Его необходимо задавать исходя из свойств данных, к которым применяется алгоритм. Чем больше исходное облако похоже на гладкую плоскость, тем больше должен быть RT. На рисунке ниже показан выбор этого параметра:
Для реализации алгоритма на практике нужно:
- Задать сетку полотна ткани и в качестве z координаты каждой из частиц установить значение высоты согласно максимальной высоте точек в перевернутом облаке;
- Представить точки сетки и точки облака в виде BEV проекции;
- Сформировать пары из точек сетки и точек лидарного облака, используя расстояние в качестве метрики сходства;
- Выполнить вычисление шага B. Если координаты высоты частицы ниже или равны координатам ранее определенной точки облака, то данная частица отмечается как недвижимая, мы меняем ее высоту на высоту точки облака;
- Выполнить вычисление шага C заданное количество итераций, каждый раз обновляя статусы тех точек, которые на предыдущем шаге не были отмечены как недвижимые;
- Присвоить точкам метки классов — плоскость земли или объект.
Рассмотрим пример работы алгоритма на реальных данных:
Плюсы и минусы алгоритма на практике
Плюсы:
- возможность аппроксимировать плоскости практически любой сложности;
- небольшое количество настраиваемых параметров.
Минусы:
- высокая чувствительность к выбросам в данных;
- большое количество вычислительных ресурсов при высоком разрешении регулярной сетки;
- real-time решения: к ним этот алгоритм часто не подходит.
Алгоритм Patchwork
Patchwork метод базируется на анализе облака точек как набора патчей — локальных областей. Это дает следующие преимущества:
- Все необходимые операции можно производить параллельно;
- Для каждой из локальных областей можно использовать набор наиболее подходящих параметров.
Давайте рассмотрим алгоритм подробнее:
В рамках алгоритма Patchwork выделяются четыре стадии:
- Разделение облака точек на локальные области и распределение точек по ним (Concentric Zone Model);
- Поиск плоскости земли путем аппроксимации каждой из областей плоскостью (R-GPF);
- Дополнительная проверка точек на принадлежность земли с помощью вычисления функции правдоподобия (GLE);
- Формирование финального разбиения на точки земли и объектов.
Теперь рассмотрим каждую стадию подробней.
Concentric Zone Model
Для использования этой модели описания облака точек нам нужно преобразовать координаты из декартовой системы в полярную:
\( r=\sqrt {x^2+y^2} \)
\( \theta =\tan^{−1}\frac{y}{x} \)
Авторы вводят переменные, определяющие результат:
- rings изображены кругами на рисунке ниже, размер определяется дистанцией в метрах;
- sectors — промежутки на каждом из колец, размер определяется значением полярного угла;
- zones отмечены различными цветами, принадлежность к зоне определяется дистанцией точки от центра координат.
Плотность точек лидарного облака уменьшается при увеличении дистанции, поэтому авторы используют разные параметры для каждой из четырех зон. В частности, размер кольца и полярный угол в ближней зоне существенно меньше, чем в дальней. Это позволяет более точно оценивать нужные параметры на 2 и 3 стадиях ввиду существенно большего количества точек.
Далее определяем ранее указанные параметры согласно представленному псевдокоду и компонуем точки по уникальным группам.
R-GPF (Region-wise Ground Plane Fitting)
Итак, мы получили группы точек, теперь нам нужно оценить плоскость земли. Для этого авторы используют следующее уравнение:
У нас есть значения нормалей и центроида. С их помощью мы можем выразить d — коэффициент плоскости, который потребуется в дальнейших шагах.
В геометрическом смысле d имеет следующее значение:
На изображении выше коэффициент плоскости d — длина перпендикуляра от точки до плоскости.
Как правило, в облаке имеется довольно большая вариация точек по их координатам. Это приводит к шумной оценке уравнения плоскости при рассматривании всех возможных точек. Авторы предлагают следующий подход для формирования целевого подмножества точек:
\( \hat{G}^{0}{n}=\left\{ \mathbf{p}{k} \in S_{n} \mid z(\mathbf{p}{k}) < \bar{z}{\text{init}} + z_{\text{seed}} \right.\} \)
\( где\ \mathbf{p}{k} — точка\ множетва\ S{n} \)
\( z(\mathbf{p}{k}) — значение\ z\ точки \)
\( \bar{z}{\text{init}} — среднее\ значение\ z\ для\ точек\ из\ S_{n} \)
\( z_{\text{seed}} — значение\ отступа \)
Наша основная гипотеза: точки с минимальной высотой, вероятнее всего, являются точками земли. В дальнейших шагах мы будем использовать те точки исходного облака, которые по координате z (высоте) меньше средней высоты в облаке + пороговое значение (может быть отрицательным).
Для определения значений нормалей и центроида у каждой полученной группы точек нужно:
- Вычислить центроид и ковариационную матрицу;
- Вычислить собственные векторы и значения матрицы ковариаций.
В геометрическом смысле мы получаем:
На рисунке выше центр координат — вычисленный нами центроид, а оси — собственные векторы матрицы ковариаций. Значения самого наименьшего из них по вариации — искомая нормаль.
Переходим к классификации всех точек облака. Для этого обращаемся к данному подходу:
\( \hat{G}^{l+1}{n}=\left\{\mathbf{p}{k} \in S_{n} \mid {d}^{l}{n} — \hat{{d}}{k} < M_{d} \right.\} \)
\( где\ \mathbf{p}{k} — точка\ множества\ S{n} \)
\( {d}^{l}{n} — коэффициент\ плоскости\ для\ центроида\ облака\ \)
\( {{d}}{k}- коэффициент\ плоскости \ для\ точки\ облака\ \)
\( M_{d} — пороговое\ значение\ для\ классификации \)
Теперь используем ранее полученные нормали и оцениваем разницу между коэффициентами плоскости. После оценки убираем из множества точки выше земли, которые мы определили на данном этапе. Для более точной оценки можно повторить предыдущие шаги еще некоторое количество итераций, каждый раз удаляя точки, не относящиеся к плоскости земли.
И вот сейчас мы можем переходить к следующему этапу! 🙂
GLE
Для уточнения начальной классификации авторы реализовали следующий функционал правдоподобия:
\( \mathcal{L}(\theta \mid \mathcal{X})=f(\mathcal{X} \mid \theta) = \prod_{n} f(\mathcal{X}{n} \mid \theta{n}) \)
\( f(\mathcal{X}n \mid \theta_n) \equiv \phi({\mathbf{v}}{3, n}) \cdot \psi(\bar{z}_n,r_n) \cdot \varphi(\psi(\bar{z}_n, r_n), \sigma_n) \)
\( где\ \phi — uprightness, \psi -elevation , \varphi -flatness \)
Применять его необходимо только к тем точкам, которые мы классифицировали как землю. Давайте подробнее разберем компоненты формулы.
Uprightness (вертикальность) была введена авторами, которые исходили из следующего предположения — вектор нормали облака точек земли должен быть близким перпендикуляру плоскости по углу наклона. Базируясь на данном предположении, они сформировали критерий:
\( где\ {\mathbf{v}}{3, n} — вектор\ нормали,{\mathbf{z}}=[0 \; 0 \;1], \)
\( \theta{\tau} — параметр\ отступа \)
Применяем этот критерий и проверяем: угол к плоскости у вектора нормали больше 90°, параметра отступа (авторы задают его как 45°). В случае 1 мы продолжаем подсчет дальнейших компонентов исходной формулы, в случае 0 — относим все точки к точкам выше земли.
Elevation (возвышенность) была введена авторами для фильтрации тех случаев, когда точками плоскости могут быть точки высоко располагающихся объектов (крыши или капоты машин). Эти объекты достаточно плоские для классификации по критерию uprightness как точки земли. Для проверки полученных точек земли на подобные свойства используется следующий критерий:
\( где\ \bar{z}n — cредняя\ высота\ точек \)
\( r{n}- дистанция\ центроида\ от\ начала\ координат \)
\( \kappa (\cdot) — экспонециально\ возрастающая\ функция \)
\( L_{\tau} — пороговое\ значение\ дистанции \)
При выполнении условия указанный алгоритм позволяет получить значение индикатора, которое определяет правдоподобность средней высоты точек облака при нахождении их центроида на определенной дистанции. Если средняя высота точек меньше результата вычисления функции k(r), то результат всего выражения будет больше или равен 0.5. Подразумевается что чем меньше полученное значение тем менее вероятно что точки относятся к плоскости земли
Flatness (ровность) была введена авторами на тот случай, если перед нами находится крутой подъем, например, когда в данных есть заезд робота на возвышенность. Здесь мы можем иметь большое среднее значение z на ближнем расстоянии. Это даст низкое значение критерия elevation. Для поиска таких ситуаций во входных данных авторы разработали критерий:
\( где\ \zeta — значение\ магнитуды > 1 \)
\( \sigma_n — значение\ вариации\ плоскости \)
\( \sigma_{\tau,m} — пороговое\ значение\ для\ конкретной\ зоны \)
Данный алгоритм применяется при меньшем 0.5 результате вычисления elevation. Магнитуда в этом случае — множитель, определяющий вклад разности между значением вариации плоскости и пороговым значением. Для подсчета нам нужно найти значение вариации плоскости:
\( \sigma_n = \frac{\lambda_{3,n}}{\lambda_{1,n} + \lambda_{2,n} + \lambda_{3,n}} \)
\( где\ \lambda_1 \geq \lambda_2 \geq \lambda_3 — собственные\ значения\ ковариационной\ матрицы\ облака \)
В геометрическом смысле мы получаем:
На изображении выше представлена трехмерная голова статуи. После подсчета собственных значений для каждой точки и ее окрестности (здесь соседними считаются 20 ближайших точек) и вычисления значения вариации мы получаем цветовую карту. Точки на ней окрашены в зависимости от величины полученных значений. В местах резкого изменения высот мы имеем наибольшее значение вариации.
Итоговая классификация
Ура, мы вычислили все 3 критерия функции правдоподобия! Сейчас нам нужно сравнить результат с заданным порогом по формуле:
\( \hat{G}=\bigcup_{n\in\left<N_{\mathcal{C}}\right>} \left[ f(\mathcal{X}n \mid \theta_n) > 0.5 \right] \hat{G}{n} \)
Таким образом, мы получили финальную классификацию точек.
Плюсы и минусы алгоритма на практике
Плюсы:
- высокая скорость исполнения (вплоть до 500 fps);
- хорошая чувствительность к малым объектам при тонкой настройке;
- интерпретируемость.
Минусы:
- авторы в своей работе используют ряд пороговых значений для всех зон, но на практике такой настройки может быть недостаточно, тогда потребуется калибровка порогов под каждую зону;
- шаг GLE нужно тонко настроить, так как при неправильной классификации все точки локальной зоны станут считаться точками объектов;
- для тонкой настройки необходимо реализовать дополнительное ПО визуализации.
Заключение
В этой статье нам удалось разобрать такие пункты, как:
- мотивация решения задачи определения плоскости земли (зачем решать?);
- алгоритм CSF и алгоритм Patchwork (что это и как они реализуются?);
- практические советы по применению этих алгоритмов (что там с этими алгоритмами на практике?).
В следующей части мы разберем решение аналогичной задачи, но уже с помощью нейронных сетей.
