send the position of the tag in the robot xyz base

aruco/ArucoDetector.cpp

@@ -1,6 +1,6 @@
 #include "ArucoDetector.h"
 
-ArucoDetector::ArucoDetector(Type::RobotPose* pose, const std::string& calibrationPath, const Team team, const bool headless) : robotPose(pose), headless(headless), team(team)
+ArucoDetector::ArucoDetector(const std::string& calibrationPath, const Team team, const bool headless) : headless(headless), team(team)
 {
     // opencv 4.8
     // this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters());
@@ -19,13 +19,6 @@ ArucoDetector::ArucoDetector(Type::RobotPose* pose, const std::string& calibrati
     parameters->perspectiveRemoveIgnoredMarginPerCell = 0.13;
     parameters->polygonalApproxAccuracyRate = 0.03;
 
-
-    this->transformationMatrix = (cv::Mat_<double>(4, 4) <<
-        cos(pose->theta), 0, sin(pose->theta), pose->position.x,
-        0, 1, 0, pose->position.y,
-        -sin(pose->theta), 0, cos(pose->theta), pose->position.z,
-        0, 0, 0, 1
-    );
     this->readCameraParameters(calibrationPath);
 
     this->cam = new lccv::PiCamera;
@@ -91,8 +84,6 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
         tags = this->arucoTags;
     }
 
-    this->updateTransformationMatrix();
-
     cv::Mat frame;
     cv::Mat frameNotRotated;
     cv::Mat frameDistored;
@@ -210,16 +201,14 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
             // Apply the homogeneous transformation to tvec
             cv::Mat translat = (cv::Mat_<double>(4, 1) << tvec.at<double>(2, 0) + 91/* TODO camera is not at the center*/ /* TODO la pince n'est pas au millieu non plus*/, tvec.at<double>(1, 0) /* + TODO HAUTEUR CAMERA */, (tvec.at<double>(0, 0)), 1);
 
-            cv::Mat transformedTvec = (transformationMatrix * translat);
-
-            result.second.emplace_back(tag, std::make_pair(transformedTvec, rotaEuler));
+            result.second.emplace_back(tag, std::make_pair(translat, rotaEuler));
         }
     }
 
-    std::sort(result.second.begin(), result.second.end(), [this](const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& a, const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& b)
+    /*std::sort(result.second.begin(), result.second.end(), [this](const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& a, const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& b)
     {
-        return distanceBetweenRobotAndTag(*robotPose, a.second.first) < distanceBetweenRobotAndTag(*robotPose, b.second.first);
-    });
+        return // TODO sort with the tvec
+    });*/
 
     if (!headless)
     {
@@ -236,16 +225,6 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
     return result;
 }
 
-void ArucoDetector::updateTransformationMatrix() {
-    this->transformationMatrix = (cv::Mat_<double>(4, 4) <<
-        cos(robotPose->theta), 0, sin(robotPose->theta), robotPose->position.x,
-        0, 1, 0, robotPose->position.y,
-        -sin(robotPose->theta), 0, cos(robotPose->theta), robotPose->position.z,
-        0, 0, 0, 1
-    );
-}
-
-
 /*void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose)
 {
     constexpr double distanceToPot = 21;

aruco/ArucoDetector.h

@@ -7,8 +7,6 @@
 class ArucoDetector {
     std::vector<ArucoTag> arucoTags;
 
-    Type::RobotPose* robotPose;
-
     cv::Mat cameraMatrix;
     cv::Mat distCoeffs;
 
@@ -26,14 +24,12 @@ class ArucoDetector {
 
     bool headless;
 
-    cv::Mat transformationMatrix;
-
     Team team;
 
     bool started = false;
 
 public:
-    ArucoDetector(Type::RobotPose* pose, const std::string& calibrationPath, Team team, bool headless = false);
+    ArucoDetector(const std::string& calibrationPath, Team team, bool headless = false);
 
     ~ArucoDetector();
 
@@ -42,8 +38,6 @@ public:
     void readCameraParameters(const std::string& path);
 
     void addArucoTag(const ArucoTag& tag);
-
-    void updateTransformationMatrix();
 /*
     static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, Type::RobotPose* robotPose);
 

arucoDetector.cpp

@@ -54,9 +54,7 @@ int main(int argc, char *argv[])
         userInput = std::thread(userInputThread);
     }
 
-    auto* robotPose = new Type::RobotPose{cv::Point3f(500, 0, 500), -CV_PI/2};
-
-    ArucoDetector detector(robotPose, calibrationPath, BLUE, headless);
+    ArucoDetector detector(calibrationPath, BLUE, headless);
 
     auto whiteFlower = ArucoTag(36, "White_flower", 19.6, FLOWER);
     whiteFlower.setFlowerObjectRepresentation();
@@ -69,7 +67,7 @@ int main(int argc, char *argv[])
 
     int port = std::stoi(argv[2]);
 
-    MyClient client(robotPose, "127.0.0.1", port);
+    MyClient client("127.0.0.1", port);
 
     client.start();
 

calibrationCharuco.cpp

@@ -11,7 +11,7 @@ int main(int argc, char *argv[]) {
 
     int squaresX = 5;
     int squaresY = 7;
-    float squareLength = 0.03378f;
+    float squareLength = 0.034f;
     float markerLength = 0.017f;
     std::string outputFile = "camera_calibration.yml";
 

tcp/MyClient.cpp

@@ -1,15 +1,10 @@
 #include "MyClient.h"
 
-MyClient::MyClient(Type::RobotPose* robotPose, const char* ip, const int port) : TCPClient(ip, port), robotPose(robotPose)
+MyClient::MyClient(const char* ip, const int port) : TCPClient(ip, port)
 {
 
 }
 
-MyClient::~MyClient()
-{
-    this->stop();
-}
-
 void MyClient::handleMessage(const std::string& message)
 {
     std::vector<std::string> messageSplited = TCPSocket::split(message, ";");
@@ -39,15 +34,6 @@ void MyClient::handleMessage(const std::string& message)
         } else if (messageSplited[2] == "ping")
         {
             this->sendMessage("aruco;ihm;pong;1");
-        } else if (messageSplited[2] == "set pos")
-        {
-            // cut the string with space and take the first, second, third and fourth element
-            std::vector<std::string> tokens = TCPSocket::split(messageSplited[3], ",");
-
-            robotPose->position.x = std::stof(tokens[0]);
-            // robotPose->position.y = std::stof(tokens[1]);
-            robotPose->position.z = std::stof(tokens[1]);
-            robotPose->theta = std::stof(tokens[2]);
         }
     }
 }

tcp/MyClient.h

@@ -7,14 +7,12 @@
 #include "../aruco/ArucoTag.h"
 
 class MyClient : public TCPClient {
-Type::RobotPose* robotPose;
-std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>> arucoTags;
+
+    std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>> arucoTags;
 
 public:
 
-    ~MyClient() override;
-
-    explicit MyClient(Type::RobotPose* robotPose, const char* ip = "127.0.0.1", int port = 8080);
+    explicit MyClient(const char* ip = "127.0.0.1", int port = 8080);
 
     void handleMessage(const std::string& message) override;