From fc6e796c287c68c87f30662989fd32f31fdc1f49 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Wed, 22 Jul 2026 17:30:03 +0800 Subject: [PATCH] =?UTF-8?q?=E9=9A=9C=E7=A2=8D=E7=89=A9=E6=A3=80=E6=B5=8Bv3?= =?UTF-8?q?=EF=BC=8C=E6=94=B9=E6=88=90=E4=BA=86=E5=8D=A1=E5=B0=94=E6=9B=BC?= =?UTF-8?q?=E6=BB=A4=E6=B3=A2=E5=AE=8C=E6=95=B4=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/obstacle_scanner/config/params.yaml | 91 ++-- .../include/obstacle_scanner/tracking.hpp | 56 ++- .../src/obstacle_scanner_node.cpp | 32 +- .../src/obstacle_scanner_node.cpp.orig | 321 +++++++++++++ .../src/obstacle_scanner_node.cpp.rej | 17 + src/obstacle_scanner/src/tracking.cpp | 439 ++++++++++++++---- src/obstacle_scanner/test/test_tracking.cpp | 324 ++++++++++--- 7 files changed, 1069 insertions(+), 211 deletions(-) create mode 100644 src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig create mode 100644 src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej diff --git a/src/obstacle_scanner/config/params.yaml b/src/obstacle_scanner/config/params.yaml index ba5f948..2d9c057 100644 --- a/src/obstacle_scanner/config/params.yaml +++ b/src/obstacle_scanner/config/params.yaml @@ -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 diff --git a/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp b/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp index 9ff4894..8d248dc 100644 --- a/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp +++ b/src/obstacle_scanner/include/obstacle_scanner/tracking.hpp @@ -2,7 +2,7 @@ #include -#include +#include #include #include @@ -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 & observations); + FrameDebug update( + const std::vector & observations, + double stamp_seconds); std::vector confirmed_tracks() const; const std::vector & 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::quiet_NaN()}; std::vector tracks_; std::vector debug_outputs_; }; diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp b/src/obstacle_scanner/src/obstacle_scanner_node.cpp index 13d4d55..e9a7d53 100644 --- a/src/obstacle_scanner/src/obstacle_scanner_node.cpp +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp @@ -31,17 +31,26 @@ public: frame_id_ = declare_parameter("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 output_tracks; std::vector 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 << ","; diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig b/src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig new file mode 100644 index 0000000..4af875d --- /dev/null +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig @@ -0,0 +1,321 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "obstacle_scanner/detection.hpp" +#include "obstacle_scanner/msg/obstacle.hpp" +#include "obstacle_scanner/msg/obstacle_array.hpp" +#include "obstacle_scanner/tracking.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class ObstacleScannerNode : public rclcpp::Node +{ +public: + ObstacleScannerNode() + : rclcpp::Node("obstacle_scanner") + { + using std::placeholders::_1; + + scan_topic_ = declare_parameter("scan_topic", "/scan"); + frame_id_ = declare_parameter("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(declare_parameter("debug_image_stride", 3))); + debug_info_stride_ = std::max( + 1, static_cast(declare_parameter("debug_info_stride", 1))); + + obstacles_pub_ = create_publisher( + "/obstacles", 10); + + if (debug_) { + debug_pub_ = create_publisher( + "/processed_scan", 10); + if (debug_info_) { + debug_info_pub_ = create_publisher( + "/obstacle_scanner/debug_info", 10); + } + } + + scan_sub_ = create_subscription( + 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 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(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 output_tracks; + std::vector 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(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(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(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(end_time - start_time).count(); + } + + static std::vector tracks_from_fit_observations( + const std::vector & observations) + { + std::vector 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 & points, + const std::vector & 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(std::round(origin + pt.x() / res)); + const int row = static_cast(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(std::round(origin + track.center_x / res)); + const int row = static_cast(std::round(origin - track.center_y / res)); + const int radius_px = std::max(1, static_cast(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 & 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::SharedPtr scan_sub_; + rclcpp::Publisher::SharedPtr obstacles_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr debug_info_pub_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej b/src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej new file mode 100644 index 0000000..e08226e --- /dev/null +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej @@ -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 << ","; + + + diff --git a/src/obstacle_scanner/src/tracking.cpp b/src/obstacle_scanner/src/tracking.cpp index 0dc7ab4..6f1aa9c 100644 --- a/src/obstacle_scanner/src/tracking.cpp +++ b/src/obstacle_scanner/src/tracking.cpp @@ -2,125 +2,417 @@ #include #include -#include +#include +#include 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 position_matrix() +{ + Eigen::Matrix matrix = Eigen::Matrix::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 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 & observations) +FrameDebug ObstacleTracker::update( + const std::vector & observations, + double stamp_seconds) { FrameDebug debug; + debug.fit_observations = static_cast(std::count_if( + observations.begin(), observations.end(), + [](const Observation & observation) { + return observation.type == ObservationType::CircleFit; + })); + debug.chord_observations = static_cast(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 observation_matched(observations.size(), false); + + std::vector 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(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 chord_candidates; + chord_candidates.reserve(observations.size() * tracks_.size()); + std::vector 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::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(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(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 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(old_size - tracks_.size()); refresh_debug_outputs(); debug.active_tracks = static_cast(tracks_.size()); debug.published_tracks = static_cast(confirmed_tracks().size()); + debug.predicted_published = static_cast(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(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 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 ldlt(innovation_covariance); + if (ldlt.info() != Eigen::Success) { + return; + } + const Eigen::Matrix 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 ObstacleTracker::confirmed_tracks() const { std::vector 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 & 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::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(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; diff --git a/src/obstacle_scanner/test/test_tracking.cpp b/src/obstacle_scanner/test/test_tracking.cpp index e946ed7..dadd980 100644 --- a/src/obstacle_scanner/test/test_tracking.cpp +++ b/src/obstacle_scanner/test/test_tracking.cpp @@ -3,7 +3,8 @@ #include #include -#include +#include +#include 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::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