zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

障碍物检测v3，改成了卡尔曼滤波完整版

Browse Source
This commit is contained in:
Orange
2026-07-22 17:30:03 +08:00
parent 07c2ad6baf
commit fc6e796c28
7 changed files with 1069 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
src/obstacle_scanner/config/params.yaml
View File

@@ -1,70 +1,81 @@
obstacle_scanner:
ros__parameters:
# Input LaserScan topic.
# 输入的 LaserScan 话题。
scan_topic: "/scan"
# Frame id used on published /obstacles and debug images.
frame_id: "laser_frame"
# 发布 /obstacles 和调试图像时使用的坐标系 ID。
frame_id: "laser_link"
# Maximum Euclidean distance in meters between adjacent scan points before
# starting a new cluster. The detector now uses scan order, so this is O(N).
# 相邻扫描点归入同一聚类时允许的最大欧氏距离，单位为米。
# 检测器按扫描顺序处理，因此时间复杂度为 O(N)。
cluster_gap: 0.1
# Drop clusters with more points than this before circle fitting. This keeps
# walls and large surfaces from becoming circular obstacles.
# 圆拟合前丢弃点数超过此值的聚类。
max_cluster_points: 20
# Minimum accepted fitted radius in meters for 3+ point circle observations.
# 接受三点及以上观测的最小拟合半径，单位为米。
radius_min: 0.03
# Maximum accepted fitted radius in meters for 3+ point circle observations.
radius_max: 0.05
# 接受三点及以上观测的最大拟合半径，单位为米。
radius_max: 0.06
# Merge the last and first scan-order clusters when their endpoint distance
# is below cluster_gap. Useful for near-360 degree scans; harmless otherwise
# unless scan edge points are physically close.
# 当扫描序列首尾两个聚类相邻时，将它们合并。
merge_wrap: true
# Enable short-term laser-frame tracking. When false, only current-frame
# circle-fit observations are published.
# 为 true 时发布跟踪后的障碍物；为 false 时仅发布当前帧拟合结果。
enable_tracking: true
# Number of successful updates before a track may be published.
track_confirm_hits: 1
# 确认一条轨迹所需的连续圆拟合命中次数。
track_confirm_fit_hits: 2
# Delete a track after this many consecutive missed frames.
track_delete_misses: 2
# 确认一条轨迹所需的圆拟合与弦线观测总命中次数。
track_confirm_total_hits: 3
# Maximum distance in meters for nearest-neighbor observation-to-track match.
association_gate: 0.25
# 已确认轨迹仍可发布预测结果的最大连续丢失帧数。
track_publish_misses: 4
# Extra tolerance in meters for accepting a two-point chord whose length is
# close to the predicted track diameter.
# 已确认轨迹连续丢失达到此帧数后删除。
track_delete_misses: 6
# 加速度过程噪声的 1σ 标准差，单位为米每二次方秒。
process_accel_noise: 3.0
# 初始速度不确定度的 1σ 标准差，单位为米每秒。
initial_velocity_stddev: 2.5
# 三点及以上圆拟合的位置噪声 1σ 标准差，单位为米。
fit_position_stddev: 0.02
# 两点弦线恢复的位置噪声 1σ 标准差，单位为米。
chord_position_stddev: 0.06
# 数据关联允许的最大马氏距离平方。
mahalanobis_gate: 9.21
# 数据关联附加的欧氏距离硬限制，单位为米。
max_association_distance: 0.35
# 位置不确定度超过此值时停止发布预测结果，单位为米。
max_position_stddev: 0.15
# 弦长接近预测直径时允许的额外容差，单位为米。
chord_tolerance: 0.02
# Weight for center updates from 3+ point circle fitting. Higher values trust
# the current fit more.
fit_update_alpha: 0.7
# 扫描时间戳缺失或无效时使用的默认时间间隔，单位为秒。
nominal_dt: 0.0833333333
# Weight for center updates from 2-point chord recovery. Lower values keep
# chord updates from pulling the track too aggressively.
chord_update_alpha: 0.3
# 有效时间戳间隔的下限和上限，单位为秒。
min_dt: 0.02
max_dt: 0.20
# Enable debug publishers. This creates /processed_scan and, when debug_info
# is true, /obstacle_scanner/debug_info.
debug: true
# Publish compact JSON debug information on /obstacle_scanner/debug_info.
# 启用调试发布器；debug_info 是仅控制文本诊断的子开关。
debug: false
debug_info: true
# Debug image width and height in pixels.
# 调试图像尺寸，以及每像素表示的米数。
debug_image_size: 500
# Meters per pixel for the debug image.
debug_resolution: 0.01
# Publish one debug image every N scan frames to reduce OpenCV drawing cost.
# 每处理 N 帧扫描发布一次调试图像或 JSON 诊断信息。
debug_image_stride: 3
# Publish one JSON debug message every N scan frames.
debug_info_stride: 1

56
src/obstacle_scanner/include/obstacle_scanner/tracking.hpp
View File

@@ -2,7 +2,7 @@
#include <Eigen/Dense>
#include <cstddef>
#include <limits>
#include <string>
#include <vector>
@@ -34,12 +34,21 @@ struct Observation
struct TrackerConfig
{
int track_confirm_hits{1};
int track_delete_misses{2};
double association_gate{0.25};
int track_confirm_fit_hits{2};
int track_confirm_total_hits{3};
int track_publish_misses{3};
int track_delete_misses{5};
double process_accel_noise{3.0};
double initial_velocity_stddev{2.5};
double fit_position_stddev{0.02};
double chord_position_stddev{0.06};
double mahalanobis_gate{9.21};
double max_association_distance{0.35};
double max_position_stddev{0.15};
double chord_tolerance{0.02};
double fit_update_alpha{0.7};
double chord_update_alpha{0.3};
double nominal_dt{1.0 / 12.0};
double min_dt{0.02};
double max_dt{0.20};
};
struct TrackOutput
@@ -59,6 +68,14 @@ struct FrameDebug
int chord_observations{0};
int chord_updates{0};
int chord_rejections{0};
int tracks_created{0};
int tracks_confirmed{0};
int tracks_deleted{0};
int fit_updates{0};
int association_rejections{0};
int predicted_published{0};
double mean_innovation{0.0};
double max_innovation{0.0};
int active_tracks{0};
int published_tracks{0};
};
@@ -68,32 +85,47 @@ class ObstacleTracker
public:
explicit ObstacleTracker(const TrackerConfig & config = TrackerConfig());
FrameDebug update(const std::vector<Observation> & observations);
FrameDebug update(
const std::vector<Observation> & observations,
double stamp_seconds);
std::vector<TrackOutput> confirmed_tracks() const;
const std::vector<TrackOutput> & tracks_for_debug() const;
private:
struct Track
{
int id{0};
Eigen::Vector2d center{0.0, 0.0};
Eigen::Vector4d state{Eigen::Vector4d::Zero()};
Eigen::Matrix4d covariance{Eigen::Matrix4d::Zero()};
double radius{0.0};
int hit_count{0};
int id{0};
int fit_hits{0};
int total_hits{0};
int missed_count{0};
UpdateSource last_source{UpdateSource::Predict};
bool confirmed{false};
bool matched{false};
UpdateSource last_source{UpdateSource::Predict};
};
int find_nearest_unmatched_track(const Eigen::Vector2d & center) const;
void predict_tracks(double dt);
void update_track(
Track & track,
const Eigen::Vector2d & center,
double position_stddev,
UpdateSource source,
FrameDebug & debug,
double & innovation_sum,
int & innovation_count);
bool chord_center_for_track(
const Observation & observation,
const Track & track,
Eigen::Vector2d & center) const;
bool is_publishable(const Track & track) const;
static TrackOutput to_output(const Track & track);
void refresh_debug_outputs();
TrackerConfig config_;
int next_id_{1};
double last_stamp_seconds_{std::numeric_limits<double>::quiet_NaN()};
std::vector<Track> tracks_;
std::vector<TrackOutput> debug_outputs_;
};

