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

Display distance to flower

Browse Source
This commit is contained in:
Félix MARQUET
2024-02-13 16:00:14 +01:00
parent 0402b6f81f
commit 50f4fbcaf1
3 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
aruco/ArucoDetector.cpp
View File

@@ -186,14 +186,15 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
return result;
}
void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix)
void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose)
{
constexpr double distanceToPot = 21;
const double distanceXFlower = translationMatrix.at<double>(0, 0);// + (distanceToPot * sin(rotationMatrix.at<double>(1, 0)));
const double distanceZFlower = translationMatrix.at<double>(2, 0);// + (distanceToPot * cos(rotationMatrix.at<double>(1, 0)));
const double distanceFlower = distanceBetweenRobotAndTag(robotPose, translationMatrix);
std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl;
std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << "distance: " << distanceFlower << std::endl;
}
void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose)

2
aruco/ArucoDetector.h
View File

@@ -44,7 +44,7 @@ public:
void addArucoTag(const ArucoTag& tag);
static 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, const Type::RobotPose& robotPose);
static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose);

2
arucoDetector.cpp
View File

@@ -90,7 +90,7 @@ int main(int argc, char *argv[])
{
if (tags.type == FLOWER)
{
ArucoDetector::flowerDetector(tags, matrix.first, matrix.first);
ArucoDetector::flowerDetector(tags, matrix.first, matrix.first, robotPose);
} else if (tags.type == SOLAR_PANEL)
{
ArucoDetector::solarPanelDetector(tags, matrix.first, matrix.first, robotPose);