SLAM на Java с OpenCV: сравнение алгоритмов автономной навигации

Меня зовут Бромбин Андрей, я студент МГТУ имени Баумана. В этой статье я расскажу, как погружался в исследование алгоритмов автономной навигации.

Введение

Визуальная одометрия играет ключевую роль в навигации малогабаритных беспилотных летательных аппаратов (БЛА), особенно в условиях, где GPS недоступен или его точность ограничена. В этой статье я делюсь результатами исследования популярных алгоритмов визуальной одометрии, реализованных на Java с использованием OpenCV. Мы сравним точность и производительность методов ORB, R2D2, SIFT и их комбинаций, а также оценим их пригодность для систем навигации БЛА.

Постановка задачи

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

Алгоритмы для анализа:

• SIFT (Scale-Invariant Feature Transform)

• SIFT + FOF (Farneback Optical Flow)

• ORB (Oriented Fast and Rotated BRIEF)

• R2D2 и Faster2D2

Данные для исследования

Мною был выбран dataset KITTI – набор данных для исследования в области визуальной одометрии. Потому что он содержит самые важные данные: параметры камер, радаров, лидаров, навигационной системы, которые позволяют совершать исследования в самых обширных областях. KITTI является оптимальным решением в области исследования методов детектирования ключевых точек, так как содержит качественные, точные и обширные характеристики каждого кадра. На самом деле датасет’ов с полноценным набором необходимых данных не так много, и старый добрый KITTI выручает.

Теоретическая основа

Simultaneous Localization and Mapping (SLAM) — это технология, позволяющая одновременно определять местоположение устройства и строить карту окружающей среды.

Формализация задачи оценки движения и позиции включает в себя определение математических моделей, позволяющих дрону определять свое перемещение и позицию относительно окружающей среды. Это важно для достижения точной и стабильной навигации без использования GPS.

Методы компьютерного зрения играют ключевую роль в визуальной одометрии для навигации дронов.

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

Фундаментальная матрица F – математическая абстракция, представляющая собой связь между камерами в трёхмерной сцене. Она описывает геометрические свойства проекции точек из одного положения камеры в другое, играя важную роль в задачах компьютерного зрения. Представляет собой матрицу 3×3, обозначаемую как F и описываемую формулой:

где R — матрица поворота,
K — матрица калибровки камеры:

где ( f_x, f_y ) — фокусные расстояния,
s — параметр наклона осей координат,
( c_x, c_y ) — координаты оптического центра.

Кососимметричная матрица [ t_x ] вычисляется следующим образом:

где ( x, y, z ) — координаты вектора смещения.

Альтернативный способ вычисления F

Фундаментальная матрица также удовлетворяет следующему уравнению:

где x и x′ — координаты соответствующих ключевых точек на двух изображениях.

Расстояние до эпиполярной линии

Метрики точности рассчитываются на основе расстояния до эпиполярных линий. Оно определяется как:

где d( x, l) — расстояние от точки до эпиполярной линии lll, вычисляемое по формуле:

где l = [a, b, c] — параметры эпиполярной линии,
x и x’ — координаты точек на изображениях.

В формуле (6), l₁ и l₂ используются для нормализации расстояния, учитывая параметризацию эпиполярной линии. Пример расстояний до эпиполярной линии представлен на рисунке 1.

Подробнее про конкретные реализации SLAM: ORB, SIFT, r2d2 поясню в следующих статьях по вашей заинтересованности.

Методология

Эксперименты включали два сценария:

1. Малое межкадровое смещение: пары изображений со смещением ~1 м.

2. Большое межкадровое смещение: пары изображений со смещением ~10 м.

Основные этапы:

• Выделение ключевых точек.

• Сопоставление и фильтрация точек (например, тест Лёва).

• Вычисление фундаментальной матрицы и эпиполярных линий.

• Оценка точности алгоритмов с использованием средних и медианных расстояний до эпиполярных линий.

Java реализация с использование openCV на примере SIFT