32
src/obstacle_scanner/src/obstacle_scanner_node.cpp
View File

@@ -31,17 +31,26 @@ public:
frame_id_ = declare_parameter<std::string>("frame_id", "laser_frame");
detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1);
detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20);
detection_config_.radius_min = declare_parameter("radius_min", 0.03);
detection_config_.radius_min = declare_parameter("radius_min", 0.02);
detection_config_.radius_max = declare_parameter("radius_max", 0.05);
detection_config_.merge_wrap = declare_parameter("merge_wrap", true);
enable_tracking_ = declare_parameter("enable_tracking", true);
tracker_config_.track_confirm_hits = declare_parameter("track_confirm_hits", 1);
tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 2);
tracker_config_.association_gate = declare_parameter("association_gate", 0.25);
tracker_config_.track_confirm_fit_hits = declare_parameter("track_confirm_fit_hits", 2);
tracker_config_.track_confirm_total_hits = declare_parameter("track_confirm_total_hits", 3);
tracker_config_.track_publish_misses = declare_parameter("track_publish_misses", 3);
tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 5);
tracker_config_.process_accel_noise = declare_parameter("process_accel_noise", 3.0);
tracker_config_.initial_velocity_stddev = declare_parameter("initial_velocity_stddev", 2.5);
tracker_config_.fit_position_stddev = declare_parameter("fit_position_stddev", 0.02);
tracker_config_.chord_position_stddev = declare_parameter("chord_position_stddev", 0.06);
tracker_config_.mahalanobis_gate = declare_parameter("mahalanobis_gate", 9.21);
tracker_config_.max_association_distance = declare_parameter("max_association_distance", 0.35);
tracker_config_.max_position_stddev = declare_parameter("max_position_stddev", 0.15);
tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02);
tracker_config_.fit_update_alpha = declare_parameter("fit_update_alpha", 0.7);
tracker_config_.chord_update_alpha = declare_parameter("chord_update_alpha", 0.3);
tracker_config_.nominal_dt = declare_parameter("nominal_dt", 1.0 / 12.0);
tracker_config_.min_dt = declare_parameter("min_dt", 0.02);
tracker_config_.max_dt = declare_parameter("max_dt", 0.20);
tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_);
debug_ = declare_parameter("debug", false);
@@ -98,7 +107,8 @@ private:
std::vector<obstacle_scanner::TrackOutput> output_tracks;
std::vector<obstacle_scanner::TrackOutput> debug_tracks;
if (enable_tracking_) {
tracker_debug = tracker_.update(observations);
const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
tracker_debug = tracker_.update(observations, stamp_seconds);
output_tracks = tracker_.confirmed_tracks();
debug_tracks = tracker_.tracks_for_debug();
} else {
@@ -275,6 +285,14 @@ private:
out << "\"chord\":" << detection_debug.chord_observations << ",";
out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
out << "\"tracks_created\":" << tracker_debug.tracks_created << ",";
out << "\"tracks_confirmed\":" << tracker_debug.tracks_confirmed << ",";
out << "\"tracks_deleted\":" << tracker_debug.tracks_deleted << ",";
out << "\"fit_updates\":" << tracker_debug.fit_updates << ",";
out << "\"association_rejections\":" << tracker_debug.association_rejections << ",";
out << "\"predicted_published\":" << tracker_debug.predicted_published << ",";
out << "\"mean_innovation\":" << tracker_debug.mean_innovation << ",";
out << "\"max_innovation\":" << tracker_debug.max_innovation << ",";
out << "\"tracks\":" << tracker_debug.active_tracks << ",";
out << "\"published\":" << published_count << ",";
out << "\"ms\":" << processing_ms << ",";

321
src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig Normal file
View File

@@ -0,0 +1,321 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "obstacle_scanner/detection.hpp"
#include "obstacle_scanner/msg/obstacle.hpp"
#include "obstacle_scanner/msg/obstacle_array.hpp"
#include "obstacle_scanner/tracking.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>
class ObstacleScannerNode : public rclcpp::Node
{
public:
ObstacleScannerNode()
: rclcpp::Node("obstacle_scanner")
{
using std::placeholders::_1;
scan_topic_ = declare_parameter<std::string>("scan_topic", "/scan");
frame_id_ = declare_parameter<std::string>("frame_id", "laser_frame");
detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1);
detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20);
detection_config_.radius_min = declare_parameter("radius_min", 0.03);
detection_config_.radius_max = declare_parameter("radius_max", 0.05);
detection_config_.merge_wrap = declare_parameter("merge_wrap", true);
enable_tracking_ = declare_parameter("enable_tracking", true);
tracker_config_.track_confirm_fit_hits = declare_parameter(" track_confirm_fit_hits, 2);
tracker_config_.track_confirm_total_hits = declare_parameter( rack_confirm_total_hits, 3);
tracker_config_.track_publish_misses = declare_parameter( rack_publish_misses, 3);
tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 2);
tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02);
tracker_config_.nominal_dt = declare_parameter(" nominal_dt, 1.0 / 12.0);
tracker_config_.min_dt = declare_parameter(min_dt, 0.02);
tracker_config_.max_dt = declare_parameter(max_dt, 0.20);
tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_);
debug_ = declare_parameter("debug", false);
debug_info_ = declare_parameter("debug_info", true);
debug_image_size_ = declare_parameter("debug_image_size", 500);
debug_resolution_ = declare_parameter("debug_resolution", 0.01);
debug_image_stride_ = std::max(
1, static_cast<int>(declare_parameter<int>("debug_image_stride", 3)));
debug_info_stride_ = std::max(
1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
"/obstacles", 10);
if (debug_) {
debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
"/processed_scan", 10);
if (debug_info_) {
debug_info_pub_ = create_publisher<std_msgs::msg::String>(
"/obstacle_scanner/debug_info", 10);
}
}
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
scan_topic_, rclcpp::SensorDataQoS(),
std::bind(&ObstacleScannerNode::scan_callback, this, _1));
RCLCPP_INFO(get_logger(), "ObstacleScannerNode started");
}
private:
void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr & msg)
{
const auto start_time = std::chrono::steady_clock::now();
frame_count_++;
std::vector<Eigen::Vector2d> points;
points.reserve(msg->ranges.size());
for (std::size_t i = 0; i < msg->ranges.size(); ++i) {
const double range = msg->ranges[i];
if (!std::isfinite(range) || range <= 0.0) {
continue;
}
const double angle = msg->angle_min + static_cast<double>(i) * msg->angle_increment;
points.emplace_back(range * std::cos(angle), range * std::sin(angle));
}
obstacle_scanner::DetectionDebug detection_debug;
const auto observations = obstacle_scanner::detect_observations(
points, detection_config_, detection_debug);
obstacle_scanner::FrameDebug tracker_debug;
std::vector<obstacle_scanner::TrackOutput> output_tracks;
std::vector<obstacle_scanner::TrackOutput> debug_tracks;
if (enable_tracking_) {
const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
tracker_debug = tracker_.update(observations, stamp_seconds);
output_tracks = tracker_.confirmed_tracks();
debug_tracks = tracker_.tracks_for_debug();
} else {
output_tracks = tracks_from_fit_observations(observations);
debug_tracks = output_tracks;
tracker_debug.published_tracks = static_cast<int>(output_tracks.size());
tracker_debug.active_tracks = tracker_debug.published_tracks;
}
const double processing_ms = elapsed_ms(start_time);
RCLCPP_DEBUG(
get_logger(),
"points=%zu clusters=%d fit=%d chord=%d tracks=%d published=%zu ms=%.3f",
points.size(), detection_debug.total_clusters,
detection_debug.fit_observations, detection_debug.chord_observations,
tracker_debug.active_tracks, output_tracks.size(), processing_ms);
auto out_msg = obstacle_scanner::msg::ObstacleArray();
out_msg.header.stamp = msg->header.stamp;
out_msg.header.frame_id = frame_id_;
for (const auto & track : output_tracks) {
obstacle_scanner::msg::Obstacle obstacle;
obstacle.center_x = track.center_x;
obstacle.center_y = track.center_y;
obstacle.radius = track.radius;
out_msg.obstacles.push_back(obstacle);
}
obstacles_pub_->publish(out_msg);
if (debug_) {
if (debug_pub_ && frame_count_ % static_cast<std::uint64_t>(debug_image_stride_) == 0) {
auto img_msg = render_debug_image(
points, debug_tracks, detection_debug, tracker_debug,
processing_ms, msg->header.stamp);
debug_pub_->publish(*img_msg);
}
if (
debug_info_pub_ &&
frame_count_ % static_cast<std::uint64_t>(debug_info_stride_) == 0)
{
std_msgs::msg::String debug_msg;
debug_msg.data = build_debug_json(
points.size(), detection_debug, tracker_debug,
debug_tracks, output_tracks.size(), processing_ms);
debug_info_pub_->publish(debug_msg);
}
}
}
static double elapsed_ms(const std::chrono::steady_clock::time_point & start_time)
{
const auto end_time = std::chrono::steady_clock::now();
return std::chrono::duration<double, std::milli>(end_time - start_time).count();
}
static std::vector<obstacle_scanner::TrackOutput> tracks_from_fit_observations(
const std::vector<obstacle_scanner::Observation> & observations)
{
std::vector<obstacle_scanner::TrackOutput> tracks;
int id = 1;
for (const auto & observation : observations) {
if (observation.type != obstacle_scanner::ObservationType::CircleFit) {
continue;
}
obstacle_scanner::TrackOutput track;
track.id = id++;
track.center_x = observation.center.x();
track.center_y = observation.center.y();
track.radius = observation.radius;
track.hit_count = 1;
track.missed_count = 0;
track.last_source = obstacle_scanner::UpdateSource::Fit;
tracks.push_back(track);
}
return tracks;
}
sensor_msgs::msg::Image::SharedPtr render_debug_image(
const std::vector<Eigen::Vector2d> & points,
const std::vector<obstacle_scanner::TrackOutput> & tracks,
const obstacle_scanner::DetectionDebug & detection_debug,
const obstacle_scanner::FrameDebug & tracker_debug,
double processing_ms,
const builtin_interfaces::msg::Time & stamp)
{
const int size = debug_image_size_;
const double res = debug_resolution_;
const int origin = size / 2;
cv::Mat img(size, size, CV_8UC3, cv::Scalar(255, 255, 255));
for (const auto & pt : points) {
const int col = static_cast<int>(std::round(origin + pt.x() / res));
const int row = static_cast<int>(std::round(origin - pt.y() / res));
if (col >= 0 && col < size && row >= 0 && row < size) {
cv::circle(img, cv::Point(col, row), 1, cv::Scalar(0, 0, 255), cv::FILLED);
}
}
cv::rectangle(
img, cv::Point(origin - 2, origin - 2),
cv::Point(origin + 2, origin + 2),
cv::Scalar(0, 255, 255), cv::FILLED);
for (const auto & track : tracks) {
draw_track(img, track, origin, res);
}
char summary[160];
std::snprintf(
summary, sizeof(summary),
"pts=%zu cls=%d fit=%d chord=%d trk=%d pub=%d ms=%.1f",
points.size(), detection_debug.total_clusters,
detection_debug.fit_observations, detection_debug.chord_observations,
tracker_debug.active_tracks, tracker_debug.published_tracks, processing_ms);
cv::putText(
img, summary, cv::Point(8, size - 12),
cv::FONT_HERSHEY_SIMPLEX, 0.32, cv::Scalar(40, 40, 40), 1);
auto image_msg = cv_bridge::CvImage(
std_msgs::msg::Header(), "bgr8", img).toImageMsg();
image_msg->header.stamp = stamp;
image_msg->header.frame_id = frame_id_;
return image_msg;
}
static void draw_track(
cv::Mat & img,
const obstacle_scanner::TrackOutput & track,
int origin,
double res)
{
const int col = static_cast<int>(std::round(origin + track.center_x / res));
const int row = static_cast<int>(std::round(origin - track.center_y / res));
const int radius_px = std::max(1, static_cast<int>(std::round(track.radius / res)));
cv::Scalar color(160, 160, 160);
int line_type = cv::LINE_8;
if (track.last_source == obstacle_scanner::UpdateSource::Fit) {
color = cv::Scalar(0, 180, 0);
} else if (track.last_source == obstacle_scanner::UpdateSource::Chord) {
color = cv::Scalar(255, 0, 0);
} else {
line_type = cv::LINE_4;
}
cv::circle(img, cv::Point(col, row), radius_px, color, 1, line_type);
cv::circle(img, cv::Point(col, row), 2, color, cv::FILLED);
char label[48];
std::snprintf(
label, sizeof(label), "#%d %s h%d/m%d",
track.id, obstacle_scanner::update_source_name(track.last_source).c_str(),
track.hit_count, track.missed_count);
cv::putText(
img, label, cv::Point(col + radius_px + 2, row),
cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1);
}
static std::string build_debug_json(
std::size_t point_count,
const obstacle_scanner::DetectionDebug & detection_debug,
const obstacle_scanner::FrameDebug & tracker_debug,
const std::vector<obstacle_scanner::TrackOutput> & tracks,
std::size_t published_count,
double processing_ms)
{
std::ostringstream out;
out << "{";
out << "\"points\":" << point_count << ",";
out << "\"clusters\":" << detection_debug.total_clusters << ",";
out << "\"fit\":" << detection_debug.fit_observations << ",";
out << "\"chord\":" << detection_debug.chord_observations << ",";
out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
out << "\"tracks\":" << tracker_debug.active_tracks << ",";
out << "\"published\":" << published_count << ",";
out << "\"ms\":" << processing_ms << ",";
out << "\"sources\":[";
for (std::size_t i = 0; i < tracks.size(); ++i) {
if (i > 0) {
out << ",";
}
out << "\"" << obstacle_scanner::update_source_name(tracks[i].last_source) << "\"";
}
out << "]}";
return out.str();
}
std::string scan_topic_;
std::string frame_id_;
obstacle_scanner::DetectionConfig detection_config_;
obstacle_scanner::TrackerConfig tracker_config_;
obstacle_scanner::ObstacleTracker tracker_;
bool enable_tracking_{true};
bool debug_{false};
bool debug_info_{true};
int debug_image_size_{500};
double debug_resolution_{0.01};
int debug_image_stride_{3};
int debug_info_stride_{1};
std::uint64_t frame_count_{0};
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
rclcpp::Publisher<obstacle_scanner::msg::ObstacleArray>::SharedPtr obstacles_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr debug_info_pub_;
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ObstacleScannerNode>());
rclcpp::shutdown();
return 0;
}

