diff --git a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp index 5b8df5f..a521b92 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp +++ b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp @@ -356,7 +356,11 @@ void ArmorDetectorNode::processFrame(PendingFrame frame) { // Draw reprojection for each armor with pose for (const auto& armor : armors_msg.armors) { - if (armor.pose.position.x > 0) { // Valid pose check + // Check depth (Z coordinate in camera frame) is positive for valid pose + double depth = std::sqrt(armor.pose.position.x * armor.pose.position.x + + armor.pose.position.y * armor.pose.position.y + + armor.pose.position.z * armor.pose.position.z); + if (depth > 0.1) { // Valid depth check (at least 10cm away) drawArmorFromMsg(debug_img, armor, K, dist_coeffs, cv::Scalar(255, 0, 255), 2); } }