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 – математическая абстракция, представляющая собой связь между камерами в трёхмерной сцене. Она описывает геометрические свойства проекции точек из одного положения камеры в другое, играя важную роль в задачах компьютерного зрения. Представляет собой матрицу 3x3, обозначаемую как 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 для данных с большим межкадровым смещением.

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

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