Стоит оговориться, что код написан исключительно в исследовательских целях. Например, отсутствие логирования, api и так далее — не предусмотрены в рамках исследования.

Блок схема:

Создание матрицы камеры:

public static Mat createCameraMatrix(double fx, double fy, double cx, double cy) {     Mat cameraMatrix = new Mat(3, 3, CvType.CV_64F);     cameraMatrix.put(0, 0, fx);     cameraMatrix.put(1, 1, fy);     cameraMatrix.put(0, 2, cx);     cameraMatrix.put(1, 2, cy);     cameraMatrix.put(2, 2, 1);     return cameraMatrix; } 

Этот метод создает матрицу камеры K с параметрами фокусного расстояния и координат оптического центра. Используется для преобразования изображений и последующего вычисления фундаментальной матрицы.

Вычисление фундаментальной матрицы:

Для улучшения описания этого метода в статье, уточним, что библиотека OpenCV предоставляет свой метод для расчета фундаментальной матрицы, а также уточним, что T_cam1_cam0 относится к трансформации между кадрами 1 и 2, а не просто двум камерам.

public static Mat computeFundamentalMatrix(Mat T_cam1_cam0, Mat cameraMatrix0) {         Mat shift = T_cam1_cam0.col(3).rowRange(0, 3);         Mat skew = new Mat(3, 3, CvType.CV_64F);         skew.put(0, 0, 0, -shift.get(2, 0)[0], shift.get(1, 0)[0],              shift.get(2, 0)[0], 0, -shift.get(0, 0)[0],              -shift.get(1, 0)[0], shift.get(0, 0)[0]);            Mat rotation = T_cam1_cam0.submat(0, 3, 0, 3);         Mat ess = new Mat();         Core.gemm(skew, rotation, 1.0, new Mat(), 0.0, ess);          Mat transposedMtx = new Mat();         Core.transpose(cameraMatrix0, transposedMtx);            Mat invMtx = new Mat();         Core.invert(transposedMtx, invMtx);          Mat temp = new Mat();         Core.gemm(invMtx, ess, 1.0, new Mat(), 0.0, temp);         Mat fundamentalMatrix = new Mat();         Core.gemm(temp, cameraMatrix0.inv(), 1.0, new Mat(), 0.0, fundamentalMatrix);          return fundamentalMatrix;     }

Также можно добавить описание, что в OpenCV есть встроенный методCalib3d.findFundamentalMat, который предоставляет более удобный способ вычисления фундаментальной матрицы.

Вычисление эпиполярных расстояний:

public static Mat calculateEpipolarDistances(MatOfPoint2f points0,                                               MatOfPoint2f points1,                                               Mat lines0, Mat lines1) {     int numPoints = (int) points0.size().height;     Mat distances = new Mat(numPoints, 1, CvType.CV_64F);     List<Point> pointsList0 = points0.toList();     List<Point> pointsList1 = points1.toList();     List<Double> distancesList = new ArrayList<>();      for (int i = 0; i < numPoints; i++) {         Point point0 = pointsList0.get(i);         Point point1 = pointsList1.get(i);         double d1 = calculateDistance(point0, lines1.row(i));         double d2 = calculateDistance(point1, lines0.row(i));         distancesList.add(d1 + d2);     }      MatOfDouble distancesMat = new MatOfDouble();     distancesMat.fromList(distancesList);     distancesMat.copyTo(distances);      return distances; }

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

Вывод статистики из расстояний:

public static MyPair calculateAndPrintStatisticsForMat(Mat distances) {     Collections.sort(distances);     double sum = 0;     for (Double distance : distances.toList()) {         sum += distance;     }     double mean = sum / distances.rows();      double median;     if (distances.rows() % 2 == 0) {         median = (distances.get(distances.rows() / 2, 0)[0] +                    + distances.get(distances.rows() / 2 - 1, 0)[0]) / 2;     } else {         median = distances.get(distances.rows() / 2, 0)[0];     }      return new MyPair(mean, median); }

Main Class

