VSAG/vision_hero
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

删除投影功能

Browse Source
This commit is contained in:
Luo Yufei
2026-03-22 12:32:03 +08:00
parent d20bdd3673
commit 2813629a40
5 changed files with 1 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
armor/include/show_images/ballistic_predicition.h
View File

@@ -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

5
armor/include/show_images/show_images.h
View File

@@ -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_ */

3
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -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<int32_t>(
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)

51
armor/src/show_images/ballistic_predicition.cpp
View File

@@ -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<double>(0, 0);
double fy = camera_matrix.at<double>(1, 1);
double cx = camera_matrix.at<double>(0, 2);
double cy = camera_matrix.at<double>(1, 2);
float u = static_cast<float>(fx * xyz_in_camera.x() / xyz_in_camera.z() + cx);
float v = static_cast<float>(fy * xyz_in_camera.y() / xyz_in_camera.z() + cy);
return cv::Point2f(u, v);
}
// ──────────────────────────────── [TEST END] ─────────────────────────────────────────────

25
armor/src/show_images/show_images.cpp
View File

@@ -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<int>(impact_pixel.x);
int v = static_cast<int>(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] ────────────────────────────────────────────────────────────────