17
src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej Normal file
View File

@@ -0,0 +1,17 @@
--- src/obstacle_scanner/src/obstacle_scanner_node.cpp
+++ src/obstacle_scanner/src/obstacle_scanner_node.cpp
@@ -275,6 +275,14 @@
out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
+ out << "\"tracks_created\":" << tracker_debug.tracks_created << ",";
+ out << "\"tracks_confirmed\":" << tracker_debug.tracks_confirmed << ",";
+ out << "\"tracks_deleted\":" << tracker_debug.tracks_deleted << ",";
+ out << "\"fit_updates\":" << tracker_debug.fit_updates << ",";
+ out << "\"association_rejections\":" << tracker_debug.association_rejections << ",";
+ out << "\"predicted_published\":" << tracker_debug.predicted_published << ",";
+ out << "\"mean_innovation\":" << tracker_debug.mean_innovation << ",";
+ out << "\"max_innovation\":" << tracker_debug.max_innovation << ",";
out << "\"tracks\":" << tracker_debug.active_tracks << ",";

439
src/obstacle_scanner/src/tracking.cpp
View File

@@ -2,125 +2,417 @@
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <vector>
namespace obstacle_scanner
{
namespace
{
struct AssociationCandidate
{
std::size_t observation_index{0};
std::size_t track_index{0};
Eigen::Vector2d center{Eigen::Vector2d::Zero()};
double mahalanobis_squared{0.0};
double euclidean_distance{0.0};
bool geometry_valid{true};
};
Eigen::Matrix<double, 2, 4> position_matrix()
{
Eigen::Matrix<double, 2, 4> matrix = Eigen::Matrix<double, 2, 4>::Zero();
matrix(0, 0) = 1.0;
matrix(1, 1) = 1.0;
return matrix;
}
bool innovation_for_track(
const Eigen::Vector4d & state,
const Eigen::Matrix4d & covariance,
const Eigen::Vector2d & center,
double position_stddev,
double & mahalanobis_squared,
double & euclidean_distance)
{
const auto h = position_matrix();
const Eigen::Vector2d innovation = center - h * state;
const Eigen::Matrix2d measurement_noise =
position_stddev * position_stddev * Eigen::Matrix2d::Identity();
const Eigen::Matrix2d innovation_covariance =
h * covariance * h.transpose() + measurement_noise;
Eigen::LDLT<Eigen::Matrix2d> ldlt(innovation_covariance);
if (ldlt.info() != Eigen::Success) {
return false;
}
const Eigen::Vector2d solved = ldlt.solve(innovation);
if (!solved.allFinite()) {
return false;
}
mahalanobis_squared = std::max(0.0, innovation.dot(solved));
euclidean_distance = innovation.norm();
return std::isfinite(mahalanobis_squared) && std::isfinite(euclidean_distance);
}
void validate_config(const TrackerConfig & config)
{
const auto finite_positive = [](double value) {
return std::isfinite(value) && value > 0.0;
};
const auto finite_nonnegative = [](double value) {
return std::isfinite(value) && value >= 0.0;
};
const bool valid =
config.track_confirm_fit_hits > 0 &&
config.track_confirm_total_hits > 0 &&
config.track_publish_misses >= 0 &&
config.track_delete_misses > config.track_publish_misses &&
finite_nonnegative(config.process_accel_noise) &&
finite_positive(config.initial_velocity_stddev) &&
finite_positive(config.fit_position_stddev) &&
finite_positive(config.chord_position_stddev) &&
finite_positive(config.mahalanobis_gate) &&
finite_positive(config.max_association_distance) &&
finite_positive(config.max_position_stddev) &&
finite_nonnegative(config.chord_tolerance) &&
finite_positive(config.nominal_dt) &&
finite_positive(config.min_dt) &&
finite_positive(config.max_dt) &&
config.min_dt <= config.nominal_dt &&
config.nominal_dt <= config.max_dt;
if (!valid) {
throw std::invalid_argument("invalid obstacle tracker configuration");
}
}
} // namespace
ObstacleTracker::ObstacleTracker(const TrackerConfig & config)
: config_(config)
{
validate_config(config_);
}
FrameDebug ObstacleTracker::update(const std::vector<Observation> & observations)
FrameDebug ObstacleTracker::update(
const std::vector<Observation> & observations,
double stamp_seconds)
{
FrameDebug debug;
debug.fit_observations = static_cast<int>(std::count_if(
observations.begin(), observations.end(),
[](const Observation & observation) {
return observation.type == ObservationType::CircleFit;
}));
debug.chord_observations = static_cast<int>(std::count_if(
observations.begin(), observations.end(),
[](const Observation & observation) {
return observation.type == ObservationType::TwoPointChord;
}));
if (!std::isfinite(stamp_seconds)) {
stamp_seconds = std::isfinite(last_stamp_seconds_) ?
last_stamp_seconds_ + config_.nominal_dt : 0.0;
}
double dt = config_.nominal_dt;
if (std::isfinite(last_stamp_seconds_)) {
const double raw_dt = stamp_seconds - last_stamp_seconds_;
if (std::isfinite(raw_dt) && raw_dt > 0.0) {
dt = std::clamp(raw_dt, config_.min_dt, config_.max_dt);
}
}
last_stamp_seconds_ = stamp_seconds;
for (auto & track : tracks_) {
track.matched = false;
}
predict_tracks(dt);
for (const auto & observation : observations) {
double innovation_sum = 0.0;
int innovation_count = 0;
std::vector<bool> observation_matched(observations.size(), false);
std::vector<AssociationCandidate> fit_candidates;
fit_candidates.reserve(observations.size() * tracks_.size());
for (std::size_t observation_index = 0;
observation_index < observations.size(); ++observation_index)
{
const auto & observation = observations[observation_index];
if (observation.type != ObservationType::CircleFit) {
continue;
}
debug.fit_observations++;
for (std::size_t track_index = 0; track_index < tracks_.size(); ++track_index) {
const auto & track = tracks_[track_index];
double mahalanobis_squared = 0.0;
double euclidean_distance = 0.0;
if (innovation_for_track(
track.state, track.covariance, observation.center,
config_.fit_position_stddev, mahalanobis_squared, euclidean_distance))
{
fit_candidates.push_back({
observation_index, track_index, observation.center,
mahalanobis_squared, euclidean_distance, true});
}
}
}
std::sort(
fit_candidates.begin(), fit_candidates.end(),
[](const AssociationCandidate & lhs, const AssociationCandidate & rhs) {
return lhs.mahalanobis_squared < rhs.mahalanobis_squared;
});
const int track_index = find_nearest_unmatched_track(observation.center);
if (track_index >= 0) {
auto & track = tracks_[static_cast<std::size_t>(track_index)];
const double alpha = config_.fit_update_alpha;
track.center = (1.0 - alpha) * track.center + alpha * observation.center;
track.radius = 0.8 * track.radius + 0.2 * observation.radius;
track.hit_count++;
track.missed_count = 0;
track.last_source = UpdateSource::Fit;
track.matched = true;
for (const auto & candidate : fit_candidates) {
if (observation_matched[candidate.observation_index] ||
tracks_[candidate.track_index].matched)
{
continue;
}
Track track;
track.id = next_id_++;
track.center = observation.center;
if (candidate.mahalanobis_squared > config_.mahalanobis_gate ||
candidate.euclidean_distance > config_.max_association_distance)
{
continue;
}
auto & track = tracks_[candidate.track_index];
const auto & observation = observations[candidate.observation_index];
track.fit_hits++;
track.total_hits++;
update_track(
track, candidate.center, config_.fit_position_stddev, UpdateSource::Fit,
debug, innovation_sum, innovation_count);
track.radius = observation.radius;
track.hit_count = 1;
track.missed_count = 0;
track.last_source = UpdateSource::Fit;
track.matched = true;
tracks_.push_back(track);
observation_matched[candidate.observation_index] = true;
debug.fit_updates++;
}
for (const auto & observation : observations) {
for (std::size_t observation_index = 0;
observation_index < observations.size(); ++observation_index)
{
const auto & observation = observations[observation_index];
if (observation.type != ObservationType::CircleFit ||
observation_matched[observation_index])
{
continue;
}
if (!tracks_.empty()) {
debug.association_rejections++;
}
Track track;
track.id = next_id_++;
track.state.head<2>() = observation.center;
track.covariance.diagonal() <<
config_.fit_position_stddev * config_.fit_position_stddev,
config_.fit_position_stddev * config_.fit_position_stddev,
config_.initial_velocity_stddev * config_.initial_velocity_stddev,
config_.initial_velocity_stddev * config_.initial_velocity_stddev;
track.radius = observation.radius;
track.fit_hits = 1;
track.total_hits = 1;
track.matched = true;
track.last_source = UpdateSource::Fit;
track.confirmed =
track.fit_hits >= config_.track_confirm_fit_hits ||
track.total_hits >= config_.track_confirm_total_hits;
tracks_.push_back(track);
observation_matched[observation_index] = true;
debug.tracks_created++;
debug.fit_updates++;
if (track.confirmed) {
debug.tracks_confirmed++;
}
}
std::vector<AssociationCandidate> chord_candidates;
chord_candidates.reserve(observations.size() * tracks_.size());
std::vector<bool> chord_geometry_valid(observations.size(), false);
for (std::size_t observation_index = 0;
observation_index < observations.size(); ++observation_index)
{
const auto & observation = observations[observation_index];
if (observation.type != ObservationType::TwoPointChord) {
continue;
}
debug.chord_observations++;
int best_index = -1;
double best_distance = std::numeric_limits<double>::infinity();
Eigen::Vector2d best_center{0.0, 0.0};
bool had_geometry_rejection = false;
for (std::size_t i = 0; i < tracks_.size(); ++i) {
const auto & track = tracks_[i];
for (std::size_t track_index = 0; track_index < tracks_.size(); ++track_index) {
const auto & track = tracks_[track_index];
if (track.matched) {
continue;
}
Eigen::Vector2d chord_center{0.0, 0.0};
if (!chord_center_for_track(observation, track, chord_center)) {
had_geometry_rejection = true;
Eigen::Vector2d center = Eigen::Vector2d::Zero();
if (!chord_center_for_track(observation, track, center)) {
continue;
}
const double distance = (chord_center - track.center).norm();
if (distance < config_.association_gate && distance < best_distance) {
best_index = static_cast<int>(i);
best_distance = distance;
best_center = chord_center;
chord_geometry_valid[observation_index] = true;
double mahalanobis_squared = 0.0;
double euclidean_distance = 0.0;
if (innovation_for_track(
track.state, track.covariance, center,
config_.chord_position_stddev, mahalanobis_squared, euclidean_distance))
{
chord_candidates.push_back({
observation_index, track_index, center,
mahalanobis_squared, euclidean_distance, true});
}
}
}
std::sort(
chord_candidates.begin(), chord_candidates.end(),
[](const AssociationCandidate & lhs, const AssociationCandidate & rhs) {
return lhs.mahalanobis_squared < rhs.mahalanobis_squared;
});
if (best_index >= 0) {
auto & track = tracks_[static_cast<std::size_t>(best_index)];
const double alpha = config_.chord_update_alpha;
track.center = (1.0 - alpha) * track.center + alpha * best_center;
track.hit_count++;
track.missed_count = 0;
track.last_source = UpdateSource::Chord;
track.matched = true;
debug.chord_updates++;
} else if (had_geometry_rejection) {
std::vector<bool> chord_matched(observations.size(), false);
for (const auto & candidate : chord_candidates) {
if (chord_matched[candidate.observation_index] ||
tracks_[candidate.track_index].matched)
{
continue;
}
if (candidate.mahalanobis_squared > config_.mahalanobis_gate ||
candidate.euclidean_distance > config_.max_association_distance)
{
continue;
}
auto & track = tracks_[candidate.track_index];
track.total_hits++;
update_track(
track, candidate.center, config_.chord_position_stddev, UpdateSource::Chord,
debug, innovation_sum, innovation_count);
chord_matched[candidate.observation_index] = true;
debug.chord_updates++;
}
for (std::size_t observation_index = 0;
observation_index < observations.size(); ++observation_index)
{
const auto & observation = observations[observation_index];
if (observation.type != ObservationType::TwoPointChord ||
chord_matched[observation_index])
{
continue;
}
bool has_in_gate_candidate = false;
for (const auto & candidate : chord_candidates) {
if (candidate.observation_index == observation_index &&
candidate.mahalanobis_squared <= config_.mahalanobis_gate &&
candidate.euclidean_distance <= config_.max_association_distance)
{
has_in_gate_candidate = true;
break;
}
}
if (chord_geometry_valid[observation_index] || has_in_gate_candidate) {
debug.association_rejections++;
} else if (!tracks_.empty()) {
debug.chord_rejections++;
}
}
for (auto & track : tracks_) {
if (track.matched) {
track.missed_count = 0;
continue;
}
track.missed_count++;
track.last_source = UpdateSource::Predict;
}
const auto old_size = tracks_.size();
tracks_.erase(
std::remove_if(
tracks_.begin(), tracks_.end(),
[&](const Track & track) {
return track.missed_count >= config_.track_delete_misses;
return (!track.confirmed && track.missed_count >= 1) ||
(track.confirmed && track.missed_count >= config_.track_delete_misses);
}),
tracks_.end());
debug.tracks_deleted = static_cast<int>(old_size - tracks_.size());
refresh_debug_outputs();
debug.active_tracks = static_cast<int>(tracks_.size());
debug.published_tracks = static_cast<int>(confirmed_tracks().size());
debug.predicted_published = static_cast<int>(std::count_if(
tracks_.begin(), tracks_.end(),
[&](const Track & track) {
return !track.matched && is_publishable(track);
}));
if (innovation_count > 0) {
debug.mean_innovation = innovation_sum / static_cast<double>(innovation_count);
}
return debug;
}
void ObstacleTracker::predict_tracks(double dt)
{
Eigen::Matrix4d transition = Eigen::Matrix4d::Identity();
transition(0, 2) = dt;
transition(1, 3) = dt;
Eigen::Matrix<double, 4, 2> gain;
gain << 0.5 * dt * dt, 0.0,
0.0, 0.5 * dt * dt,
dt, 0.0,
0.0, dt;
const Eigen::Matrix4d process_noise =
std::pow(config_.process_accel_noise, 2) * gain * gain.transpose();
for (auto & track : tracks_) {
track.state = transition * track.state;
track.covariance =
transition * track.covariance * transition.transpose() + process_noise;
track.covariance =
0.5 * (track.covariance + track.covariance.transpose());
}
}
void ObstacleTracker::update_track(
Track & track,
const Eigen::Vector2d & center,
double position_stddev,
UpdateSource source,
FrameDebug & debug,
double & innovation_sum,
int & innovation_count)
{
const auto h = position_matrix();
const Eigen::Matrix2d measurement_noise =
position_stddev * position_stddev * Eigen::Matrix2d::Identity();
const Eigen::Vector2d innovation = center - h * track.state;
const Eigen::Matrix2d innovation_covariance =
h * track.covariance * h.transpose() + measurement_noise;
Eigen::LDLT<Eigen::Matrix2d> ldlt(innovation_covariance);
if (ldlt.info() != Eigen::Success) {
return;
}
const Eigen::Matrix<double, 4, 2> kalman_gain =
track.covariance * h.transpose() * ldlt.solve(Eigen::Matrix2d::Identity());
track.state += kalman_gain * innovation;
const Eigen::Matrix4d identity = Eigen::Matrix4d::Identity();
const Eigen::Matrix4d residual_projection = identity - kalman_gain * h;
track.covariance =
residual_projection * track.covariance * residual_projection.transpose() +
kalman_gain * measurement_noise * kalman_gain.transpose();
track.covariance =
0.5 * (track.covariance + track.covariance.transpose());
track.missed_count = 0;
track.last_source = source;
track.matched = true;
innovation_sum += innovation.norm();
innovation_count++;
debug.max_innovation = std::max(debug.max_innovation, innovation.norm());
if (
!track.confirmed &&
(track.fit_hits >= config_.track_confirm_fit_hits ||
track.total_hits >= config_.track_confirm_total_hits))
{
track.confirmed = true;
debug.tracks_confirmed++;
}
}
std::vector<TrackOutput> ObstacleTracker::confirmed_tracks() const
{
std::vector<TrackOutput> outputs;
for (const auto & track : tracks_) {
if (track.hit_count >= config_.track_confirm_hits) {
if (is_publishable(track)) {
outputs.push_back(to_output(track));
}
}
@@ -132,24 +424,6 @@ const std::vector<TrackOutput> & ObstacleTracker::tracks_for_debug() const
return debug_outputs_;
}
int ObstacleTracker::find_nearest_unmatched_track(const Eigen::Vector2d & center) const
{
int best_index = -1;
double best_distance = std::numeric_limits<double>::infinity();
for (std::size_t i = 0; i < tracks_.size(); ++i) {
const auto & track = tracks_[i];
if (track.matched) {
continue;
}
const double distance = (center - track.center).norm();
if (distance < config_.association_gate && distance < best_distance) {
best_index = static_cast<int>(i);
best_distance = distance;
}
}
return best_index;
}
bool ObstacleTracker::chord_center_for_track(
const Observation & observation,
const Track & track,
@@ -165,8 +439,7 @@ bool ObstacleTracker::chord_center_for_track(
}
const double half_chord = 0.5 * chord_length;
const double radius_squared = track.radius * track.radius;
const double h_squared = radius_squared - half_chord * half_chord;
const double h_squared = track.radius * track.radius - half_chord * half_chord;
if (h_squared < 0.0) {
return false;
}
@@ -181,14 +454,24 @@ bool ObstacleTracker::chord_center_for_track(
return true;
}
bool ObstacleTracker::is_publishable(const Track & track) const
{
const double position_stddev = std::sqrt(std::max(
track.covariance(0, 0), track.covariance(1, 1)));
return track.confirmed &&
track.missed_count <= config_.track_publish_misses &&
std::isfinite(position_stddev) &&
position_stddev <= config_.max_position_stddev;
}
TrackOutput ObstacleTracker::to_output(const Track & track)
{
TrackOutput output;
output.id = track.id;
output.center_x = track.center.x();
output.center_y = track.center.y();
output.center_x = track.state.x();
output.center_y = track.state.y();
output.radius = track.radius;
output.hit_count = track.hit_count;
output.hit_count = track.total_hits;
output.missed_count = track.missed_count;
output.last_source = track.last_source;
return output;

324
src/obstacle_scanner/test/test_tracking.cpp
View File

@@ -3,7 +3,8 @@
#include <gtest/gtest.h>
#include <cmath>
#include <vector>
#include <limits>
#include <stdexcept>
namespace obstacle_scanner
{
@@ -13,104 +14,279 @@ namespace
TrackerConfig test_config()
{
TrackerConfig config;
config.track_confirm_hits = 1;
config.track_delete_misses = 2;
config.association_gate = 0.25;
config.track_confirm_fit_hits = 2;
config.track_confirm_total_hits = 3;
config.track_publish_misses = 3;
config.track_delete_misses = 5;
config.process_accel_noise = 3.0;
config.initial_velocity_stddev = 2.5;
config.fit_position_stddev = 0.02;
config.chord_position_stddev = 0.06;
config.mahalanobis_gate = 9.21;
config.max_association_distance = 0.35;
config.max_position_stddev = 0.15;
config.chord_tolerance = 0.02;
config.fit_update_alpha = 0.7;
config.chord_update_alpha = 0.3;
config.nominal_dt = 1.0 / 12.0;
config.min_dt = 0.02;
config.max_dt = 0.20;
return config;
}
TEST(ObstacleTrackerTest, CircleFitCreatesConfirmedTrack)
Observation fit(double x, double y = 0.0, double radius = 0.04)
{
Observation observation;
observation.type = ObservationType::CircleFit;
observation.center = Eigen::Vector2d(x, y);
observation.radius = radius;
observation.cluster_size = 4;
return observation;
}
Observation chord(double x, double half_height = 0.02)
{
Observation observation;
observation.type = ObservationType::TwoPointChord;
observation.p1 = Eigen::Vector2d(x, -half_height);
observation.p2 = Eigen::Vector2d(x, half_height);
observation.cluster_size = 2;
return observation;
}
void confirm_track(ObstacleTracker & tracker, double x = 1.0)
{
tracker.update({fit(x)}, 0.0);
tracker.update({fit(x)}, 1.0 / 12.0);
}
void seed_moving_track(ObstacleTracker & tracker)
{
tracker.update({fit(1.0)}, 0.0);
tracker.update({fit(1.1666667)}, 1.0 / 12.0);
tracker.update({fit(1.3333333)}, 2.0 / 12.0);
}
TEST(ObstacleTrackerTest, FirstFitIsTentativeAndSecondFitConfirms)
{
ObstacleTracker tracker(test_config());
Observation obs;
obs.type = ObservationType::CircleFit;
obs.center = Eigen::Vector2d(1.0, 0.2);
obs.radius = 0.04;
obs.cluster_size = 4;
FrameDebug debug = tracker.update({obs});
const FrameDebug first = tracker.update({fit(1.0, 0.2)}, 0.0);
EXPECT_TRUE(tracker.confirmed_tracks().empty());
EXPECT_EQ(first.tracks_created, 1);
EXPECT_EQ(first.tracks_confirmed, 0);
EXPECT_EQ(first.fit_updates, 1);
ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 1);
const FrameDebug second = tracker.update({fit(1.0, 0.2)}, 1.0 / 12.0);
const auto tracks = tracker.confirmed_tracks();
ASSERT_EQ(tracks.size(), 1u);
EXPECT_EQ(debug.fit_observations, 1);
EXPECT_EQ(debug.published_tracks, 1);
EXPECT_EQ(second.tracks_confirmed, 1);
EXPECT_EQ(second.fit_updates, 1);
EXPECT_EQ(tracks[0].hit_count, 2);
EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit);
EXPECT_NEAR(tracks[0].center_x, 1.0, 1e-9);
EXPECT_NEAR(tracks[0].center_y, 0.2, 1e-9);
EXPECT_NEAR(tracks[0].radius, 0.04, 1e-9);
EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit);
}
TEST(ObstacleTrackerTest, TwoPointChordUpdatesExistingTrack)
TEST(ObstacleTrackerTest, TracksTwoMetersPerSecondAcrossOneEmptyFrame)
{
ObstacleTracker tracker(test_config());
Observation fit;
fit.type = ObservationType::CircleFit;
fit.center = Eigen::Vector2d(1.0, 0.0);
fit.radius = 0.05;
fit.cluster_size = 4;
tracker.update({fit});
tracker.update({fit(1.0)}, 0.0);
tracker.update({fit(1.1666667)}, 1.0 / 12.0);
tracker.update({fit(1.3333333)}, 2.0 / 12.0);
Observation chord;
chord.type = ObservationType::TwoPointChord;
chord.p1 = Eigen::Vector2d(0.985, -0.02);
chord.p2 = Eigen::Vector2d(0.985, 0.02);
chord.cluster_size = 2;
FrameDebug debug = tracker.update({chord});
const FrameDebug debug = tracker.update({}, 3.0 / 12.0);
const auto tracks = tracker.confirmed_tracks();
ASSERT_EQ(tracks.size(), 1u);
EXPECT_EQ(debug.chord_observations, 1);
EXPECT_EQ(debug.chord_updates, 1);
EXPECT_EQ(debug.chord_rejections, 0);
EXPECT_EQ(tracks[0].last_source, UpdateSource::Chord);
EXPECT_GT(tracks[0].center_x, 1.0);
EXPECT_NEAR(tracks[0].center_y, 0.0, 1e-6);
}
TEST(ObstacleTrackerTest, TwoPointChordLongerThanDiameterIsRejected)
{
ObstacleTracker tracker(test_config());
Observation fit;
fit.type = ObservationType::CircleFit;
fit.center = Eigen::Vector2d(1.0, 0.0);
fit.radius = 0.04;
fit.cluster_size = 4;
tracker.update({fit});
Observation chord;
chord.type = ObservationType::TwoPointChord;
chord.p1 = Eigen::Vector2d(1.0, -0.08);
chord.p2 = Eigen::Vector2d(1.0, 0.08);
chord.cluster_size = 2;
FrameDebug debug = tracker.update({chord});
const auto tracks = tracker.confirmed_tracks();
ASSERT_EQ(tracks.size(), 1u);
EXPECT_EQ(debug.chord_updates, 0);
EXPECT_EQ(debug.chord_rejections, 1);
EXPECT_EQ(debug.predicted_published, 1);
EXPECT_EQ(tracks[0].last_source, UpdateSource::Predict);
EXPECT_NEAR(tracks[0].center_x, 1.5, 0.08);
}
TEST(ObstacleTrackerTest, StaleTracksAreDeleted)
TEST(ObstacleTrackerTest, TwoFitsSeparatedBySixteenCentimetersShareOneTrack)
{
ObstacleTracker tracker(test_config());
Observation fit;
fit.type = ObservationType::CircleFit;
fit.center = Eigen::Vector2d(1.0, 0.0);
fit.radius = 0.04;
fit.cluster_size = 4;
tracker.update({fit});
tracker.update({});
EXPECT_EQ(tracker.confirmed_tracks().size(), 1u);
tracker.update({fit(1.0)}, 0.0);
const FrameDebug debug = tracker.update({fit(1.1666667)}, 1.0 / 12.0);
tracker.update({});
EXPECT_TRUE(tracker.confirmed_tracks().empty());
EXPECT_EQ(debug.tracks_created, 0);
ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
}
TEST(ObstacleTrackerTest, FarFitStartsTentativeTrackWithoutDuplicatePublication)
{
ObstacleTracker tracker(test_config());
confirm_track(tracker);
const FrameDebug debug = tracker.update({fit(1.5)}, 2.0 / 12.0);
ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
EXPECT_EQ(debug.tracks_created, 1);
EXPECT_EQ(debug.association_rejections, 1);
}
TEST(ObstacleTrackerTest, OneFitAndTwoChordsConfirmOneTrack)
{
ObstacleTracker tracker(test_config());
tracker.update({fit(1.0)}, 0.0);
const FrameDebug first_chord = tracker.update({chord(0.985)}, 1.0 / 12.0);
const FrameDebug second_chord = tracker.update({chord(0.985)}, 2.0 / 12.0);
ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
EXPECT_EQ(first_chord.chord_updates, 1);
EXPECT_EQ(second_chord.chord_updates, 1);
EXPECT_EQ(second_chord.tracks_confirmed, 1);
}
TEST(ObstacleTrackerTest, ChordBeyondAssociationDistanceIsRejected)
{
ObstacleTracker tracker(test_config());
confirm_track(tracker);
const FrameDebug debug = tracker.update({chord(1.5, 0.01)}, 2.0 / 12.0);
ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
EXPECT_EQ(debug.chord_updates, 0);
EXPECT_EQ(debug.chord_rejections, 0);
EXPECT_EQ(debug.association_rejections, 1);
}
TEST(ObstacleTrackerTest, ValidChordUsesFarSideCandidate)
{
ObstacleTracker tracker(test_config());
tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);
const FrameDebug debug = tracker.update({chord(0.985)}, 1.0 / 12.0);
ASSERT_EQ(debug.chord_updates, 1);
ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
EXPECT_GT(tracker.tracks_for_debug()[0].center_x, 1.0);
}
TEST(ObstacleTrackerTest, ConfirmedTrackPublishesThroughThirdMissOnly)
{
ObstacleTracker tracker(test_config());
confirm_track(tracker);
EXPECT_EQ(tracker.update({}, 2.0 / 12.0).published_tracks, 1);
EXPECT_EQ(tracker.update({}, 3.0 / 12.0).published_tracks, 1);
EXPECT_EQ(tracker.update({}, 4.0 / 12.0).published_tracks, 1);
EXPECT_EQ(tracker.update({}, 5.0 / 12.0).published_tracks, 0);
EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
EXPECT_EQ(tracker.update({}, 6.0 / 12.0).tracks_deleted, 1);
EXPECT_TRUE(tracker.tracks_for_debug().empty());
}
TEST(ObstacleTrackerTest, LargePositionUncertaintyStopsPublication)
{
TrackerConfig config = test_config();
config.process_accel_noise = 10.0;
config.max_position_stddev = 0.02;
ObstacleTracker tracker(config);
confirm_track(tracker);
const FrameDebug debug = tracker.update({}, 2.0 / 12.0);
EXPECT_EQ(debug.published_tracks, 0);
EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
}
TEST(ObstacleTrackerTest, TentativeTrackIsDeletedOnFirstMiss)
{
ObstacleTracker tracker(test_config());
tracker.update({fit(1.0)}, 0.0);
const FrameDebug debug = tracker.update({}, 1.0 / 12.0);
EXPECT_EQ(debug.tracks_deleted, 1);
EXPECT_TRUE(tracker.tracks_for_debug().empty());
}
TEST(ObstacleTrackerTest, InvalidTimestampBoundsAreRejected)
{
TrackerConfig config = test_config();
config.min_dt = 0.20;
config.max_dt = 0.02;
EXPECT_THROW(ObstacleTracker tracker(config), std::invalid_argument);
}
TEST(ObstacleTrackerTest, DuplicateAndInvalidTimestampsUseNominalDelta)
{
ObstacleTracker reference(test_config());
ObstacleTracker duplicate(test_config());
ObstacleTracker invalid(test_config());
seed_moving_track(reference);
seed_moving_track(duplicate);
seed_moving_track(invalid);
reference.update({}, 3.0 / 12.0);
duplicate.update({}, 2.0 / 12.0);
invalid.update({}, std::numeric_limits<double>::quiet_NaN());
ASSERT_EQ(reference.tracks_for_debug().size(), 1u);
ASSERT_EQ(duplicate.tracks_for_debug().size(), 1u);
ASSERT_EQ(invalid.tracks_for_debug().size(), 1u);
EXPECT_NEAR(
duplicate.tracks_for_debug()[0].center_x,
reference.tracks_for_debug()[0].center_x, 1e-9);
EXPECT_NEAR(
invalid.tracks_for_debug()[0].center_x,
reference.tracks_for_debug()[0].center_x, 1e-9);
}
TEST(ObstacleTrackerTest, ValidTimestampDeltaIsClampedToConfiguredBounds)
{
ObstacleTracker lower_reference(test_config());
ObstacleTracker lower_clamped(test_config());
ObstacleTracker upper_reference(test_config());
ObstacleTracker upper_clamped(test_config());
seed_moving_track(lower_reference);
seed_moving_track(lower_clamped);
seed_moving_track(upper_reference);
seed_moving_track(upper_clamped);
const double last_stamp = 2.0 / 12.0;
lower_reference.update({}, last_stamp + test_config().min_dt);
lower_clamped.update({}, last_stamp + 0.001);
upper_reference.update({}, last_stamp + test_config().max_dt);
upper_clamped.update({}, last_stamp + 1.0);
EXPECT_NEAR(
lower_clamped.tracks_for_debug()[0].center_x,
lower_reference.tracks_for_debug()[0].center_x, 1e-9);
EXPECT_NEAR(
upper_clamped.tracks_for_debug()[0].center_x,
upper_reference.tracks_for_debug()[0].center_x, 1e-9);
}
TEST(ObstacleTrackerTest, GreedyAssociationAssignsTwoFitsToTwoTracks)
{
ObstacleTracker tracker(test_config());
tracker.update({fit(1.0), fit(2.0)}, 0.0);
const FrameDebug debug = tracker.update(
{fit(2.05), fit(1.05)}, 1.0 / 12.0);
EXPECT_EQ(debug.tracks_created, 0);
EXPECT_EQ(debug.fit_updates, 2);
ASSERT_EQ(tracker.confirmed_tracks().size(), 2u);
ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
EXPECT_EQ(tracker.tracks_for_debug()[1].hit_count, 2);
}
TEST(ObstacleTrackerTest, FitAssociationTakesPriorityOverChordInSameFrame)
{
ObstacleTracker tracker(test_config());
tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);
const FrameDebug debug = tracker.update(
{chord(0.985), fit(1.01, 0.0, 0.05)}, 1.0 / 12.0);
EXPECT_EQ(debug.fit_updates, 1);
EXPECT_EQ(debug.chord_updates, 0);
ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
EXPECT_EQ(tracker.confirmed_tracks()[0].last_source, UpdateSource::Fit);
}
} // namespace