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

reproject force roll and pitch fix line

Browse Source
This commit is contained in:
cyy_mac
2026-03-26 04:28:25 +08:00
parent 671978125f
commit 8cfffe8f32
2 changed files with 17 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -98,20 +98,13 @@ static inline std::vector<cv::Point2f> reprojectArmorToImage(
auto corners_armor = getArmorCornerPoints(is_big); auto corners_armor = getArmorCornerPoints(is_big);
// Transform corners from armor frame to camera frame // Convert rotation matrix to Rodrigues vector
std::vector<cv::Point3f> corners_cam; cv::Mat rvec;
for (const auto& p_armor : corners_armor) { cv::Rodrigues(rot_mat, rvec);
cv::Point3d p_cam;
p_cam.x = rot_mat(0, 0) * p_armor.x + rot_mat(0, 1) * p_armor.y + rot_mat(0, 2) * p_armor.z + tvec[0];
p_cam.y = rot_mat(1, 0) * p_armor.x + rot_mat(1, 1) * p_armor.y + rot_mat(1, 2) * p_armor.z + tvec[1];
p_cam.z = rot_mat(2, 0) * p_armor.x + rot_mat(2, 1) * p_armor.y + rot_mat(2, 2) * p_armor.z + tvec[2];
corners_cam.push_back(p_cam);
}
// Project to image plane // Project to image plane (cv::projectPoints handles both rotation and translation)
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F);
std::vector<cv::Point2f> corners_2d; std::vector<cv::Point2f> corners_2d;
cv::projectPoints(corners_cam, rvec, tvec, K, dist_coeffs, corners_2d); cv::projectPoints(corners_armor, rvec, tvec, K, dist_coeffs, corners_2d);
return corners_2d; return corners_2d;
} }

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

@@ -103,24 +103,26 @@ Eigen::Vector3d ArmorPoseEstimator::rotationMatrixToRPY(const Eigen::Matrix3d &R
return rpy; return rpy;
} }
// Force rotation to have roll=0 and pitch=15 degrees, keeping original yaw // Force rotation to have roll=0 and pitch=forced_degrees, keeping original yaw
// Uses quaternion to avoid gimbal lock issues with Euler angles
Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg) { Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg) {
// Extract current RPY // Extract current RPY using tf2 (standard ZYX convention)
Eigen::Quaterniond q(R); Eigen::Quaterniond q(R);
tf2::Quaternion tf_q(q.x(), q.y(), q.z(), q.w()); tf2::Quaternion tf_q(q.x(), q.y(), q.z(), q.w());
Eigen::Vector3d rpy; double r, p, y;
tf2::Matrix3x3(tf_q).getRPY(rpy[0], rpy[1], rpy[2]); tf2::Matrix3x3(tf_q).getRPY(r, p, y);
// Keep original yaw, force roll and pitch // Keep original yaw, force roll=0 and pitch=forced_pitch_deg
double yaw = rpy[2];
double roll = forced_roll_deg * M_PI / 180.0; double roll = forced_roll_deg * M_PI / 180.0;
double pitch = forced_pitch_deg * M_PI / 180.0; double pitch = forced_pitch_deg * M_PI / 180.0;
// Reconstruct rotation matrix from RPY (ZYX order: yaw, pitch, roll) // Reconstruct using quaternion to avoid gimbal lock
tf2::Quaternion q_new;
q_new.setRPY(roll, pitch, y);
Eigen::Matrix3d R_new; Eigen::Matrix3d R_new;
R_new = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) R_new = Eigen::Matrix3d(q_new);
* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
return R_new; return R_new;
} }