Трекинг объектов при помощи GNN

В современном мире задачи отслеживания объектов (трекинга) имеют широкое применение – от автономных транспортных средств и систем видеонаблюдения до радарного и сенсорного мониторинга. Одной из ключевых проблем в этой области стала ассоциация измерений с треками, особенно в условиях неопределённости, шумов и возможных пересечений траекторий.

Мы рассмотрим реализацию классического алгоритма трекинга GNN.

Представленный код включает следующие шаги.

1. Моделирование движения объектов по гиперболическим траекториям.
2. Реализация фильтра Калмана для предсказания и обновления состояний треков.
3. Визуализация результатов в виде анимированного графика.

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

Далее перейдём к алгоритму и начнём с объявления структур данных и константы для системы трекинга объектов.

1. Track — изменяемая структура, хранящая состояние объекта (координаты, скорость), матрицу ковариации (неопределённость), возраст трека, количество пропущенных измерений и историю состояний для визуализации.
2. Detection — неизменяемая структура для хранения измерений (координат и время).
3. Матрица наблюдений H размера 2×4, которая выделяет позиционные компоненты (x,y) из вектора состояния.

Эти структуры формируют основу для реализации алгоритма.

using LinearAlgebra

# Изменяемая структура трека
mutable struct Track
    state::Vector{Float64}     # Вектор состояния
    covariance::Matrix{Float64} # Матрица ковариации
    age::Int                   # Возраст трека (число обновлений)
    missed::Int                # Число пропущенных измерений
    history::Vector{Vector{Float64}} # История состояний для отрисовки
end

# Добавляем history в конструктор Track
Track(state, covariance, age, missed) = Track(state, covariance, age, missed, [copy(state)])

# Измерение: [x, y, timestamp]
struct Detection
    position::Vector{Float64}
    time::Float64
end

const H = [1 0 0 0; 0 1 0 0];  # Матрица наблюдений

Далее рассмотрим реализацию фильтра Калмана для трекинга.

1. predict! обновляет состояние трека на основе модели постоянной скорости (CV), предсказывая новое положение и увеличивая неопределённость (ковариацию).
2. update! корректирует состояние трека по измерению, используя матрицу усиления Калмана (K), уменьшая неопределённость и сохраняя историю изменений.

# Функция предсказания состояния трека
function predict!(track::Track, dt::Float64)
    F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]  # Матрица перехода (CV)
    track.state = F * track.state
    track.covariance = F * track.covariance * F' + diagm([0.1, 0.1, 0.01, 0.01])
    return track
end

# Функция обновления трека на основе измерения
function update!(track::Track, detection::Detection)
    y = detection.position - H * track.state
    S = H * track.covariance * H' + diagm([0.5, 0.5])
    K = track.covariance * H' / S
    track.state += K * y
    track.covariance = (I - K * H) * track.covariance
    track.age += 1
    track.missed = 0
    push!(track.history, copy(track.state)) # Сохраняем историю
    return track
end

[ Info: Starting sender/receiver loops

update! (generic function with 1 method)

Перейдём к функции алгоритма трекинга. Global Nearest Neighbor (GNN) — простой метод, основанный на жадном сопоставлении измерений с ближайшими треками. В нашем коде функция trackerGNN реализует алгоритм GNN для сопоставления треков и измерений. Сам алгорим состоит из следующих частей.

1. Предсказание: копирует и предсказывает новые состояния всех треков (predict!).
2. Расчёт расстояний: строит матрицу расстояний между треками и измерениями на основе статистического расстояния Махаланобиса.
3. Жадное сопоставление: находит пары «трек-измерение» с минимальным расстоянием, проверяя порог (gate) и исключает уже сопоставленные треки/измерения.
4. Обновление и фильтрация: обновляет треки по сопоставленным измерениям (update!), а также удаляет треки, пропустившие >3 измерений.

function trackerGNN(detections::Vector{Detection}, existing_tracks::Vector{Track}, gate::Float64)
    predicted = [predict!(deepcopy(t), 1.0) for t in existing_tracks] # Предсказание состояний

    # Матрица расстояний
    dist = zeros(length(predicted), length(detections))
    for (i,t) in enumerate(predicted), (j,d) in enumerate(detections)
        y = d.position - H*t.state
        dist[i,j] = y' / (H*t.covariance*H') * y
    end

    dets_assigned = tracks_assigned = Int[] # Cопоставление
    for _ in 1:min(length(predicted), length(detections))
        i,j = argmin(dist).I
        dist[i,j] > gate && break
        push!(tracks_assigned, i)
        push!(dets_assigned, j)
        dist[i,:] .= dist[:,j] .= Inf
    end
        
    updated = deepcopy(predicted) # Обновление и фильтрация
    foreach(((i,j),) -> update!(updated[i], detections[j]), zip(tracks_assigned, dets_assigned))
    filter!(t -> t.missed < 3, updated)
end

trackerGNN (generic function with 1 method)

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

1. Синие точки (scatter) – текущие измерения (детекции).
2. Синие пунктирные линии – реальные траектории объектов (obj1, obj2).
3. Красные квадраты – треки GNN с историей (сплошная линия).

