diff --git a/CMakeLists.txt b/CMakeLists.txt index 06d3e89..e05c100 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) set(COMMON_SOURCES utils/utils.h + utils/utils.cpp ) set(calibrationSources diff --git a/arucoDetector.cpp b/arucoDetector.cpp index 7bc6399..00f569b 100644 --- a/arucoDetector.cpp +++ b/arucoDetector.cpp @@ -51,6 +51,17 @@ int main(int argc, char *argv[]) { break; } + + for (auto p : res.second) + { + if (p.first.type == FLOWER) + { + ArucoDetector::flowerDetector(p.first, p.second.first, p.second.first); + } else if (p.first.type == SOLAR_PANEL) + { + ArucoDetector::solarPanelDetector(p.first, p.second.first, p.second.first, robotPose); + } + } } return 0; diff --git a/calibration_images/calibration_results.yaml b/calibration_images/calibration_results.yaml index ec86a19..ecc1a39 100644 --- a/calibration_images/calibration_results.yaml +++ b/calibration_images/calibration_results.yaml @@ -4,12 +4,12 @@ cameraMatrix: !!opencv-matrix rows: 3 cols: 3 dt: d - data: [ 1.5210854147825523e+03, 0., 9.5264072920750630e+02, 0., - 1.5220010501373945e+03, 5.5175450881770780e+02, 0., 0., 1. ] + data: [ 1.5045814501202790e+03, 0., 9.6442035005869900e+02, 0., + 1.5066446935120762e+03, 5.3637144548900244e+02, 0., 0., 1. ] distCoeffs: !!opencv-matrix rows: 1 cols: 5 dt: d - data: [ 3.6818238470785941e-01, -2.4288949326716387e+00, - 2.4611682433000304e-03, 6.8682976250353874e-04, - 4.9441917218577958e+00 ] + data: [ 3.9626454172776632e-01, -2.5689007335445475e+00, + -4.2189376866905628e-03, 2.2079481537779400e-03, + 4.5390098277697222e+00 ] diff --git a/utils/ArucoDetector.cpp b/utils/ArucoDetector.cpp index 50cb798..9862853 100644 --- a/utils/ArucoDetector.cpp +++ b/utils/ArucoDetector.cpp @@ -1,8 +1,11 @@ #include "ArucoDetector.h" -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) +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->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters()); + this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + this->parameters = cv::aruco::DetectorParameters(); + this->transformationMatrix = (cv::Mat_(4, 4) << cos(pose.theta), 0, sin(pose.theta), pose.position.x, 0, 1, 0, pose.position.y, @@ -60,12 +63,12 @@ void ArucoDetector::addArucoTag(const ArucoTag& tag) this->arucoTags.push_back(tag); } -std::pair>> ArucoDetector::detectArucoTags() +std::pair>>> ArucoDetector::detectArucoTags() { cv::Mat frame; cap >> frame; // Capture frame from the camera - std::pair>> result; + std::pair>>> result; if (frame.empty()) { std::cerr << "Error capturing frame." << std::endl; @@ -79,10 +82,10 @@ std::pair>> ArucoDetector::detectAr std::vector markerIds; std::vector> markerCorners; - // cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams); + cv::aruco::detectMarkers(frame, &this->dictionary, markerCorners, markerIds, ¶meters); - // opencv 4.9 - detector.detectMarkers(frame, markerCorners, markerIds); + // opencv 4.8 + // detector.detectMarkers(frame, markerCorners, markerIds); if (!markerIds.empty()) { @@ -131,21 +134,13 @@ std::pair>> ArucoDetector::detectAr cv::Mat transformedTvec = (transformationMatrix * translat); - result.second.emplace_back(transformedTvec, rotaEuler); - - if (tag.type == FLOWER) - { - flowerDetector(tag, transformedTvec, rotaEuler); - } else if (tag.type == SOLAR_PANEL) - { - solarPanelDetector(tag, transformedTvec, rotaEuler); - } + result.second.emplace_back(tag, std::make_pair(transformedTvec, rotaEuler)); } } - std::sort(result.second.begin(), result.second.end(), [](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 a.first.at(2, 0) < b.first.at(2, 0); + return distanceBetweenRobotAndTag(robotPose, a.second.first) < distanceBetweenRobotAndTag(robotPose, b.second.first); }); if (!headless) @@ -174,7 +169,7 @@ void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translati std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl; } -void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix) +void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose) { std::cout << tag.name << " Pos : x: " << translationMatrix.at(0, 0) << " z: " << translationMatrix.at(2, 0) << " " << std::endl; const auto yaw = rotationMatrix.at(2, 0); diff --git a/utils/ArucoDetector.h b/utils/ArucoDetector.h index c8f2d87..bfdf963 100644 --- a/utils/ArucoDetector.h +++ b/utils/ArucoDetector.h @@ -13,7 +13,9 @@ class ArucoDetector { cv::VideoCapture cap; - cv::aruco::ArucoDetector detector; + cv::aruco::Dictionary dictionary; + cv::aruco::DetectorParameters parameters; + // cv::aruco::ArucoDetector detector; bool headless; @@ -22,22 +24,20 @@ class ArucoDetector { Team team; 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(const 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(); - std::pair>> detectArucoTags(); + std::pair>>> detectArucoTags(); void readCameraParameters(const std::string& path); void addArucoTag(const ArucoTag& tag); - void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix); + static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix); - void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix); + static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose); }; diff --git a/utils/utils.cpp b/utils/utils.cpp new file mode 100644 index 0000000..5439a73 --- /dev/null +++ b/utils/utils.cpp @@ -0,0 +1,6 @@ +#include "utils.h" + +double distanceBetweenRobotAndTag(const Type::RobotPose robotPose, const cv::Mat& a) +{ + return sqrt(pow(robotPose.position.x + a.at(0, 0), 2) + pow(robotPose.position.y + a.at(1, 0), 2) + pow(robotPose.position.z + a.at(2, 0), 2)); +} diff --git a/utils/utils.h b/utils/utils.h index 40db759..bb5f6f4 100644 --- a/utils/utils.h +++ b/utils/utils.h @@ -24,7 +24,4 @@ enum Team YELLOW }; -double distance(cv::Mat a) -{ - -} +double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a); \ No newline at end of file