1. Предисловие

1. В предыдущем блоге было представлено отслеживание визуального направления. Однако в сегодняшней сфере интеллектуального вождения радарное восприятие по-прежнему занимает основную роль. Сегодня я расскажу о 3D-отслеживании облаков точек.
2. Визуальное сопровождение вхождения является результатом обнаружения цели. Радарное отслеживание вхождений может быть результатом обнаружения облака точек,Это также может быть результатом кластеризации облаков точек. В дополнение к некоторым структурам данных, соблюдайте расчеты,Алгоритм радиолокационного слежения примерно такой же, как и метод визуального слежения, представленный ранее.
3. В этой статье в основном обсуждается, как радар выполняет вычисления согласования и корреляции, а также анализируется структура кода. Эталоном является код Аполлона, и общий эффект довольно хорош.

2. Каталог кода

Радар отслеживает все файлы кода

3. Интерпретация кода

3.1. Описание файла.

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

Некоторые дополнения:

геометрический центр облака точек (центр масс)

Состояние угла курса по умолчанию (1, 0, 0)

Размер целевой коробки длина, ширина, высота (длина, ширина, высота)

целевое состояние (x, y, z, vx, vy, vz)

Уравнение состояния Равномерная скорость x = x + v * t

3.2. Структура Кодекса

Общая последовательность чтения кода следующая.

(1) Отслеживание

hm_tracker.cpp

bool HmObjectTracker::Track()

A Инициализация B Входные данные преобразования данных C Прогноз D Сопоставление E Обновление F Результат

Файл hm_tracker.cpp Функция отслеживания, по сути, является основной функцией всей операции.

(2) Матч

hungarian_matcher.cpp

void HungarianMatcher::Match()

A Вычислить корреляционную матрицу B Вычислить связные компоненты C Сопоставить каждый подграф

Существует множество методов сопоставления. Основным из них является метод A для расчета корреляционной матрицы.

(3) Расчет корреляционной матрицы

track_object_distance.cpp

float TrackObjectDistance::ComputeDistance()

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

4. Расчет корреляционной матрицы

Эта часть является ядром, давайте ее внимательно изучим. Перехватить часть кода

double TrackObjectDistance::s_location_distance_weight_ = 0.6;
double TrackObjectDistance::s_direction_distance_weight_ = 0.2;
double TrackObjectDistance::s_bbox_size_distance_weight_ = 0.1;
double TrackObjectDistance::s_point_num_distance_weight_ = 0.1;
double TrackObjectDistance::s_histogram_distance_weight_ = 0.5;

// измерение нового_объекта track_predictпредсказывать 
float TrackObjectDistance::ComputeDistance(
    ObjectTrackPtr track, const Eigen::VectorXf& track_predict,
    const std::shared_ptr<TrackedObject>& new_object) {
  // Compute distance for given track & object
  float location_distance = ComputeLocationDistance(track, track_predict, new_object);  // медленная скорость быстрый
  float direction_distance = ComputeDirectionDistance(track, track_predict, new_object);
  float bbox_size_distance = ComputeBboxSizeDistance(track, new_object);
  float point_num_distance = ComputePointNumDistance(track, new_object);
  float histogram_distance = ComputeHistogramDistance(track, new_object);

  float result_distance = s_location_distance_weight_ * location_distance +
                          s_direction_distance_weight_ * direction_distance +
                          s_bbox_size_distance_weight_ * bbox_size_distance +
                          s_point_num_distance_weight_ * point_num_distance +
                          s_histogram_distance_weight_ * histogram_distance;
  return result_distance;
}

4.1、ComputeLocationDistance

Вычислите разницу расстояний до центральной точки. Диапазон значений: [0,

）

когдатекущий_объект середина V <= 2m/s Евклидово расстояние

когдатекущий_объект середина V > 2m/s Разложено по направлению скорости Установите систему координат на основе направления скорости и направления вертикальной скорости. Отклонение в направлении скорости проецирования составляет 1/2 отклонения расстояния, Отклонение в направлении проецируемой вертикальной скорости в 2 раза превышает отклонение расстояния. Квадратный корень используется для нахождения окончательного отклонения.

Это можно просто понять так: когда цель движется с высокой скоростью, отклонение смещения в направлении скорости будет немного больше. Чтобы компенсировать это отклонение, вес смещения в направлении уменьшается для расчета окончательного отклонения смещения. (это мое понимание).

4.2、ComputeDirectionDistance

Рассчитайте расстояние в направлении. Диапазон значений [0,2].

Вычислите значение косинуса смещения в направлении скорости cos_theta и, наконец, верните 1-cos_theta.

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

Когда отклонение смещения равно 0, значение cos по умолчанию установлено на 0,994.

4.3、ComputeBboxSizeDistance

Диапазон значений[0,1]

Это немного сложно, позвольте мне объяснить это подробно

old_dir — это направление текущей цели. Если значение отклонения по умолчанию равно 0, значение по умолчанию — (1, 0, 0).

new_idr определяет направление цели

old_size Размер текущей цели (bbox.length, bbox.width, bbox.height)

new_size определяет размер цели

nter&pos_id=img-506WdR2l-1706774251770)

Вычислите dot_00, dot_01. dot_00 можно понимать как угол между двумя целевыми направлениями, dot_01 можно понимать как угол между целью и вертикальным направлением другой цели. Угол, превышающий 90 градусов, здесь не рассматривается, поскольку целевое направление может быть альфа или 180-альфа.

Почему две ситуации? Это можно понимать как выравнивание длины и ширины цели (мы заранее не знаем соответствующую длину и ширину цели) и сначала оценку на основе состояния угла цели. Когда угол меньше 45°, длина цели совпадает с длиной другой цели. В противном случае длина цели выравнивается с шириной другой цели, и окончательно рассчитывается соотношение разницы длины или ширины, а минимальное значение принимается в качестве окончательного значения.

4.4、ComputePointNumDistance

Диапазон значений[0,1]

Эта формула относительно проста

Это легко понять. Чем ближе числа облаков точек, тем легче их сопоставить.

4.5、ComputePointNumDistance

Диапазон значений[0,3]

Расстояние гистограммы: разделите все облака точек цели на 10 интервалов по одной оси координат, а затем разделите их в общей сложности на 30 интервалов по оси xyz.

Если все облака точек расположены равномерно, то характеристики целевой формы shape_features = [0,1] * 30

Конкретный процесс расчета shape_features. Возьмем в качестве примера ось X, то же самое применимо к осям Y и Z.

Рассчитайте максимальное значение оси X целевого облака точек и разделите интервал на 10 равных частей. Запишите количество облаков точек всех точек за 10 интервалов.

Если он расположен совершенно равномерно, то результат

shape_feature_x = [0.1]*10

shape_features = shape_feature_x + shape_feature_y + shape_feature_z

Зная определение характеристик формы цели, мы можем получить

4.6、result_distance

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

5. Результаты

потому что rviz Результаты отслеживания облака точек не могут быть отображены, тогда мы помещаем результаты радарного отслеживания topic запиши это,а потом визуализируй。когда然也可以在过程серединасохранить изображение。

Общий эффект отслеживания хороший. хвалить!