function plot_tracks(tracks::Vector{Track}, detections::Vector{Detection}, obj1, obj2, step)
    p = scatter(getindex.(d.position[1] for d in detections), getindex.(d.position[2] for d in detections), label="Detections", color=:blue, markersize=6)
    # Реальные траектории
    plot!(getindex.(p[1] for p in obj1[1:step]), getindex.(p[2] for p in obj1[1:step]), label="", color=:blue, linestyle=:dash)
    plot!(getindex.(p[1] for p in obj2[1:step]), getindex.(p[2] for p in obj2[1:step]), label="", color=:blue, linestyle=:dash)

    # Вспомогательная функция для отрисовки треков
    function plot_tracks!(tracks, label, color, marker)
        for (i, t) in enumerate(tracks)
            scatter!([t.state[1]], [t.state[2]], label=i==1 ? label : "", color=color, markersize=8, marker=marker)
            # Отрисовка истории трека
            if length(t.history) > 1
                plot!(getindex.(h[1] for h in t.history), getindex.(h[2] for h in t.history), 
                      label="", color=color, linestyle=:solid, linewidth=2)
            end
        end
    end
    
    # Треки разных типов с историей
    plot_tracks!(tracks, "GNN Track", :red, :square)
    plot!(legend=:topleft, title="Шаг: $step", xlims=(-5, 5), ylims=(-5, 5))
end

plot_tracks (generic function with 1 method)

Функция generate_hyperbola_trajectories генерирует две симметричные гиперболические траектории, а именно создаёт диапазон параметра t от -3 до 3 с равномерным распределением, после чего вычисляет координаты точек для двух гипербол.

• Первая: acosh(t), y = bsinh(t)
• Вторая: -acosh(t), y = bsinh(t)

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

# Функция для генерации траекторий гипербол
function generate_hyperbola_trajectories(steps, a=1.0, b=1.0)
    t = range(-3, 3, length=steps)
    [(a*cosh(ti), b*sinh(ti)) for ti in t], [(-a*cosh(ti), b*sinh(ti)) for ti in t]
end

generate_hyperbola_trajectories (generic function with 3 methods)

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

Цикл симуляции включает в себя следующие шаги.

1. Алгоритм добавления на каждом шаге гауссовский шум к реальным позициям.
2. Обновляет трекеров GNN и визуализирует текущее состояние через plot_tracks.
3. Сохраняет последний кадр как PNG-изображение, а также создает анимированный GIF (5 кадров/сек) всего процесса трекинга.

steps = 50
obj1, obj2 = generate_hyperbola_trajectories(steps)

# Инициализация треков с историей
initial_state1 = [obj1[1][1], obj1[1][2], 0.5, 0.5]
initial_state2 = [obj2[1][1], obj2[1][2], -0.5, 0.5]
tracks_gnn = [Track(initial_state1, Matrix(1.0I, 4, 4), 1, 0),
              Track(initial_state2, Matrix(1.0I, 4, 4), 2, 0)]

# Создаем GIF
anim = @animate for step in 1:steps
    # Генерируем измерения для текущего шага
    noise = 0.3(randn(4))
    detections = [Detection([obj1[step][1] + noise[1], obj1[step][2] + noise[2]], step), 
                  Detection([obj2[step][1] + noise[3], obj2[step][2] + noise[4]], step)]
    # Обновляем трек
    updated_tracks_gnn = trackerGNN(detections, deepcopy(tracks_gnn), 50.0)
    # Обновляем исходные треки для следующего шага
    global tracks_gnn = updated_tracks_gnn
    # Визуализация
    plot_tracks(tracks_gnn, detections, obj1, obj2, step)
    if step == steps
        savefig("last_frame.png")
    end
end 
# Сохраняем GIF
gif(anim, "tracking_simulation.gif", fps=5)

Вывод

Результаты:

1. Последний шаг моделирования показывает, что алгоритм успешно отслеживает оба объекта, несмотря на добавленный шум в измерения.
2. Траектории треков (красные линии) близки к реальным траекториям объектов (синие пунктирные линии), что подтверждает точность алгоритма. При этом самые большие неточности мы видим именно в момент соприкосновения траекторий.
3. Значение LIIar: 50 – пороговое значение для расстояния Махалонобиса указывает на устойчивость алгоритма к ложным срабатываниям.

using Images
println("Последний шаг моделирования:")
img = load("last_frame.png")

Последний шаг моделирования:

Представленный алгоритм демонстрирует высокую эффективность для задач трекинга объектов с нелинейными траекториями. Ключевыми преимуществами являются:

• устойчивость к шуму благодаря фильтру Калмана;
• простота реализации метода GNN для сопоставления измерений;
• наглядная визуализация, позволяющая легко интерпретировать результаты.

Алгоритм успешно решает задачу трекинга и может быть применён в таких областях, как автономные системы, видеонаблюдение и робототехника. Для дальнейшего улучшения алгоритма можно рассмотреть использование более сложных моделей движения (например, модель постоянного ускорения) или методов трекинга, таких как JPDA или MHT, для работы в условиях высокой плотности объектов.