Визуальная Одометрия (VIO) — Навигация по тому, что видит камера

Визуальная одометрия (Visual Odometry, VO) — это метод определения перемещения робота путем анализа последовательности изображений с одной или нескольких камер. Это фундаментальная технология, позволяющая роботам ориентироваться там, где GPS недоступен, а энкодеры ненадежны.

Ключевая идея: Камера — пассивный сенсор, который видит мир так же, как человек. Анализируя, как смещаются характерные точки в кадре при движении, можно вычислить собственное перемещение.

Движение камеры в 3D пространстве описывается вращением \(\mathbf{R}\) и сдвигом \( \mathbf{t} \):

\[ \mathbf{X}_t = \mathbf{R} \cdot \mathbf{X}_{t-1} + \mathbf{t} \] где \( \mathbf{X} \) — координаты точки в пространстве.

В координатах изображения используется матрица проекции: \[ \mathbf{x} = \mathbf{K} [\mathbf{R} \mid \mathbf{t}] \mathbf{X} \] где \( \mathbf{K} \) — матрица внутренних параметров камеры.

Для двух камер (или одной камеры в разные моменты времени) существует фундаментальное соотношение: \[ \mathbf{x}_2^\top \mathbf{F} \mathbf{x}_1 = 0 \] где \( \mathbf{F} \) — фундаментальная матрица, \( \mathbf{x}_1 \) и \( \mathbf{x}_2 \) — соответственные точки на двух изображениях.

Из \( \mathbf{F} \) можно извлечь \( \mathbf{R} \) и \( \mathbf{t} \) с точностью до масштаба.

Зная положение камеры в двух точках и соответствующие точки на изображениях, можно восстановить 3D-координаты: \[ \lambda_1 \mathbf{x}_1 = \mathbf{X}, \quad \lambda_2 \mathbf{x}_2 = \mathbf{R} \mathbf{X} + \mathbf{t} \]

Оптимизация всех параметров (положений камер и 3D-точек) минимизацией ошибки репроекции: \[ \min_{\mathbf{R}_i, \mathbf{t}_i, \mathbf{X}_j} \sum_{i,j} |\mathbf{x}_{ij} - \pi(\mathbf{R}_i \mathbf{X}_j + \mathbf{t}_i)|^2 \] где \( \pi \) — функция проекции.

Принцип: Использует одну камеру.

Проблема: Неизвестность абсолютного масштаба (нельзя отличить перемещение на 1 метр мимо объекта размером 1 метр от перемещения на 2 метра мимо объекта размером 2 метров).

Решение: Инициализация по известным размерам объектов или интеграция с другими сенсорами (IMU).

Принцип: Использует две камеры с известным взаимным расположением (базой).

Преимущество: Известен абсолютный масштаб, возможность сразу получать 3D-карту.

Вычисления: Требуется соответствие точек на левом и правом изображениях (stereo matching).

Принцип: Объединение данных камеры и IMU.

Преимущества:

• IMU помогает в быстрых движениях (когда камера “замыливается”)
• Камера корректирует дрейф IMU
• Стабильная работа в сложных условиях

Этапы:

1. Обнаружение признаков: Harris, FAST, SIFT, SURF, ORB
2. Сопоставление: FLANN, brute-force matching
3. Исключение выбросов: RANSAC для нахождения фундаментальной матрицы
4. Восстановление движения: PnP (Perspective-n-Point)

Плюсы: Стабильность, хорошо изучен

Минусы: Требует текстурированных сцен

Принцип: Минимизация ошибки по яркости пикселей: \[ E = \sum_{\mathbf{x}} |I_1(\mathbf{x}) - I_2(\mathbf{w}(\mathbf{x}, \mathbf{R}, \mathbf{t}))|^2 \] где \( \mathbf{w} \) — функция деформации (warping).

Плюсы: Использует всю информацию изображения, работает на слаботекстурированных сценах

Минусы: Чувствительность к изменениям освещения, требует хорошего начального приближения

Принцип: Комбинация обоих подходов (например, SVO — Semi-direct Visual Odometry)

• Прямой метод для быстрого отслеживания
• Feature-based для построения карты и перелокализации

\[ \mathbf{d} = [d_1, d_2, …, d_n]^\top \] где \( d_i \) — характеристики окрестности точки.

• Фильтр Калмана: Для слияния с IMU
• Оптимизация на графах (g2o): Для глобальной консистентности
• Инкрементальная оптимизация: iSAM, iSAM2

Оборудование:

• Raspberry Pi 4 (4GB)
• Стереокамера Raspberry Pi Camera Module 2 (или две камеры v2)
• IMU (например, ICM-20948)

Установка (Ubuntu + ROS2):

# Установка ROS2 Humble
sudo apt install ros-humble-ros-base
# Установка пакета VIO
sudo apt install ros-humble-visual-odometry
# Установка драйвера камеры
sudo apt install ros-humble-raspicam2

Конфигурационный файл для VIO:

