diff --git a/app/auto_aim_async/src/rbt_threads.rs b/app/auto_aim_async/src/rbt_threads.rs index a51b215..1d05602 100644 --- a/app/auto_aim_async/src/rbt_threads.rs +++ b/app/auto_aim_async/src/rbt_threads.rs @@ -564,8 +564,10 @@ fn run_video_preprocess_loop( frame_id = frame_id.wrapping_add(1); let mut rbt_frame = RbtFrame::new(); let preprocess_started = StdInstant::now(); + let gray_frame = frame_img.to_luma8(); let transform = preprocess_letterbox_f16(rbt_frame.pre_data(), &frame_img); summary.preprocess_total += preprocess_started.elapsed(); + rbt_frame.set_gray_frame(gray_frame); rbt_frame.set_letterbox_transform(transform); rbt_frame.set_id(frame_id); rbt_frame.set_state(RbtFrameStage::Pre); @@ -912,7 +914,7 @@ pub fn post_process( ); let decoded_armor_count = armors.values().map(Vec::len).sum::(); let decoded_enemy_count = armors.len(); - let solved_enemies = enemys_solver(armors, &cam_k, &rec)?; + let solved_enemies = enemys_solver(armors, &cam_k, frame.gray_frame(), &rec)?; let solved_armor_count = solved_enemies .values() .filter_map(|result| result.as_ref()) @@ -1694,5 +1696,9 @@ mod tests { assert_eq!(transform.image_width, frame.width()); assert_eq!(transform.image_height, frame.height()); assert!(transform.scale > 0.0); + + let gray = frame.to_luma8(); + assert_eq!(gray.width(), frame.width()); + assert_eq!(gray.height(), frame.height()); } } diff --git a/app/single_frame_dev/src/main.rs b/app/single_frame_dev/src/main.rs index c0b1e57..8a79a0a 100644 --- a/app/single_frame_dev/src/main.rs +++ b/app/single_frame_dev/src/main.rs @@ -51,7 +51,7 @@ async fn main() -> RbtResult<()> { // 获取相机内参 let cam_k = auto_aim_handle.cfg.cam_cfg.cam_k(); // 解算检测到的所有装甲板，得到所有地方单位的解算结果 - let enemys = enemys_solver(detector_result, &cam_k, &auto_aim_handle.rec)?; + let enemys = enemys_solver(detector_result, &cam_k, None, &auto_aim_handle.rec)?; // 3. 执行 estimator estimator_poll.update(&auto_aim_handle.cfg.estimator_cfg, enemys); diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index 73a933a..d37e13f 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/docs/vivsionn-gap-checklist.md @@ -14,7 +14,7 @@ This checklist tracks the functional gaps found while comparing this Rust worksp | [x] P0 能量机关 tracker/aimer | 相位 EKF、大小符曲线模型、相位化预瞄、pitch lead | 大符曲线 EKF（基于共享不定长 EKF）+ 两轮飞行时间迭代 + yaw preview horizon + pitch lead + 配置化偏置 | 大符走 `BigBuffCurveEskf` 曲线预测（`speed=a·sin(phase)+base-a`），小符保留常速；aimer 两轮弹道迭代、yaw MPC horizon 由 tracker 预瞄生成 | | [ ] P1 主线热更新调参 | `param.yaml` 每秒 reload，曝光/发控/MPC 可调 | 只有实验入口，主线没接 watcher | 上车调参效率会差。本轮不做热更新，配置仅启动加载 | | [x] P1 配置面补齐 | 大量曝光、门控、MPC、buff 参数 | `rbt_cfg.toml` 主要是 detector/cam/estimator | 新增顶层 `energy_mechanism_cfg`（tracker/aimer/mpc），补齐大符曲线 EKF 全部 knob，serde 默认值保证旧配置兼容 | -| [ ] P1 PnP 稳态保护 | 角点细化、位姿 sanity gate | 直接网络角点 + IPPE | 建议补，降低跳点。本轮延期 | +| [x] P1 PnP 稳态保护 | 角点细化、位姿 sanity gate | 主线解码后随 `RbtFrame` 流一份原尺寸灰度帧到 solver；Rust IPPE 输入前做灰度灯条端点细化 + 几何规整，输出后做深度/有限值/重投影 RMSE sanity gate | 已补齐 Rust IPPE 路径的稳态保护和灰度帧上下文；未引入 OpenCV/C++ PnP 旁路 | | [ ] P1 离线录制/回放 | 可录 `.avi + .csv`，强制 task mode 回放 | 缺主链路复盘工具 | 调现场问题很关键 | | [ ] P1 通信/MPC smoke 工具 | `testSerial`、`can_mpc_yaw_test` 工具链完整 | `comm_test` 基本空 | 接 CAN 后应尽快补 | | [ ] P2 显示/HUD/录制旁路 | MJPEG/Rerun/HUD/CSV/plot 脚本多 | 有 Rerun 和日志，但观测面较薄 | 影响调试效率 | diff --git a/lib/src/rbt_base/rbt_algorithm/rbt_ippe.rs b/lib/src/rbt_base/rbt_algorithm/rbt_ippe.rs index 06cd7fa..2c6ff31 100644 --- a/lib/src/rbt_base/rbt_algorithm/rbt_ippe.rs +++ b/lib/src/rbt_base/rbt_algorithm/rbt_ippe.rs @@ -1,10 +1,15 @@ /// 针对装甲板场景高度特化的 PnpSolver /// 使用 IPPE 方法，四个特征点 +use image::GrayImage; use log::error; // 硬编码的世界坐标，满足 IPPE 的规范坐标系要求 (Z=0, 中心在原点) pub const ARMOR_LIGHT_WEIGHT: f64 = 135.0; pub const ARMOR_LIGHT_HEIGHT: f64 = 55.0; +const MAX_REPROJECTION_RMSE_PX: f64 = 8.0; +const MIN_ARMOR_DEPTH_MM: f64 = 100.0; +const MAX_ARMOR_DEPTH_MM: f64 = 12_000.0; +const BRIGHT_PIXEL_THRESHOLD: u8 = 150; // 世界坐标系点，原点在装甲板中心，Z=0平面，省去归一化 const ARMOR_WORLD_POINTS: [na::Point3; 4] = [ @@ -64,29 +69,33 @@ impl ArmorPnpSolver { img_coord: &[na::Point2; 4], cam_k: &na::Matrix3, ) -> Option> { + self.solve_with_gray(img_coord, cam_k, None) + } + + pub fn solve_with_gray( + &self, + img_coord: &[na::Point2; 4], + cam_k: &na::Matrix3, + gray_img: Option<&GrayImage>, + ) -> Option> { + let refined_img_coord = refine_armor_corners(img_coord, gray_img)?; // 使用 IPPE 算法求解出两个可能解 - if let Some((pose1, pose2)) = self.solve_ippe(img_coord, cam_k) { + if let Some((pose1, pose2)) = self.solve_ippe(&refined_img_coord, cam_k) { // 简单判断解的合理性 - let pose1_valid = if self.is_pose_valid(&pose1) { - Some(pose1) - } else { - None - }; - let pose2_valid = if self.is_pose_valid(&pose2) { - Some(pose2) - } else { - None - }; + let pose1_valid = self.valid_pose_with_reprojection(&pose1, &refined_img_coord, cam_k); + let pose2_valid = self.valid_pose_with_reprojection(&pose2, &refined_img_coord, cam_k); // 根据解的合理性情况返回最终解 match (pose1_valid, pose2_valid) { - (Some(p1), Some(p2)) => { - let err1 = self.eval_reproj_err(&p1, img_coord, cam_k); - let err2 = self.eval_reproj_err(&p2, img_coord, cam_k); - if err1 < err2 { Some(p1) } else { Some(p2) } + (Some((p1, err1)), Some((p2, err2))) => { + if err1 < err2 { + Some(p1) + } else { + Some(p2) + } } - (Some(p1), None) => Some(p1), - (None, Some(p2)) => Some(p2), + (Some((p1, _)), None) => Some(p1), + (None, Some((p2, _))) => Some(p2), (None, None) => None, } } else { @@ -286,11 +295,36 @@ impl ArmorPnpSolver { ata.try_inverse().map(|inv| inv * atb) } - /// 检查姿态是否有效 - /// 逻辑较为简单，检查点是否在相机前方 + fn valid_pose_with_reprojection( + &self, + pose: &na::Isometry3, + uvs: &[na::Point2; 4], + k: &na::Matrix3, + ) -> Option<(na::Isometry3, f64)> { + if !self.is_pose_valid(pose) { + return None; + } + + let reproj_err = self.eval_reproj_err(pose, uvs, k); + if reproj_err.is_finite() && reproj_err <= MAX_REPROJECTION_RMSE_PX { + Some((*pose, reproj_err)) + } else { + None + } + } + fn is_pose_valid(&self, pose: &na::Isometry3) -> bool { // 首先检查平移向量的 Z 分量，这是一个快速的初步筛选 - if pose.translation.vector.z <= 0.0 { + let t = pose.translation.vector; + if !t.iter().all(|value| value.is_finite()) + || t.z <= MIN_ARMOR_DEPTH_MM + || t.z >= MAX_ARMOR_DEPTH_MM + { + return false; + } + + let rot = pose.rotation.to_rotation_matrix(); + if !rot.matrix().iter().all(|value| value.is_finite()) { return false; } // 确保所有点变换后都在相机前方 @@ -335,6 +369,194 @@ impl ArmorPnpSolver { } } +fn refine_armor_corners( + corners: &[na::Point2; 4], + gray_img: Option<&GrayImage>, +) -> Option<[na::Point2; 4]> { + if corners + .iter() + .any(|point| !point.coords.iter().all(|value| value.is_finite())) + { + return None; + } + + let mut refined = if let Some(gray_img) = gray_img { + refine_lightbar_endpoints_from_gray(corners, gray_img).unwrap_or(*corners) + } else { + *corners + }; + + let left = refined[1] - refined[0]; + let right = refined[2] - refined[3]; + let len_left = left.norm(); + let len_right = right.norm(); + if len_left <= 1e-3 || len_right <= 1e-3 { + return None; + } + + let width_top = (refined[3] - refined[0]).norm(); + let width_bottom = (refined[2] - refined[1]).norm(); + let avg_height = 0.5 * (len_left + len_right); + let avg_width = 0.5 * (width_top + width_bottom); + if avg_width <= 1e-3 || avg_height <= 1e-3 { + return None; + } + + let aspect = avg_width / avg_height; + if !(0.15..=8.0).contains(&aspect) { + return None; + } + + let dir_left = left / len_left; + let dir_right = right / len_right; + let dot_prod = dir_left.dot(&dir_right); + let len_diff = (len_left - len_right).abs() / len_left.max(len_right); + + if dot_prod > 0.96 && len_diff < 0.20 { + let mut avg_dir = dir_left + dir_right; + let avg_dir_norm = avg_dir.norm(); + if avg_dir_norm > 1e-6 { + avg_dir /= avg_dir_norm; + let target_len = 0.5 * (len_left + len_right); + let mid_left = na::Point2::from((refined[0].coords + refined[1].coords) * 0.5); + let mid_right = na::Point2::from((refined[3].coords + refined[2].coords) * 0.5); + let target_left_top = mid_left - avg_dir * (target_len * 0.5); + let target_left_bottom = mid_left + avg_dir * (target_len * 0.5); + let target_right_bottom = mid_right + avg_dir * (target_len * 0.5); + let target_right_top = mid_right - avg_dir * (target_len * 0.5); + let alpha = 0.2; + refined[0] = lerp_point(refined[0], target_left_top, alpha); + refined[1] = lerp_point(refined[1], target_left_bottom, alpha); + refined[2] = lerp_point(refined[2], target_right_bottom, alpha); + refined[3] = lerp_point(refined[3], target_right_top, alpha); + } + } + + Some(refined) +} + +fn refine_lightbar_endpoints_from_gray( + corners: &[na::Point2; 4], + gray_img: &GrayImage, +) -> Option<[na::Point2; 4]> { + if gray_img.width() == 0 || gray_img.height() == 0 { + return None; + } + + let mut refined = *corners; + refine_one_lightbar(&mut refined, gray_img, 0, 1); + refine_one_lightbar(&mut refined, gray_img, 3, 2); + Some(refined) +} + +fn refine_one_lightbar( + corners: &mut [na::Point2; 4], + gray_img: &GrayImage, + bottom_idx: usize, + top_idx: usize, +) { + let bottom = corners[bottom_idx]; + let top = corners[top_idx]; + let segment = top - bottom; + let length = segment.norm(); + if length <= 1e-3 { + return; + } + + let width = length * 0.5; + let x_min = ((bottom.x.min(top.x) - width).floor().max(0.0)) as u32; + let x_max = ((bottom.x.max(top.x) + width) + .ceil() + .min((gray_img.width().saturating_sub(1)) as f64)) as u32; + let y_min = ((bottom.y.min(top.y) - length * 0.5).floor().max(0.0)) as u32; + let y_max = ((bottom.y.max(top.y) + length * 0.5) + .ceil() + .min((gray_img.height().saturating_sub(1)) as f64)) as u32; + if x_min > x_max || y_min > y_max { + return; + } + + let mut bright_points = Vec::new(); + for y in y_min..=y_max { + for x in x_min..=x_max { + if gray_img.get_pixel(x, y)[0] > BRIGHT_PIXEL_THRESHOLD { + bright_points.push(na::Point2::new(x as f64, y as f64)); + } + } + } + if bright_points.len() <= 10 { + return; + } + + let Some(axis) = principal_axis(&bright_points) else { + return; + }; + let mut axis = axis; + if axis.dot(&segment) < 0.0 { + axis = -axis; + } + + let center = na::Point2::from((bottom.coords + top.coords) * 0.5); + let mut projections = bright_points + .iter() + .map(|point| (point - center).dot(&axis)) + .filter(|projection| projection.is_finite()) + .collect::>(); + if projections.len() <= 10 { + return; + } + projections.sort_by(|lhs, rhs| lhs.total_cmp(rhs)); + let low_idx = ((projections.len() - 1) as f64 * 0.05).round() as usize; + let high_idx = ((projections.len() - 1) as f64 * 0.95).round() as usize; + let refined_bottom = center + axis * projections[low_idx]; + let refined_top = center + axis * projections[high_idx]; + + if (refined_bottom - bottom).norm() < 8.0 && (refined_top - top).norm() < 8.0 { + corners[bottom_idx] = refined_bottom; + corners[top_idx] = refined_top; + } +} + +fn principal_axis(points: &[na::Point2]) -> Option> { + if points.is_empty() { + return None; + } + let n = points.len() as f64; + let mean = points + .iter() + .fold(na::Vector2::::zeros(), |acc, point| acc + point.coords) + / n; + let mut cov_xx = 0.0; + let mut cov_xy = 0.0; + let mut cov_yy = 0.0; + for point in points { + let centered = point.coords - mean; + cov_xx += centered.x * centered.x; + cov_xy += centered.x * centered.y; + cov_yy += centered.y * centered.y; + } + cov_xx /= n; + cov_xy /= n; + cov_yy /= n; + + let trace = cov_xx + cov_yy; + let det = cov_xx * cov_yy - cov_xy * cov_xy; + let discriminant = (trace * trace * 0.25 - det).max(0.0).sqrt(); + let lambda = trace * 0.5 + discriminant; + let axis = if cov_xy.abs() > 1e-9 { + na::Vector2::new(lambda - cov_yy, cov_xy) + } else if cov_xx >= cov_yy { + na::Vector2::x() + } else { + na::Vector2::y() + }; + axis.try_normalize(1e-9) +} + +fn lerp_point(from: na::Point2, to: na::Point2, alpha: f64) -> na::Point2 { + na::Point2::from(from.coords * (1.0 - alpha) + to.coords * alpha) +} + fn rotate_vec_to_z_axis(a: &na::Vector3) -> na::Matrix3 { match nalgebra::Rotation3::rotation_between(&a.normalize(), &na::Vector3::z_axis()) { Some(rot) => rot.matrix().into_owned(), @@ -410,3 +632,113 @@ fn isotropic_normalize( ); Some((centered_points, transformation_matrix)) } + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_close(actual: f64, expected: f64, tolerance: f64) { + assert!( + (actual - expected).abs() <= tolerance, + "actual={actual}, expected={expected}, tolerance={tolerance}" + ); + } + + #[test] + fn refine_armor_corners_rejects_non_finite_points() { + let points = [ + na::Point2::new(10.0, 20.0), + na::Point2::new(10.0, f64::NAN), + na::Point2::new(50.0, 40.0), + na::Point2::new(50.0, 20.0), + ]; + + assert!(refine_armor_corners(&points, None).is_none()); + } + + #[test] + fn refine_armor_corners_regularizes_parallel_lightbars() { + let points = [ + na::Point2::new(10.0, 20.0), + na::Point2::new(10.0, 42.0), + na::Point2::new(50.0, 38.0), + na::Point2::new(50.0, 19.0), + ]; + + let refined = refine_armor_corners(&points, None).unwrap(); + let left_len = (refined[1] - refined[0]).norm(); + let right_len = (refined[2] - refined[3]).norm(); + + assert!((left_len - right_len).abs() < 3.0_f64); + assert_close(refined[0].x, 10.0, 1e-9); + assert_close(refined[3].x, 50.0, 1e-9); + } + + #[test] + fn gray_refinement_moves_lightbar_endpoints_toward_bright_pixels() { + let mut gray = GrayImage::new(80, 80); + for y in 16..=44 { + for x in 9..=11 { + gray.put_pixel(x, y, image::Luma([220])); + } + } + for y in 18..=46 { + for x in 49..=51 { + gray.put_pixel(x, y, image::Luma([220])); + } + } + let points = [ + na::Point2::new(10.0, 20.0), + na::Point2::new(10.0, 42.0), + na::Point2::new(50.0, 44.0), + na::Point2::new(50.0, 22.0), + ]; + + let refined = refine_armor_corners(&points, Some(&gray)).unwrap(); + + assert!(refined[0].y < points[0].y); + assert!(refined[1].y > points[1].y); + assert!(refined[3].y < points[3].y); + assert!(refined[2].y > points[2].y); + } + + #[test] + fn solve_rejects_degenerate_corner_geometry() { + let solver = ArmorPnpSolver::new().unwrap(); + let k = na::Matrix3::new(1600.0, 0.0, 320.0, 0.0, 1705.7, 192.0, 0.0, 0.0, 1.0); + let points = [ + na::Point2::new(10.0, 20.0), + na::Point2::new(10.0, 20.0), + na::Point2::new(50.0, 40.0), + na::Point2::new(50.0, 20.0), + ]; + + assert!(solver.solve(&points, &k).is_none()); + } + + #[test] + fn refine_armor_corners_keeps_high_yaw_projection() { + let points = [ + na::Point2::new(100.0, 20.0), + na::Point2::new(102.0, 70.0), + na::Point2::new(120.0, 72.0), + na::Point2::new(118.0, 22.0), + ]; + + assert!(refine_armor_corners(&points, None).is_some()); + } + + #[test] + fn solve_accepts_nominal_ippe_case() { + let solver = ArmorPnpSolver::new().unwrap(); + let k = na::Matrix3::new(1600.0, 0.0, 320.0, 0.0, 1705.7, 192.0, 0.0, 0.0, 1.0); + let points = [ + na::Point2::new(197.125, 203.125), + na::Point2::new(191.25, 231.625), + na::Point2::new(235.875, 236.375), + na::Point2::new(241.5, 207.375), + ]; + + assert!(solver.solve(&points, &k).is_some()); + } +} diff --git a/lib/src/rbt_mod/rbt_detector/rbt_frame.rs b/lib/src/rbt_mod/rbt_detector/rbt_frame.rs index 68e3af5..fc2ad2a 100644 --- a/lib/src/rbt_mod/rbt_detector/rbt_frame.rs +++ b/lib/src/rbt_mod/rbt_detector/rbt_frame.rs @@ -2,6 +2,7 @@ use tokio::time::Instant; use crate::rbt_infra::rbt_global::FAILED_COUNT; use crate::rbt_mod::rbt_detector::rbt_yolo::LetterboxTransform; +use image::GrayImage; use log::{debug, error, warn}; pub const ARMOR_INPUT_WIDTH: usize = 640; @@ -42,6 +43,7 @@ impl RbtFrame { ]), infer_post: nd::Array2::::zeros([ARMOR_OUTPUT_ROWS, ARMOR_OUTPUT_COLS]), letterbox: LetterboxTransform::default(), + gray_frame: None, }, id: 0, stage: RbtFrameStage::Init, @@ -88,6 +90,14 @@ impl RbtFrame { self.data.letterbox } + pub fn set_gray_frame(&mut self, gray_frame: GrayImage) { + self.data.gray_frame = Some(gray_frame); + } + + pub fn gray_frame(&self) -> Option<&GrayImage> { + self.data.gray_frame.as_ref() + } + pub fn time_used(&self) -> std::time::Duration { self.time.elapsed() } @@ -139,4 +149,22 @@ pub struct RbtFrameData { pre_infer: nd::Array4, infer_post: nd::Array2, letterbox: LetterboxTransform, + gray_frame: Option, +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn rbt_frame_carries_original_gray_frame() { + let mut frame = RbtFrame::new(); + let gray = GrayImage::new(32, 24); + + frame.set_gray_frame(gray); + + let stored = frame.gray_frame().expect("gray frame should be stored"); + assert_eq!(stored.width(), 32); + assert_eq!(stored.height(), 24); + } } diff --git a/lib/src/rbt_mod/rbt_solver.rs b/lib/src/rbt_mod/rbt_solver.rs index 51f7d6f..8c74a10 100644 --- a/lib/src/rbt_mod/rbt_solver.rs +++ b/lib/src/rbt_mod/rbt_solver.rs @@ -8,7 +8,8 @@ use crate::rbt_infra::rbt_err::{RbtError, RbtResult}; use crate::rbt_mod::rbt_armor::detected_armor::DetectedArmor; use crate::rbt_mod::rbt_armor::solved_armor::SolvedArmor; use crate::rbt_mod::rbt_estimator::rbt_enemy_dynamic_model::EnemyId; -use log::{debug, error, warn}; +use image::GrayImage; +use log::{debug, warn}; use std::collections::HashMap; use std::ops::{Deref, DerefMut}; @@ -76,6 +77,7 @@ pub enum DetectedEnemyArmor { pub fn enemys_solver( detector_result: HashMap>, cam_k: &na::Matrix3, + gray_frame: Option<&GrayImage>, rec: &rr::RecordingStream, ) -> RbtResult { // 0. 构建全单位解算结果，内部是一个HashMap @@ -91,19 +93,24 @@ pub fn enemys_solver( // 1.2 针对每一块装甲板求解pnp let mut enemy_solved_armors = Vec::with_capacity(detected_enemy_armors_num); + let pnp_solver = ArmorPnpSolver::new().ok_or(RbtError::StringError( + "Failed to create ArmorPnpSolver Instant".to_string(), + ))?; for armor in enemy_armors.into_iter() { let armor_key_points_na = armor.corner_points().map(|p| p.into()); - let pnp_solver = ArmorPnpSolver::new().ok_or(RbtError::StringError( - "Failed to create ArmorPnpSolver Instant".to_string(), - ))?; - if let Some(camera_pose) = pnp_solver.solve(&armor_key_points_na, cam_k) { + if let Some(camera_pose) = + pnp_solver.solve_with_gray(&armor_key_points_na, cam_k, gray_frame) + { let solved_armor = SolvedArmor::new(armor, camera_pose, 0.0, 0.0, 0.0); enemy_solved_armors.push(solved_armor); } else { - error!("❌ PnP solving failed!"); - todo!("Solve失败应该后续使用ESKF纯预测进行一次更新"); + warn!("PnP solving rejected an armor for {enemy_id:?}"); }; } + if enemy_solved_armors.is_empty() { + debug!("enemys_solver: no stable PnP result for {enemy_id:?}"); + continue; + } // 1.3 将 pnp 结果转换为机体坐标系 for solved_armor in enemy_solved_armors.iter_mut() {