modelec/detection_pot
1
0
Fork 0
mirror of https://github.com/modelec/detection_pot.git synced 2026-01-19 17:17:20 +01:00
Code Issues Packages Projects Releases Wiki Activity

send the position of the tag in the robot xyz base | log rotation matrix

Browse Source
This commit is contained in:
ackimixs
2024-04-10 12:59:02 +02:00
parent d5489ff2b2
commit c52858aef2
3 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
aruco/ArucoDetector.cpp
View File

@@ -189,6 +189,8 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
cv::Mat rotationMatrix;
cv::Rodrigues(rvec, rotationMatrix);
std::cout << rvec << std::endl;
// Extract Euler angles from the rotation matrix
double roll, pitch, yaw;
pitch = asin(rotationMatrix.at<double>(2, 0));
@@ -196,6 +198,8 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
yaw = atan2(-rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0));
// Angles can be used to calculate the distance to the center of the flower.
std::cout << roll, pitch, yaw << std::endl;
cv::Mat rotaEuler = (cv::Mat_<double>(3, 1) << roll, pitch, yaw);
// Apply the homogeneous transformation to tvec
@@ -205,10 +209,10 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
}
}
/*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 // TODO sort with the tvec
});*/
return distanceBetweenRobotAndTag(a.second.first) < distanceBetweenRobotAndTag(b.second.first);
});
if (!headless)
{

6
utils/utils.cpp
View File

@@ -1,6 +1,6 @@
#include "utils.h"
double distanceBetweenRobotAndTag(const Type::RobotPose& robotPose, const cv::Mat& a)
double distanceBetweenRobotAndTag(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));
}
return sqrt(pow(a.at<double>(0, 0), 2) + pow(a.at<double>(1, 0), 2) + pow(a.at<double>(2, 0), 2));
}

8
utils/utils.h
View File

@@ -10,12 +10,6 @@ namespace Type
float pitch;
float yaw;
};
struct RobotPose
{
cv::Point3f position;
float theta; // rotation around the y axis
};
}
enum Team
@@ -24,4 +18,4 @@ enum Team
YELLOW
};
double distanceBetweenRobotAndTag(const Type::RobotPose& robotPose, const cv::Mat& a);
double distanceBetweenRobotAndTag(const cv::Mat& a);