diff --git a/README.md b/README.md index 5f5d53f..162ce5d 100644 --- a/README.md +++ b/README.md @@ -13,10 +13,26 @@ ## 配置开关 - `enabled`：总开关。 -- `record_raw`：写原始视频和 topic 数据 TSV。 +- `record_raw`：写原始视频、overlay 视频和 topic 数据 TSV。 - `realtime_preview`：打开实时窗口。 - `overlay.detector`：绘制 detector 框、角点和置信度。 -- `overlay.tracker`：绘制 tracker EKF 中心和装甲板点。 +- `overlay.tracker`：绘制 tracker 中心和 tracker 选中观测对应的 EKF 面。 - `overlay.candidate_debug`：显示候选统计。 +- `overlay.model_faces`：额外绘制 EKF 模型补全的全部装甲板点，默认关闭。 关闭 `record_raw` 和 `realtime_preview` 时，模块不会启动线程或注册回调。 + +`record_raw` 打开后默认写： + +- `raw.avi`：原始图像。 +- `overlay.avi`：带 detector、tracker 匹配面和状态信息的预览视频。 +- `detector.tsv`、`metrics.tsv`、`target.tsv`、`ekf_points.tsv`、 + `candidate_debug.tsv`、`candidate_items.tsv`：按 topic 落盘的数据。 + +## 颜色约定 + +- detector 普通框：按装甲板颜色绘制。 +- `M`：tracker 当前匹配的 detector 观测，品红色。 +- `TC`：tracker 目标中心，绿色。 +- `EF`：tracker 匹配面的 EKF 投影，黄色。 +- `model*`：EKF 模型补全的其它装甲板点，灰色，仅 `overlay.model_faces=true` 时显示。 diff --git a/VisionPreview.hpp b/VisionPreview.hpp index beb3b17..69a9823 100644 --- a/VisionPreview.hpp +++ b/VisionPreview.hpp @@ -12,8 +12,10 @@ module_description: 视觉链路预览与原始数据落盘 detector: true tracker: true candidate_debug: false + model_faces: false output_dir: "/tmp/autoaim_preview" raw_video_name: "raw.avi" + overlay_video_name: "overlay.avi" preview_window_name: "autoaim_preview" preview_scale: 0.5 preview_wait_key_ms: 1 @@ -97,9 +99,10 @@ class VisionPreview : public LibXR::Application struct OverlayConfig { - bool detector = true; // 绘制 detector 原始识别框、角点和 PnP 文本。 - bool tracker = true; // 绘制 tracker EKF 中心和装甲板投影点。 - bool candidate_debug = false; // 仅显示轻量候选统计，不画复杂候选表。 + bool detector = true; // 绘制 detector 原始识别框、角点和置信度。 + bool tracker = true; // 绘制 tracker 中心和匹配面 EKF 投影。 + bool candidate_debug = false; // 显示候选统计。 + bool model_faces = false; // 额外绘制 EKF 模型补全的全部装甲板点。 }; struct RuntimeParam @@ -110,6 +113,7 @@ class VisionPreview : public LibXR::Application OverlayConfig overlay{}; std::string_view output_dir = "/tmp/autoaim_preview"; std::string_view raw_video_name = "raw.avi"; + std::string_view overlay_video_name = "overlay.avi"; std::string_view preview_window_name = "autoaim_preview"; double preview_scale = 0.5; int preview_wait_key_ms = 1; @@ -122,6 +126,7 @@ class VisionPreview : public LibXR::Application image_topic_name_(sync.ImageTopicName()), output_dir_(runtime.output_dir), raw_video_name_(runtime.raw_video_name), + overlay_video_name_(runtime.overlay_video_name), preview_window_name_(runtime.preview_window_name) { (void)hw; @@ -269,6 +274,12 @@ class VisionPreview : public LibXR::Application return runtime_.enabled && (runtime_.record_raw || runtime_.realtime_preview); } + bool OverlayEnabled() const + { + return runtime_.overlay.detector || runtime_.overlay.tracker || + runtime_.overlay.candidate_debug; + } + void RegisterCallbacks() { LibXR::Topic::Domain detector_domain("armor_detector"); @@ -584,7 +595,7 @@ class VisionPreview : public LibXR::Application WriteRawVideo(bgr); } - if (!realtime_preview_enabled_) + if (!realtime_preview_enabled_ && !(RecordEnabled() && OverlayEnabled())) { return; } @@ -595,14 +606,27 @@ class VisionPreview : public LibXR::Application if (runtime_.overlay.detector && snapshot.detector_valid) { - DrawDetector(canvas, snapshot.detector); + DrawDetector(canvas, snapshot.detector, + snapshot.candidate_valid ? &snapshot.candidate : nullptr); } if (runtime_.overlay.tracker && snapshot.ekf_valid) { - DrawTracker(canvas, snapshot.ekf); + DrawTracker(canvas, snapshot.ekf, + snapshot.detector_valid ? &snapshot.detector : nullptr, + snapshot.candidate_valid ? &snapshot.candidate : nullptr); } DrawStatus(canvas, timestamp_us, snapshot); + if (RecordEnabled() && OverlayEnabled()) + { + WriteOverlayVideo(canvas); + } + + if (!realtime_preview_enabled_) + { + return; + } + if (runtime_.preview_scale > 0.0 && std::abs(runtime_.preview_scale - 1.0) > 1e-6) { @@ -700,11 +724,40 @@ class VisionPreview : public LibXR::Application return ARMOR_NUMBER_NAMES[index]; } - void DrawDetector(cv::Mat& canvas, const DetectorMessage& detector) + std::array MatchedFaces( + const CandidateDebugMessage* candidate) const + { + std::array matched_faces{}; + matched_faces.fill(-1); + if (candidate == nullptr || candidate->matched == 0U) + { + return matched_faces; + } + + const uint8_t count = std::min(candidate->count, + CandidateDebugMessage::kMaxItems); + for (uint8_t i = 0; i < count; ++i) + { + const auto& item = candidate->items[i]; + if (item.armor_index < matched_faces.size()) + { + matched_faces[item.armor_index] = static_cast(item.face_index); + } + } + return matched_faces; + } + + void DrawDetector(cv::Mat& canvas, const DetectorMessage& detector, + const CandidateDebugMessage* candidate) { - for (const auto& armor : detector.results) + const auto matched_faces = MatchedFaces(candidate); + for (std::size_t index = 0; index < detector.results.size(); ++index) { + const auto& armor = detector.results[index]; + const bool is_matched = + index < matched_faces.size() && matched_faces[index] >= 0; const cv::Scalar color = ArmorColorToScalar(armor.color); + const cv::Scalar draw_color = is_matched ? cv::Scalar(255, 0, 255) : color; std::array points{}; for (std::size_t i = 0; i < armor.points.size(); ++i) { @@ -712,68 +765,138 @@ class VisionPreview : public LibXR::Application } const cv::Point* polygon = points.data(); const int point_count = static_cast(points.size()); - cv::polylines(canvas, &polygon, &point_count, 1, true, color, 2, cv::LINE_AA); - cv::rectangle(canvas, armor.box, color, 1, cv::LINE_AA); + cv::polylines(canvas, &polygon, &point_count, 1, true, draw_color, 2, + cv::LINE_AA); + cv::rectangle(canvas, armor.box, draw_color, 1, cv::LINE_AA); std::ostringstream label; - label << ArmorNumberName(armor.number) << " " - << std::fixed << std::setprecision(2) << armor.confidence; + if (is_matched) + { + label << "M" << index << " f=" << matched_faces[index] << " "; + } + label << ArmorNumberName(armor.number) << " " << std::fixed + << std::setprecision(2) << armor.confidence; cv::putText(canvas, label.str(), cv::Point(std::max(armor.box.x, 4), std::max(armor.box.y - 6, 18)), - cv::FONT_HERSHEY_SIMPLEX, 0.55, color, 1, cv::LINE_AA); + cv::FONT_HERSHEY_SIMPLEX, 0.55, draw_color, 1, cv::LINE_AA); } } - void DrawTracker(cv::Mat& canvas, const EkfPointsMessage& ekf) + bool ProjectPoint(const cv::Mat& canvas, const LibXR::Position& point, + cv::Point2d& uv) const { const cv::Mat camera_matrix = ScaledCameraMatrix(canvas); const cv::Mat dist_coeffs = DistCoeffs(); - auto project = [&](const LibXR::Position& point, cv::Point2d& uv) + const Eigen::Vector3d pc(point.x(), point.y(), point.z()); + if (!(pc.z() > 1e-6) || !std::isfinite(pc.x()) || !std::isfinite(pc.y()) || + !std::isfinite(pc.z())) { - const Eigen::Vector3d pc(point.x(), point.y(), point.z()); - if (!(pc.z() > 1e-6) || !std::isfinite(pc.x()) || !std::isfinite(pc.y()) || - !std::isfinite(pc.z())) - { - return false; - } + return false; + } - std::vector object_points{cv::Point3d(pc.x(), pc.y(), pc.z())}; - cv::Mat rvec = cv::Mat::zeros(1, 3, CV_64F); - cv::Mat tvec = cv::Mat::zeros(1, 3, CV_64F); - std::vector image_points; - cv::projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs, - image_points); - uv = image_points[0]; - return uv.x >= 0.0 && uv.x < canvas.cols && uv.y >= 0.0 && uv.y < canvas.rows; - }; + std::vector object_points{cv::Point3d(pc.x(), pc.y(), pc.z())}; + cv::Mat rvec = cv::Mat::zeros(1, 3, CV_64F); + cv::Mat tvec = cv::Mat::zeros(1, 3, CV_64F); + std::vector image_points; + cv::projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs, + image_points); + uv = image_points[0]; + return uv.x >= 0.0 && uv.x < canvas.cols && uv.y >= 0.0 && uv.y < canvas.rows; + } + void DrawTracker(cv::Mat& canvas, const EkfPointsMessage& ekf, + const DetectorMessage* detector, + const CandidateDebugMessage* candidate) + { cv::Point2d center_uv; - const bool center_visible = ekf.valid[0] && project(ekf.center_cam, center_uv); + const bool center_visible = + ekf.valid[0] && ProjectPoint(canvas, ekf.center_cam, center_uv); if (center_visible) { cv::circle(canvas, center_uv, 5, cv::Scalar(0, 255, 0), 2, cv::LINE_AA); - cv::putText(canvas, "T", center_uv + cv::Point2d(6, -6), + cv::putText(canvas, "TC", center_uv + cv::Point2d(6, -6), cv::FONT_HERSHEY_SIMPLEX, 0.55, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); } + DrawMatchedEkfFaces(canvas, ekf, detector, candidate); + if (!runtime_.overlay.model_faces) + { + return; + } + for (int i = 0; i < std::min(ekf.count, 4); ++i) { cv::Point2d armor_uv; - if (!ekf.valid[i + 1] || !project(ekf.armors_cam[i], armor_uv)) + if (!ekf.valid[i + 1] || !ProjectPoint(canvas, ekf.armors_cam[i], armor_uv)) { continue; } - cv::circle(canvas, armor_uv, 4, cv::Scalar(255, 255, 0), 2, cv::LINE_AA); + cv::circle(canvas, armor_uv, 3, cv::Scalar(180, 180, 180), 1, cv::LINE_AA); + std::ostringstream label; + label << "model" << i; + cv::putText(canvas, label.str(), armor_uv + cv::Point2d(7, -7), + cv::FONT_HERSHEY_SIMPLEX, 0.45, cv::Scalar(180, 180, 180), 1, + cv::LINE_AA); if (center_visible) { - cv::line(canvas, center_uv, armor_uv, cv::Scalar(80, 180, 255), 1, + cv::line(canvas, center_uv, armor_uv, cv::Scalar(120, 120, 120), 1, cv::LINE_AA); } } } + void DrawMatchedEkfFaces(cv::Mat& canvas, const EkfPointsMessage& ekf, + const DetectorMessage* detector, + const CandidateDebugMessage* candidate) + { + if (detector == nullptr || candidate == nullptr || candidate->matched == 0U) + { + return; + } + + const double sx = static_cast(canvas.cols) / + static_cast(std::max(camera_info.width, 1)); + const double sy = static_cast(canvas.rows) / + static_cast(std::max(camera_info.height, 1)); + const uint8_t count = std::min(candidate->count, + CandidateDebugMessage::kMaxItems); + for (uint8_t i = 0; i < count; ++i) + { + const auto& item = candidate->items[i]; + if (item.armor_index >= detector->results.size()) + { + continue; + } + const int face_index = static_cast(item.face_index); + if (face_index < 0 || face_index >= 4 || face_index >= ekf.count || + !ekf.valid[face_index + 1]) + { + continue; + } + + cv::Point2d ef_uv; + if (!ProjectPoint(canvas, ekf.armors_cam[face_index], ef_uv)) + { + continue; + } + + const auto& armor = detector->results[item.armor_index]; + const cv::Point2d det_uv(armor.center.x * sx, armor.center.y * sy); + const double err_px = std::hypot(ef_uv.x - det_uv.x, ef_uv.y - det_uv.y); + cv::circle(canvas, ef_uv, 6, cv::Scalar(255, 255, 0), 2, cv::LINE_AA); + cv::line(canvas, det_uv, ef_uv, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); + + std::ostringstream label; + label << "EF f=" << face_index << " e=" << std::fixed + << std::setprecision(0) << err_px; + cv::putText(canvas, label.str(), ef_uv + cv::Point2d(8, -8), + cv::FONT_HERSHEY_SIMPLEX, 0.45, cv::Scalar(255, 255, 0), 1, + cv::LINE_AA); + } + } + cv::Mat ScaledCameraMatrix(const cv::Mat& canvas) const { const auto& k = camera_info.camera_matrix; @@ -860,8 +983,9 @@ class VisionPreview : public LibXR::Application target_file_.open(output_dir_ + "/target.tsv", std::ios::out); ekf_file_.open(output_dir_ + "/ekf_points.tsv", std::ios::out); candidate_file_.open(output_dir_ + "/candidate_debug.tsv", std::ios::out); + candidate_items_file_.open(output_dir_ + "/candidate_items.tsv", std::ios::out); if (!detector_file_ || !metrics_file_ || !target_file_ || !ekf_file_ || - !candidate_file_) + !candidate_file_ || !candidate_items_file_) { XR_LOG_ERROR("VisionPreview failed to open record files under: %s", output_dir_.c_str()); @@ -878,6 +1002,11 @@ class VisionPreview : public LibXR::Application ekf_file_ << "image_timestamp_us\tpoint_index\tvalid\tx\ty\tz\n"; candidate_file_ << "image_timestamp_us\tcount\tselected_index\tmatched" << "\tdetection_count\ttracked_armors_num\n"; + candidate_items_file_ << "image_timestamp_us\titem_index\tarmor_index" + << "\tface_index\tsame_number\timage_track_id" + << "\timage_track_confirmed\tnumber\ttype\tscore" + << "\tposition_diff\tyaw_diff\tcenter_x\tcenter_y" + << "\tpredicted_yaw\tmeasured_yaw\n"; record_ready_ = true; FlushRecordFiles(); } @@ -899,6 +1028,23 @@ class VisionPreview : public LibXR::Application raw_writer_.write(bgr); } + void WriteOverlayVideo(const cv::Mat& bgr) + { + if (!overlay_writer_ready_) + { + const std::string path = output_dir_ + "/" + overlay_video_name_; + const int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G'); + overlay_writer_ready_ = overlay_writer_.open( + path, fourcc, runtime_.record_fps, cv::Size(bgr.cols, bgr.rows), true); + if (!overlay_writer_ready_) + { + XR_LOG_ERROR("VisionPreview failed to open overlay video: %s", path.c_str()); + return; + } + } + overlay_writer_.write(bgr); + } + void WriteRecords(const std::vector& detector_records, const std::vector& metrics_records, const std::vector& target_records, @@ -972,6 +1118,26 @@ class VisionPreview : public LibXR::Application << static_cast(candidate.matched) << '\t' << static_cast(candidate.detection_count) << '\t' << static_cast(candidate.tracked_armors_num) << '\n'; + const uint8_t count = std::min(candidate.count, + CandidateDebugMessage::kMaxItems); + for (uint8_t i = 0; i < count; ++i) + { + const auto& item = candidate.items[i]; + candidate_items_file_ << candidate.image_timestamp_us << '\t' + << static_cast(i) << '\t' + << static_cast(item.armor_index) << '\t' + << static_cast(item.face_index) << '\t' + << static_cast(item.same_number) << '\t' + << static_cast(item.image_track_id) << '\t' + << static_cast(item.image_track_confirmed) + << '\t' << static_cast(item.number) << '\t' + << static_cast(item.type) << '\t' + << item.score << '\t' << item.position_diff + << '\t' << item.yaw_diff << '\t' << item.center_x + << '\t' << item.center_y << '\t' + << item.predicted_yaw << '\t' + << item.measured_yaw << '\n'; + } } FlushRecordFiles(); @@ -984,6 +1150,7 @@ class VisionPreview : public LibXR::Application target_file_.flush(); ekf_file_.flush(); candidate_file_.flush(); + candidate_items_file_.flush(); } bool RecordEnabled() const @@ -995,6 +1162,7 @@ class VisionPreview : public LibXR::Application std::string image_topic_name_; std::string output_dir_; std::string raw_video_name_; + std::string overlay_video_name_; std::string preview_window_name_; std::atomic running_{false}; @@ -1027,7 +1195,10 @@ class VisionPreview : public LibXR::Application std::ofstream target_file_{}; std::ofstream ekf_file_{}; std::ofstream candidate_file_{}; + std::ofstream candidate_items_file_{}; cv::VideoWriter raw_writer_{}; + cv::VideoWriter overlay_writer_{}; bool raw_writer_ready_{false}; + bool overlay_writer_ready_{false}; bool record_ready_{false}; };