From 2813629a40686635b185d6379a991cd7f6cbe0ac Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sun, 22 Mar 2026 12:32:03 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A0=E9=99=A4=E6=8A=95=E5=BD=B1=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../show_images/ballistic_predicition.h | 36 ------------- armor/include/show_images/show_images.h | 5 -- armor/src/armor_finder/anti_top/anti_top.cpp | 3 +- .../src/show_images/ballistic_predicition.cpp | 51 ------------------- armor/src/show_images/show_images.cpp | 25 --------- 5 files changed, 1 insertion(+), 119 deletions(-) diff --git a/armor/include/show_images/ballistic_predicition.h b/armor/include/show_images/ballistic_predicition.h index 562d9a6..f4cad27 100644 --- a/armor/include/show_images/ballistic_predicition.h +++ b/armor/include/show_images/ballistic_predicition.h @@ -56,42 +56,6 @@ public: * @return 弹着点相对枪口的垂直高度 (m，高于枪口为正) */ static double get_impact_y(double d, double pitch, double v0, double k = 0.1); - - /** - * [TEST] 用于验证弹道算法正确性：将弹着点反解算到相机坐标系， - * 再通过 project_to_pixel 投影到像素并用红点显示。 - * 如果实验结果正确，可直接注释调用处，不影响其他结构。 - * - * 坐标变换链： - * xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g) - * - * @param xyz_in_world 目标在世界坐标系中的位置 (m，来自 PnP Solver) - * @param pitch_imu 当前 IMU pitch 角 (度，仰为正) - * @param v0 枪口初速度 (m/s) - * @param R_camera2gimbal 相机到云台的旋转矩阵 - * @param R_gimbal2world 云台到世界的旋转矩阵 - * @param t_camera2gimbal 相机原点在云台系中的平移向量 (m) - * @param k 空气阻力系数 - * @return ImpactPoint { 弹着点相机坐标(m), 飞行时间(s) } - */ - static ImpactPoint get_impact_in_camera( - const Eigen::Vector3d& xyz_in_world, - double pitch_imu, - double v0, - const Eigen::Matrix3d& R_camera2gimbal, - const Eigen::Matrix3d& R_gimbal2world, - const Eigen::Vector3d& t_camera2gimbal, - double k = 0.1); - - /** - * [TEST] 将相机坐标系3D点简化投影到像素坐标（针孔模型，无畲变）。 - * @param xyz_in_camera 弹着点在相机坐标系中的位置 (m) - * @param camera_matrix OpenCV 相机内参矩阵 (3x3 CV_64F) - * @return 像素坐标 - */ - static cv::Point2f project_to_pixel( - const Eigen::Vector3d& xyz_in_camera, - const cv::Mat& camera_matrix); }; #endif // BALLISTIC_PREDICITION_H diff --git a/armor/include/show_images/show_images.h b/armor/include/show_images/show_images.h index 308309c..37e8ab1 100644 --- a/armor/include/show_images/show_images.h +++ b/armor/include/show_images/show_images.h @@ -15,9 +15,4 @@ void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlo void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes); void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos); -// [TEST] 在图像上标注弹着点（红色小圆点），用于验证弹道算法正确性。 -// 如果实验结果正确，直接注释调用处即可，不影响其他结构。 -// impact_pixel: 由 BallisticSolver::project_to_pixel() 计算得到的像素坐标。 -void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel); - #endif /* _SHOW_IMAGES_H_ */ diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 25554ab..c5cde03 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -55,12 +55,11 @@ void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) { // 飞行时间补偿（ms） double fly_time_ms = BallisticSolver::get_flight_time( dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0; - constexpr double sys_delay_ms = SYSTEM_DELAY; // setconfig.h 默认 50 ms // 修正公式：子弹命中时刻 = 发令时刻 + shoot_delay + sys_delay + fly_time // 令子弹命中时刻 = front_time + periodms×2 int32_t delay_raw = static_cast( - front_time + periodms * 2 - curr_time - sys_delay_ms - fly_time_ms); + front_time + periodms * 2 - curr_time - SYSTEM_DELAY - fly_time_ms); // 若错过当前窗口（delay_raw < 0），顺延一个周期 uint16_t shoot_delay = (delay_raw > 0) diff --git a/armor/src/show_images/ballistic_predicition.cpp b/armor/src/show_images/ballistic_predicition.cpp index 205e2b2..766833a 100644 --- a/armor/src/show_images/ballistic_predicition.cpp +++ b/armor/src/show_images/ballistic_predicition.cpp @@ -49,54 +49,3 @@ double BallisticSolver::get_impact_y(double d, double pitch, double v0, double k double t = get_flight_time(d, pitch, v0, k); return (1.0 / k) * (v0 * sin(pitch_rad) + g / k) * (1.0 - exp(-k * t)) - (g / k) * t; } - -// ──────────────────── [TEST] 以下两个函数用于可视化验证弹道算法正确性 ──────────────────── -// 如果测试通过，直接注释掉 armor_finder.cpp 里的调用，不影响其他逻辑。 - -/** - * [TEST] 将弹着点从世界坐标系反解算到相机坐标系。 - * 变换链：xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g) - */ -ImpactPoint BallisticSolver::get_impact_in_camera( - const Eigen::Vector3d& xyz_in_world, - double pitch_imu, - double v0, - const Eigen::Matrix3d& R_camera2gimbal, - const Eigen::Matrix3d& R_gimbal2world, - const Eigen::Vector3d& t_camera2gimbal, - double k) -{ - // 水平距离：世界系 x=前, y=左 - double d = std::sqrt(xyz_in_world.x() * xyz_in_world.x() + - xyz_in_world.y() * xyz_in_world.y()); - - double impact_z = get_impact_y(d, pitch_imu, v0, k); - double fly_time = get_flight_time(d, pitch_imu, v0, k); - - // 弹着点世界坐标：水平 (x,y) 与目标相同，z 换为弹道高度 - Eigen::Vector3d impact_world(xyz_in_world.x(), xyz_in_world.y(), impact_z); - - // 世界 → 云台 → 相机 - Eigen::Vector3d impact_gimbal = R_gimbal2world.transpose() * impact_world; - Eigen::Vector3d impact_camera = R_camera2gimbal.transpose() * (impact_gimbal - t_camera2gimbal); - - return ImpactPoint{ impact_camera, fly_time }; -} - -/** - * [TEST] 针孔投影：相机坐标系 3D 点 → 像素坐标（不含畸变，仅用于可视化）。 - */ -cv::Point2f BallisticSolver::project_to_pixel( - const Eigen::Vector3d& xyz_in_camera, - const cv::Mat& camera_matrix) -{ - if (xyz_in_camera.z() <= 0) return cv::Point2f(-1, -1); // 在相机背后，无效 - double fx = camera_matrix.at(0, 0); - double fy = camera_matrix.at(1, 1); - double cx = camera_matrix.at(0, 2); - double cy = camera_matrix.at(1, 2); - float u = static_cast(fx * xyz_in_camera.x() / xyz_in_camera.z() + cx); - float v = static_cast(fy * xyz_in_camera.y() / xyz_in_camera.z() + cy); - return cv::Point2f(u, v); -} -// ──────────────────────────────── [TEST END] ───────────────────────────────────────────── diff --git a/armor/src/show_images/show_images.cpp b/armor/src/show_images/show_images.cpp index 7b2f288..ce7851e 100644 --- a/armor/src/show_images/show_images.cpp +++ b/armor/src/show_images/show_images.cpp @@ -155,28 +155,3 @@ void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const c rectangle(image2show, pos, Scalar(0, 255, 0), 1); imshow(window_names, image2show); } - -// ─────────────── [TEST] 弹着点可视化 ─────────────────────────────────────────────────────── -// 在图像上画一个红色小圆点标注弹道落点位置，用于验证弹道算法正确性。 -// 验证通过后直接注释调用处即可，函数本身不影响任何发包逻辑。 -void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel) { - static Mat image2show; - if (src.type() == CV_8UC1) { - cvtColor(src, image2show, COLOR_GRAY2RGB); - } else if (src.type() == CV_8UC3) { - image2show = src.clone(); - } else { - return; - } - - // 越界检查 - int u = static_cast(impact_pixel.x); - int v = static_cast(impact_pixel.y); - if (u >= 0 && u < image2show.cols && v >= 0 && v < image2show.rows) { - // 红色实心圆，半径 3px - cv::circle(image2show, cv::Point(u, v), 3, Scalar(0, 0, 255), -1); - } - - imshow(window_name, image2show); -} -// ─────────────── [TEST END] ──────────────────────────────────────────────────────────────── \ No newline at end of file