cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

reproject finishi ？

Browse Source
This commit is contained in:
cyy_mac
2026-03-26 05:00:04 +08:00
parent f3d26bcd93
commit 488a4a586f
2 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/rm_auto_aim/armor_detector/include/armor_detector/armor_reproject.hpp
View File

@@ -50,12 +50,11 @@ namespace fyt::auto_aim {
static inline std::vector<cv::Point3f> getArmorCornerPoints(bool is_big) {
double w = is_big ? LARGE_ARMOR_WIDTH : SMALL_ARMOR_WIDTH;
double h = is_big ? LARGE_ARMOR_HEIGHT : SMALL_ARMOR_HEIGHT;
// Rotate 90 degrees: y' = z, z' = -y
return std::vector<cv::Point3f>{
cv::Point3f(0, -h / 2, -w / 2), // 0: bottom-left (rotated)
cv::Point3f(0, h / 2, -w / 2), // 1: top-left (rotated)
cv::Point3f(0, h / 2, w / 2), // 2: top-right (rotated)
cv::Point3f(0, -h / 2, w / 2) // 3: bottom-right (rotated)
cv::Point3f(0, w / 2, -h / 2), // 0: bottom-left
cv::Point3f(0, w / 2, h / 2), // 1: top-left
cv::Point3f(0, -w / 2, h / 2), // 2: top-right
cv::Point3f(0, -w / 2, -h / 2) // 3: bottom-right
};
}

3
src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp
View File

@@ -55,8 +55,7 @@ std::vector<rm_interfaces::msg::Armor> ArmorPoseEstimator::extractArmorPoses(
double armor_roll = rotationMatrixToRPY(R_gimbal_camera_ * R)[0] * 180 / M_PI;
// Force roll=0, pitch=15 degrees for reprojection consistency
R = forceRPY(R, 0.0, 15.0);
// No forced roll/pitch - use original PnP result
if (use_ba_ && std::abs(armor_roll) < 15) {
// Use BA alogorithm to optimize the pose from PnP