camera:
  left_topic: "/left/image_raw"
  right_topic: "/right/image_raw"
  calibration_file: "/opt/ros/humble/share/calibration/stereo.yaml"
  
vio:
  algorithm: "stereo"
  use_imu: true
  imu_topic: "/imu/data"
  
output:
  odometry_topic: "/vo/odometry"
  pose_topic: "/vo/pose"

import cv2
import numpy as np

# Инициализация детектора ORB
orb = cv2.ORB_create()
matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

prev_frame = None
prev_kp = None
prev_des = None

while True:
    frame = capture_frame()  # Получение кадра
    
    # Обнаружение ключевых точек
    kp, des = orb.detectAndCompute(frame, None)
    
    if prev_frame is not None:
        # Сопоставление признаков
        matches = matcher.match(prev_des, des)
        
        if len(matches) > 10:
            # Подготовка точек для findEssentialMat
            src_pts = np.float32([prev_kp[m.queryIdx].pt for m in matches])
            dst_pts = np.float32([kp[m.trainIdx].pt for m in matches])
            
            # Нахождение существенной матрицы
            E, mask = cv2.findEssentialMat(src_pts, dst_pts, focal=1.0, pp=(0,0))
            
            # Восстановление вращения и сдвига
            _, R, t, mask = cv2.recoverPose(E, src_pts, dst_pts)
            
            # Интегрирование движения
            # (здесь нужна полная реализация интегратора)
            
    prev_frame = frame.copy()
    prev_kp = kp
    prev_des = des

Состояние системы: \[ \mathbf{x} = [\mathbf{p}, \mathbf{q}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a]^\top \] где:

• \( \mathbf{p} \) — положение
• \( \mathbf{q} \) — ориентация (кватернион)
• \( \mathbf{v} \) — скорость
• \( \mathbf{b}_g \) — дрейф гироскопа
• \( \mathbf{b}_a \) — дрейф акселерометра

Prediction step (IMU): \[ \dot{\mathbf{p}} = \mathbf{v} \] \[ \dot{\mathbf{v}} = \mathbf{R}(\mathbf{q}) (\mathbf{a}_m - \mathbf{b}_a) - \mathbf{g} \] \[ \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \\ \mathbf{\omega}_m - \mathbf{b}_g \end{bmatrix} \]

Update step (камера): \[ \mathbf{z} = \pi(\mathbf{R}^\top (\mathbf{X} - \mathbf{p})) + \mathbf{n} \] где \( \mathbf{n} \) — шум измерений.

• Камеры: Матрица \( \mathbf{K} \), коэффициенты искажения
• Стерео: Вращение и сдвиг между камерами
• Камера-IMU: Временная синхронизация и взаимное расположение

• Камеры: Глобальный затвор предпочтительнее rolling shutter для быстрого движения
• Синхронизация: Аппаратная синхронизация камер и IMU
• Вычисления: GPU/VPU для реального времени

• Наборы данных: KITTI, EuRoC MAV, TUM RGB-D
• Метрики: ATE (Absolute Trajectory Error), RPE (Relative Pose Error)
• Инструменты: EVO, rpg_trajectory_evaluation

• Self-supervised VO: Обучение без размеченных данных
• Depth from motion: Оценка глубины из видео
• End-to-end VIO: Полностью обученная система

Принцип: Камеры, реагирующие на изменения яркости Преимущества: Высокая временная разрешающая способность (> 10 000 кадров/с эквивалент), низкая задержка, высокий динамический диапазон

Идея: Использование семантической сегментации для:

• Исключения динамических объектов
• Использования семантической информации для лучшей оценки
• Семантической loop closure

Цель: Почувствовать принцип работы на практике.

Что нужно: Веб-камера, Python, OpenCV.

Задание:

1. Напишите программу, которая считывает кадры с веб-камеры
2. Реализуйте детектирование ORB-признаков
3. Сопоставляйте признаки между последовательными кадрами
4. Вычисляйте существенную матрицу и восстанавливайте вращение/сдвиг
5. Визуализируйте траекторию в 2D

Сложности, с которыми столкнетесь:

• Масштаб будет “плавать”
• При поворотах будет теряться трекинг
• Накопление ошибки будет очевидным

Вывод: Вы поймете, почему промышленные системы используют стерео, IMU и сложную оптимизацию.

Визуальная одометрия — мост между компьютерным зрением и робототехникой:

1. Полноценный V-SLAM — как перейти от одометрии к построению карты и глобальной консистентности
2. Оптимизация графов (g2o) — как поддерживать глобальную консистентность карты
3. Сенсорная fusion с IMU — как создать надежную систему VIO
4. Нейросетевые методы в VO — современные подходы с использованием глубокого обучения

Ключевой совет: Начинайте с простого — монокулярного VO на наборах данных (KITTI). Затем переходите к стерео. Для реального робота сразу планируйте интеграцию с IMU. И помните: калибровка и синхронизация — 50% успеха в VIO.