Загрузка библиотеки OpenCV и создание матрицы камеры:

    System.loadLibrary(Core.NATIVE_LIBRARY_NAME);      double fx = 501.4757919305817;     double fy = 501.4757919305817;     double cx = 421.7953735163109;     double cy = 167.65799492501083;      Mat cameraMatrix0 = createCameraMatrix(fx, fy, cx, cy);     System.out.println("Camera Matrix:");     System.out.println(cameraMatrix0.dump());

Этот этап загружает библиотеку OpenCV и создает матрицу камеры с параметрами фокусного расстояния и координатами оптического центра.

Загрузка изображений и преобразование их в серый цвет:

    File[] listOfFiles = folder.listFiles();      for (int i = 0; i < listOfFiles.length - 31; i++) {         String imagePath0 = listOfFiles[i].getAbsolutePath();         String imagePath1 = listOfFiles[i + 30].getAbsolutePath();         Mat image0 = Imgcodecs.imread(imagePath0);         Mat image1 = Imgcodecs.imread(imagePath1);          if (image0.empty() || image1.empty()) {             System.err.println("One of the images is empty or could not be read.");             continue;         }          Imgproc.cvtColor(image0, image0, Imgproc.COLOR_BGR2GRAY);         Imgproc.cvtColor(image1, image1, Imgproc.COLOR_BGR2GRAY);     }

Здесь загружаются изображения из указанной папки, и оба изображения преобразуются в серый цвет средствами OpenCV.

Обнаружение ключевых точек и сопоставление дескрипторов:

    MatOfKeyPoint keypoints0 = new MatOfKeyPoint();     MatOfKeyPoint keypoints1 = new MatOfKeyPoint();     SIFT sift = SIFT.create();     sift.detect(image0, keypoints0);     sift.detect(image1, keypoints1);      Mat descriptors0 = new Mat();     Mat descriptors1 = new Mat();     sift.compute(image0, keypoints0, descriptors0);     sift.compute(image1, keypoints1, descriptors1);

Метод SIFT.create() используется для обнаружения ключевых точек, а также вычисляются дескрипторы для изображений с помощью OpenCV.

Сопоставление ключевых точек:

    DescriptorMatcher matcher = DescriptorMatcher.create(DescriptorMatcher.BRUTEFORCE);     MatOfDMatch matches = new MatOfDMatch();     matcher.match(descriptors0, descriptors1, matches);      MatOfDMatch goodMatches = new MatOfDMatch();     List<DMatch> matchesList = matches.toList();     double minDist = 100.0;     for (DMatch match : matchesList) {         if (match.distance < minDist) {             minDist = match.distance;         }     }      for (DMatch match : matchesList) {         if (match.distance < 2 * minDist) {             goodMatchesList.add(match);         }     }      goodMatches.fromList(goodMatchesList);

В этом этапе выполняется сопоставление дескрипторов с использованием BruteForce Matcher, а затем отбираются хорошие (наименьшие) совпадения.

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

    Mat lines0 = new Mat();     Mat lines1 = new Mat();     Mat fundamentalMatrix = computeFundamentalMatrix(cameraMatrix0, new Mat());     if (fundamentalMatrix.size().equals(new Size(3, 3))) {         Calib3d.computeCorrespondEpilines(keypoints0, 1, fundamentalMatrix, lines0);         Calib3d.computeCorrespondEpilines(keypoints1, 2, fundamentalMatrix.t(), lines1);          Features2d.drawMatches(image0, keypoints0, image1, keypoints1, goodMatches, outputImage);         HighGui.imshow("Matches", outputImage);         HighGui.waitKey();     } else {         System.out.println("Ошибка: Неверный размер фундаментальной матрицы.");     }

Здесь рассчитываются эпиполярные линии, и результат отображается на экране с использованием метода HighGui.imsho

Результаты исследования

Описание экспериментов

Были выбраны 5 эффективных на сегодняшний день алгоритмов для исследования: SIFT, SIFT+FOF (Farneback Optical Flow), ORB, r2d2 и его ускоренная версия faster2d2.

