modelec/detection_pot
1
0
Fork 0
mirror of https://github.com/modelec/detection_pot.git synced 2026-01-18 16:47:33 +01:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
ackimixs
2024-01-19 12:46:59 +01:00
parent 4566973631
commit 3df784488f
7 changed files with 45 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -8,6 +8,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} )
set(COMMON_SOURCES set(COMMON_SOURCES
utils/utils.h utils/utils.h
utils/utils.cpp
) )
set(calibrationSources set(calibrationSources

11
arucoDetector.cpp
View File

@@ -51,6 +51,17 @@ int main(int argc, char *argv[])
{ {
break; 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; return 0;

10
calibration_images/calibration_results.yaml
View File

@@ -4,12 +4,12 @@ cameraMatrix: !!opencv-matrix
rows: 3 rows: 3
cols: 3 cols: 3
dt: d dt: d
data: [ 1.5210854147825523e+03, 0., 9.5264072920750630e+02, 0., data: [ 1.5045814501202790e+03, 0., 9.6442035005869900e+02, 0.,
1.5220010501373945e+03, 5.5175450881770780e+02, 0., 0., 1. ] 1.5066446935120762e+03, 5.3637144548900244e+02, 0., 0., 1. ]
distCoeffs: !!opencv-matrix distCoeffs: !!opencv-matrix
rows: 1 rows: 1
cols: 5 cols: 5
dt: d dt: d
data: [ 3.6818238470785941e-01, -2.4288949326716387e+00, data: [ 3.9626454172776632e-01, -2.5689007335445475e+00,
2.4611682433000304e-03, 6.8682976250353874e-04, -4.2189376866905628e-03, 2.2079481537779400e-03,
4.9441917218577958e+00 ] 4.5390098277697222e+00 ]

33
utils/ArucoDetector.cpp
View File

@@ -1,8 +1,11 @@
#include "ArucoDetector.h" #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_<double>(4, 4) << this->transformationMatrix = (cv::Mat_<double>(4, 4) <<
cos(pose.theta), 0, sin(pose.theta), pose.position.x, cos(pose.theta), 0, sin(pose.theta), pose.position.x,
0, 1, 0, pose.position.y, 0, 1, 0, pose.position.y,
@@ -60,12 +63,12 @@ void ArucoDetector::addArucoTag(const ArucoTag& tag)
this->arucoTags.push_back(tag); this->arucoTags.push_back(tag);
} }
std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectArucoTags() std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> ArucoDetector::detectArucoTags()
{ {
cv::Mat frame; cv::Mat frame;
cap >> frame; // Capture frame from the camera cap >> frame; // Capture frame from the camera
std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> result; std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> result;
if (frame.empty()) { if (frame.empty()) {
std::cerr << "Error capturing frame." << std::endl; std::cerr << "Error capturing frame." << std::endl;
@@ -79,10 +82,10 @@ std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectAr
std::vector<int> markerIds; std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners; std::vector<std::vector<cv::Point2f>> markerCorners;
// cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams); cv::aruco::detectMarkers(frame, &this->dictionary, markerCorners, markerIds, &parameters);
// opencv 4.9 // opencv 4.8
detector.detectMarkers(frame, markerCorners, markerIds); // detector.detectMarkers(frame, markerCorners, markerIds);
if (!markerIds.empty()) if (!markerIds.empty())
{ {
@@ -131,21 +134,13 @@ std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectAr
cv::Mat transformedTvec = (transformationMatrix * translat); cv::Mat transformedTvec = (transformationMatrix * translat);
result.second.emplace_back(transformedTvec, rotaEuler); result.second.emplace_back(tag, std::make_pair(transformedTvec, rotaEuler));
if (tag.type == FLOWER)
{
flowerDetector(tag, transformedTvec, rotaEuler);
} else if (tag.type == SOLAR_PANEL)
{
solarPanelDetector(tag, transformedTvec, rotaEuler);
}
} }
} }
std::sort(result.second.begin(), result.second.end(), [](const std::pair<cv::Mat, cv::Mat>& a, const 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 a.first.at<double>(2, 0) < b.first.at<double>(2, 0); return distanceBetweenRobotAndTag(robotPose, a.second.first) < distanceBetweenRobotAndTag(robotPose, b.second.first);
}); });
if (!headless) 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; 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<double>(0, 0) << " z: " << translationMatrix.at<double>(2, 0) << " " << std::endl; std::cout << tag.name << " Pos : x: " << translationMatrix.at<double>(0, 0) << " z: " << translationMatrix.at<double>(2, 0) << " " << std::endl;
const auto yaw = rotationMatrix.at<double>(2, 0); const auto yaw = rotationMatrix.at<double>(2, 0);

14
utils/ArucoDetector.h
View File

@@ -13,7 +13,9 @@ class ArucoDetector {
cv::VideoCapture cap; cv::VideoCapture cap;
cv::aruco::ArucoDetector detector; cv::aruco::Dictionary dictionary;
cv::aruco::DetectorParameters parameters;
// cv::aruco::ArucoDetector detector;
bool headless; bool headless;
@@ -22,22 +24,20 @@ class ArucoDetector {
Team team; Team team;
public: public:
// TODO triée les tag dans la liste en fonction de la distance ArucoDetector(const Type::RobotPose& pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
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(float x, float y, float z, float theta, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
~ArucoDetector(); ~ArucoDetector();
std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> detectArucoTags(); std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> detectArucoTags();
void readCameraParameters(const std::string& path); void readCameraParameters(const std::string& path);
void addArucoTag(const ArucoTag& tag); 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);
}; };

6
utils/utils.cpp Normal file
View File

@@ -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<double>(0, 0), 2) + pow(robotPose.position.y + a.at<double>(1, 0), 2) + pow(robotPose.position.z + a.at<double>(2, 0), 2));
}

5
utils/utils.h
View File

@@ -24,7 +24,4 @@ enum Team
YELLOW YELLOW
}; };
double distance(cv::Mat a) double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a);
{
}