added rotation of aruco code into consideration in camera position

tello-vision.cpp

@@ -13,6 +13,8 @@
 #define MATLAB_PORT    11112 
 #define MAXLINE        1024 
 
+cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R);
+
 int main(int argc, char** argv) {
 
     cv::Mat cameraMatrix = ( cv::Mat_<double>(3,3) << 1280.1, 0.0, 531.592,
@@ -24,7 +26,7 @@ int main(int argc, char** argv) {
 
     int sockfd; 
     char buffer[MAXLINE]; 
-    char hello[] = "x:123400000000;y:5678000000;";
+    char hello[] = "x:123400000000;y:5678000000;z:123400000000;rx:123400000000;ry:5678000000;rz:123400000000;";
     struct sockaddr_in cliaddr; 
 
     // Creating socket file descriptor 
@@ -53,11 +55,15 @@ int main(int argc, char** argv) {
     cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
 
     std::vector<cv::Vec3d> rvecs, tvecs;
-    float markerLength = 24.5/1000.0;
+    cv::Vec3d eulerAngles{ 0, 0, 1 };
+    cv::Mat R  = cv::Mat::eye(3,3,CV_32F);
+    cv::Mat Ri = cv::Mat::eye(3,3,CV_32F);
+
+    float markerLength = 4.5/100.0;
 
     for(;;) {
 
-        cap >> frame; // get a first frame!
+        cap >> frame;
 
         cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
         cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
@@ -68,13 +74,19 @@ int main(int argc, char** argv) {
         int minElementIndex = std::min_element(markerIds.begin(),markerIds.end()) - markerIds.begin();
 
         if (markerIds.size()>0) {
-            int dx = static_cast<int>(markerCorners[minElementIndex][0].x) - static_cast<int>(markerCorners[minElementIndex][1].x);
-            int dy = static_cast<int>(markerCorners[minElementIndex][0].y) - static_cast<int>(markerCorners[minElementIndex][1].y);
-            int d2 = dx*dx + dy*dy;
-            int dist_cm = static_cast<int>(100.0*tvecs[minElementIndex][2]);
-            sprintf(hello, "x:%d;y:%d;\n", static_cast<int>(100.0*tvecs[minElementIndex][2]), dist_cm);
+            cv::Rodrigues( 	rvecs[minElementIndex], R ); 
+            cv::transpose(R,Ri);
+            cv::Mat_<float> p_Ti_b_Ti(-Ri*tvecs[minElementIndex]);
+            int target_x = static_cast<int>(100.0*p_Ti_b_Ti(0));
+            int target_y = static_cast<int>(100.0*p_Ti_b_Ti(1));
+            int target_z = static_cast<int>(100.0*p_Ti_b_Ti(2));
+            eulerAngles = rotationMatrixToEulerAngles(R);
+            int target_rx = static_cast<int>(180.0/3.14159*eulerAngles[0]);
+            int target_ry = static_cast<int>(180.0/3.14159*eulerAngles[1]);
+            int target_rz = static_cast<int>(180.0/3.14159*eulerAngles[2]);
+            sprintf(hello, "x:%d;y:%d;z:%d;rx:%d;ry:%d;rz:%d;\n", target_x, target_y, target_z, target_rx, target_ry, target_rz);
         } else {
-            sprintf(hello, "x:%d;y:%d;\n", 0, 50);
+            sprintf(hello, "x:%d;y:%d;z:%d;rx:%d;ry:%d;rz:%d;\n", 0, 0, 50, 0, 0, 0);
         }
 
         sendto(sockfd, (const char *)hello, strlen(hello),  
@@ -93,3 +105,29 @@ int main(int argc, char** argv) {
     return 0;
 
 }
+
+// Calculates rotation matrix to euler angles
+// The result is the same as MATLAB except the order
+// of the euler angles ( x and z are swapped ).
+cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R) {
+ 
+    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
+ 
+    bool singular = sy < 1e-6; // If
+ 
+    float x, y, z;
+    if (!singular)
+    {
+        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
+        y = atan2(-R.at<double>(2,0), sy);
+        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
+    }
+    else
+    {
+        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
+        y = atan2(-R.at<double>(2,0), sy);
+        z = 0;
+    }
+    return cv::Vec3f(x, y, z);
+ 
+}