Стоит отметить, что FOF не является методом детектирования, но я включил его в исследуемые методы, поскольку он с субпиксельной точностью предсказывает для второго изображения положение ключевых точек, выделенных на первом изображении, что позволяет увеличить точность детектирования методом SIFT.

ORB не входит в число высокоточных методов, но является одним из самых быстрых алгоритмов. Хоть ORB, очевидно, проиграет по точности остальным, я включил его в исследование, поскольку полезно оценить его характеристики и узнать, насколько именно он проигрывает.

Для каждого из выбранных методов было проведено по 2 эксперимента:

Эксперимент с серией пар изображений с малым межкадровым смещением. Выбранный набор данных содержал 2000 изображений, из которых были сформированы пары таким образом, чтобы среднее расстояние в процессе съёмки между кадрами составляло в среднем 1 метр.

Эксперимент с серией пар изображений с большим межкадровым смещением.
Я получил данную серию, создав пары из 2000 изображений таким образом, чтобы межкадровое смещение между двумя изображениями вновь образованных пар составляло порядка 10 метров. Для этого я формировал пары, пропуская каждые 30 изображений первоначальной серии, то есть создал пары из изображений: №0 — №30, №1 — №31 и так далее.

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

Результаты для серии пар изображений с малым межкадровым смещением:

В результате работы математической модели были получены целевые метрики по всем алгоритмам для малого межкадрового смещения (таблица 1).

Таблица 1 – целевые метрики для изображений с малым межкадровым смещением.

Как видно из таблицы 1, для серии изображений с малым межкадровым смещением метод SIFT + FOF показал максимальную точность идентификации ключевых точек, то есть минимальное медианное расстояние от точек до эпиполярных линий.

Также хорошие показатели медианного расстояния дали алгоритмы r2d2 и SIFT. При этом SIFT характеризуется большим количеством выбросов и худшим результатом по средним расстояниям без фильтрации.

Заметим, что SIFT + FOF оказались ближе всех к субпиксельной точности.

ORB в данном эксперименте дал худший результат с больше чем трёхкратным превышением медианного расстояния от точек до эпиполярных линий лучшего результата.

Стоит отметить, что для серии пар изображений с малым смещением фильтрация сопоставления точек с помощью теста Лёва не дал положительного эффекта ни для одного из исследованных методов идентификации ключевых точек. Наглядно таблицу 1 можно представить на графиках 1 – 2.

Результаты для серии пар с большим межкадровым смещением:

В результате работы математической модели были получены целевые метрики по всем алгоритмам для большого межкадрового смещения (таблица 2).

Таблица 2 – целевые метрики для изображений с большим межкадровым смещением.

Как видно из таблицы 2, для серии пар изображений с большим межкадровым смещением максимальную точность идентификации ключевых точек, то есть минимальное медианное расстояние от точек до эпиполярных линий, дал R2D2. Второе место занимает SIFT+FOF. Ускоренная версия R2D2 очевидно имеет большее количество ошибочных идентификаций, что и отразилось на результатах.

Худший результат медианного расстояния у метода SIFT: в 31 раз хуже чем у R2D2. Также он оказался методом с самыми большими выбросами, что отразилось на среднем расстоянии, которое больше в 13 раз, чем у лучшего результата R2D2.

Стоит заметить, что фильтрация по тесту Лёва дала значительное улучшение целевых метрик для алгоритмов: SIFT, ORB. В случае с SIFT улучшение результата более чем в 5 раз. Наглядно таблицу 2 можно представить на графиках 3 — 4.

Выводы

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

1. Метод SIFT + FOF для данных с малым межкадровым смещением.

2. Метод R2D2 для данных с большим межкадровым смещением.

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

В таком случае, в зависимости от будущих потребностей, имея полученные в ходе исследования результаты, можно будет найти лучший вариант для конкретных задач в системах навигации малогабаритных БЛА на основе визуальной одометрии.