diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp index fbfe743..0400f35 100644 --- a/aruco/ArucoDetector.cpp +++ b/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_(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>>> Ar tags = this->arucoTags; } - this->updateTransformationMatrix(); - cv::Mat frame; cv::Mat frameNotRotated; cv::Mat frameDistored; @@ -210,16 +201,14 @@ std::pair>>> Ar // Apply the homogeneous transformation to tvec cv::Mat translat = (cv::Mat_(4, 1) << tvec.at(2, 0) + 91/* TODO camera is not at the center*/ /* TODO la pince n'est pas au millieu non plus*/, tvec.at(1, 0) /* + TODO HAUTEUR CAMERA */, (tvec.at(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>& a, const std::pair>& b) + /*std::sort(result.second.begin(), result.second.end(), [this](const std::pair>& a, const std::pair>& 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>>> Ar return result; } -void ArucoDetector::updateTransformationMatrix() { - this->transformationMatrix = (cv::Mat_(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; diff --git a/aruco/ArucoDetector.h b/aruco/ArucoDetector.h index a49a7c9..7591f3a 100644 --- a/aruco/ArucoDetector.h +++ b/aruco/ArucoDetector.h @@ -7,8 +7,6 @@ class ArucoDetector { std::vector 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); diff --git a/arucoDetector.cpp b/arucoDetector.cpp index 4cc9152..2d5b1f3 100644 --- a/arucoDetector.cpp +++ b/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(); diff --git a/calibrationCharuco.cpp b/calibrationCharuco.cpp index c9a708f..d4bbee0 100644 --- a/calibrationCharuco.cpp +++ b/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"; diff --git a/tcp/MyClient.cpp b/tcp/MyClient.cpp index 78ce397..ee96d31 100644 --- a/tcp/MyClient.cpp +++ b/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 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 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]); } } } diff --git a/tcp/MyClient.h b/tcp/MyClient.h index ac1a34c..0c2ca37 100644 --- a/tcp/MyClient.h +++ b/tcp/MyClient.h @@ -7,14 +7,12 @@ #include "../aruco/ArucoTag.h" class MyClient : public TCPClient { -Type::RobotPose* robotPose; -std::vector>> arucoTags; + + std::vector>> 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;