diff --git a/arucoDetector.cpp b/arucoDetector.cpp index 6ff7f0b..7bc6399 100644 --- a/arucoDetector.cpp +++ b/arucoDetector.cpp @@ -34,20 +34,20 @@ int main(int argc, char *argv[]) // End argument parser - const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), 0}; + const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), CV_PI/2}; - ArucoDetector detector(robotPose, calibrationPath, cameraId, headless); + ArucoDetector detector(robotPose, calibrationPath, BLUE, cameraId, headless); while (true) { - const int res = detector.detectArucoTags(); + const auto res = detector.detectArucoTags(); - if (res == -2) + if (res.first == -2) { std::cerr << "Error: Could not capture frame." << std::endl; return -2; } - if (res == 1) + if (res.first == 1) { break; } diff --git a/utils/ArucoDetector.cpp b/utils/ArucoDetector.cpp index 0d74c0e..50cb798 100644 --- a/utils/ArucoDetector.cpp +++ b/utils/ArucoDetector.cpp @@ -1,6 +1,6 @@ #include "ArucoDetector.h" -ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& calibrationPath, const int cameraId, const bool headless) : robotPose(pose), headless(headless) +ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : robotPose(pose), headless(headless), team(team) { this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters()); this->transformationMatrix = (cv::Mat_(4, 4) << @@ -22,12 +22,17 @@ ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& cali cv::namedWindow("ArUco Detection", cv::WINDOW_NORMAL); } - this->addArucoTag(ArucoTag(36, "White flower", 20, FLOWER)); - this->addArucoTag(ArucoTag(13, "Purple flower", 20, FLOWER)); - this->addArucoTag(ArucoTag(47, "Solar panel", 20, FLOWER)); + auto whiteFlower = ArucoTag(36, "White flower", 20, FLOWER); + whiteFlower.setFlowerObjectRepresentation(); + this->addArucoTag(whiteFlower); + auto purpleFlower = ArucoTag(13, "Purple flower", 20, FLOWER); + purpleFlower.setFlowerObjectRepresentation(); + this->addArucoTag(purpleFlower); + + this->addArucoTag(ArucoTag(47, "Solar panel", 50, SOLAR_PANEL)); } -ArucoDetector::ArucoDetector(const float x, const float y, const float z, const float theta, const std::string& calibrationPath, const int cameraId, const bool headless) : ArucoDetector(Type::RobotPose{cv::Point3f(x, y, z), theta}, calibrationPath, cameraId, headless) +ArucoDetector::ArucoDetector(const float x, const float y, const float z, const float theta, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : ArucoDetector(Type::RobotPose{cv::Point3f(x, y, z), theta}, calibrationPath, team, cameraId, headless) { } @@ -55,14 +60,17 @@ void ArucoDetector::addArucoTag(const ArucoTag& tag) this->arucoTags.push_back(tag); } -int ArucoDetector::detectArucoTags() +std::pair>> ArucoDetector::detectArucoTags() { cv::Mat frame; cap >> frame; // Capture frame from the camera + std::pair>> result; + if (frame.empty()) { std::cerr << "Error capturing frame." << std::endl; - return -2; + result.first = -2; + return result; } cv::Mat gray; @@ -84,7 +92,8 @@ int ArucoDetector::detectArucoTags() cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds); } - for (size_t i = 0; i < markerCorners.size(); i++) { + for (size_t i = 0; i < markerCorners.size(); i++) + { int id = markerIds[i]; if (std::find_if(arucoTags.begin(), arucoTags.end(), [id](const ArucoTag& tag) { return tag.id == id; }) == arucoTags.end()) @@ -122,6 +131,8 @@ int ArucoDetector::detectArucoTags() cv::Mat transformedTvec = (transformationMatrix * translat); + result.second.emplace_back(transformedTvec, rotaEuler); + if (tag.type == FLOWER) { flowerDetector(tag, transformedTvec, rotaEuler); @@ -132,29 +143,59 @@ int ArucoDetector::detectArucoTags() } } + std::sort(result.second.begin(), result.second.end(), [](const std::pair& a, const std::pair& b) + { + return a.first.at(2, 0) < b.first.at(2, 0); + }); + if (!headless) { cv::imshow("ArUco Detection", frame); } - if (cv::waitKey(10) == 27) // Press 'Esc' to exit - return 1; + if (cv::waitKey(10) == 27) + { + // Press 'Esc' to exit + result.first = 1; + return result; + } - return 0; + result.first = 0; + return result; } void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix) { constexpr double distanceToPot = 21; - const double distanceXFlower = translationMatrix.at(0, 0) + (distanceToPot * sin(rotationMatrix.at(1, 0))); - const double distanceZFlower = translationMatrix.at(2, 0) + (distanceToPot * cos(rotationMatrix.at(1, 0))); + const double distanceXFlower = translationMatrix.at(0, 0);// + (distanceToPot * sin(rotationMatrix.at(1, 0))); + const double distanceZFlower = translationMatrix.at(2, 0);// + (distanceToPot * cos(rotationMatrix.at(1, 0))); std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl; } -void ArucoDetector::solarPanelDetector(ArucoTag tag, cv::Mat translationMatrix, cv::Mat rotationMatrix) +void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix) { std::cout << tag.name << " Pos : x: " << translationMatrix.at(0, 0) << " z: " << translationMatrix.at(2, 0) << " " << std::endl; + const auto yaw = rotationMatrix.at(2, 0); + + const auto rotationBaseTable = (-yaw) + robotPose.theta; + + std::cout << " Rotation: " << rotationBaseTable * (180 / CV_PI) << std::endl; + + if (rotationBaseTable > 70 && rotationBaseTable <= 110) + { + std::cout << "Mid" << std::endl; + } + else if (rotationBaseTable > 30 && rotationBaseTable <= 70) + { + std::cout << "Blue side" << std::endl; + } + else if (rotationBaseTable > 110 && rotationBaseTable <= 150) + { + std::cout << "Yellow side" << std::endl; + } + + // BLUE => 90, YELLOW => -90 } diff --git a/utils/ArucoDetector.h b/utils/ArucoDetector.h index 2fddd38..c8f2d87 100644 --- a/utils/ArucoDetector.h +++ b/utils/ArucoDetector.h @@ -19,14 +19,18 @@ class ArucoDetector { cv::Mat transformationMatrix; -public: - ArucoDetector(Type::RobotPose pose, const std::string& calibrationPath, int cameraId = 0, bool headless = false); + Team team; - ArucoDetector(float x, float y, float z, float theta, const std::string& calibrationPath, int cameraId = 0, bool headless = false); +public: + // TODO triée les tag dans la liste en fonction de la distance + + ArucoDetector(Type::RobotPose pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false); + + ArucoDetector(float x, float y, float z, float theta, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false); ~ArucoDetector(); - int detectArucoTags(); + std::pair>> detectArucoTags(); void readCameraParameters(const std::string& path); @@ -34,6 +38,6 @@ public: void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix); - void solarPanelDetector(ArucoTag type, cv::Mat translationMatrix, cv::Mat rotationMatrix); + void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix); }; diff --git a/utils/ArucoTag.h b/utils/ArucoTag.h index 0bff2af..6598eb4 100644 --- a/utils/ArucoTag.h +++ b/utils/ArucoTag.h @@ -29,9 +29,10 @@ public: void setFlowerObjectRepresentation() { - this->objectRepresenation.ptr(0)[0] = cv::Vec3f(-length/2.f, length/2.f, 0); - this->objectRepresenation.ptr(0)[1] = cv::Vec3f(length/2.f, length/2.f, 0); - this->objectRepresenation.ptr(0)[2] = cv::Vec3f(length/2.f, -length/2.f, 0); - this->objectRepresenation.ptr(0)[3] = cv::Vec3f(-length/2.f, -length/2.f, 0); + this->objectRepresenation = cv::Mat(4, 1, CV_32FC3); + this->objectRepresenation.ptr(0)[0] = cv::Vec3f(-19/2.f, 19/2.f, 0); + this->objectRepresenation.ptr(0)[1] = cv::Vec3f(19/2.f, 19/2.f, 0); + this->objectRepresenation.ptr(0)[2] = cv::Vec3f(18.7f/2.f, -19/2.f, 0); + this->objectRepresenation.ptr(0)[3] = cv::Vec3f(-18.7f/2.f, -19/2.f, 0); } }; diff --git a/utils/utils.h b/utils/utils.h index ffad7ac..40db759 100644 --- a/utils/utils.h +++ b/utils/utils.h @@ -17,3 +17,14 @@ namespace Type float theta; // rotation around the y axis }; } + +enum Team +{ + BLUE, + YELLOW +}; + +double distance(cv::Mat a) +{ + +}