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);
// Transform corners from armor frame to camera frame
std::vector<cv::Point3f> corners_cam;
for (const auto& p_armor : corners_armor) {
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);
}
// Convert rotation matrix to Rodrigues vector
cv::Mat rvec;
cv::Rodrigues(rot_mat, rvec);
// Project to image plane
cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F);
// Project to image plane (cv::projectPoints handles both rotation and translation)
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;
}

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;
}
// 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) {
// Extract current RPY
// Extract current RPY using tf2 (standard ZYX convention)
Eigen::Quaterniond q(R);
tf2::Quaternion tf_q(q.x(), q.y(), q.z(), q.w());
Eigen::Vector3d rpy;
tf2::Matrix3x3(tf_q).getRPY(rpy[0], rpy[1], rpy[2]);
double r, p, y;
tf2::Matrix3x3(tf_q).getRPY(r, p, y);
// Keep original yaw, force roll and pitch
double yaw = rpy[2];
// Keep original yaw, force roll=0 and pitch=forced_pitch_deg
double roll = forced_roll_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;
R_new = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
R_new = Eigen::Matrix3d(q_new);
return R_new;
}