enhancement

test/src/QRcode.cpp

@@ -29,6 +29,26 @@ void processFrame(const cv::Mat& frame)
         {
             ROS_INFO("Marker ID: %d", markerIds[i]);
             cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
+
+            // Print the rotation and translation vectors
+            ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
+            ROS_INFO_STREAM("Translation Vector: " << tvecs[i]);
+
+            // Calculate the distance from the camera to the marker
+            double distance = cv::norm(tvecs[i]);
+            ROS_INFO("Distance from camera: %f meters", distance);
+
+            // Convert rotation vector to rotation matrix
+            cv::Mat rotationMatrix;
+            cv::Rodrigues(rvecs[i], rotationMatrix);
+
+            // Extract Euler angles from the rotation matrix
+            cv::Vec3d eulerAngles;
+            eulerAngles[0] = atan2(rotationMatrix.at<double>(2, 1), rotationMatrix.at<double>(2, 2)); // Roll
+            eulerAngles[1] = atan2(-rotationMatrix.at<double>(2, 0), sqrt(pow(rotationMatrix.at<double>(2, 1), 2) + pow(rotationMatrix.at<double>(2, 2), 2))); // Pitch
+            eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
+
+            ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
         }
     }
     else