sebastien.dubois
/
EU_Robot_Group-D
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

bug with arcuo tag 7

This commit is contained in:
Thomas BONNIER 2024-12-13 11:58:05 +01:00
parent 3b7ba64aa9
commit 20841f7d60
2 changed files with 40 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
test/src/QRcode.cpp
View File

@ -41,93 +41,26 @@ void loadTransformationMatrix(const std::string& filename)
fs.release();
}
cv::Mat upscaleImage(const cv::Mat& image, double scaleFactor)
cv::Mat enhanceImage(const cv::Mat& image)
{
cv::Mat upscaled;
cv::resize(image, upscaled, cv::Size(), scaleFactor, scaleFactor, cv::INTER_LINEAR);
return upscaled;
}
cv::Mat downscaleImage(const cv::Mat& image, double scaleFactor)
{
if (scaleFactor <= 0)
{
throw std::invalid_argument("Scale factor must be greater than 0.");
}
cv::Mat downscaled;
cv::resize(image, downscaled, cv::Size(), scaleFactor, scaleFactor, cv::INTER_LINEAR);
return downscaled;
}
cv::Mat sharpenImage(const cv::Mat& image)
{
cv::Mat sharpened;
cv::Mat gray, blurred, equalized, thresholded, sharpened;
cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
cv::GaussianBlur(gray, blurred, cv::Size(5, 5), 0);
cv::equalizeHist(blurred, equalized);
cv::adaptiveThreshold(equalized, thresholded, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
cv::Mat sharpeningKernel = (cv::Mat_<float>(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0);
cv::filter2D(image, sharpened, -1, sharpeningKernel);
cv::filter2D(thresholded, sharpened, -1, sharpeningKernel);
return sharpened;
}
cv::Mat reduceNoise(const cv::Mat& image)
{
cv::Mat denoised;
cv::GaussianBlur(image, denoised, cv::Size(5, 5), 0);
return denoised;
}
cv::Mat gammaCorrection(const cv::Mat& image, double gamma)
{
cv::Mat adjusted;
cv::Mat lookupTable(1, 256, CV_8U);
uchar* p = lookupTable.ptr();
for (int i = 0; i < 256; ++i)
{
p[i] = cv::saturate_cast<uchar>(pow(i / 255.0, gamma) * 255.0);
}
cv::LUT(image, lookupTable, adjusted);
return adjusted;
}
cv::Mat enhanceImage(const cv::Mat& image)
{
cv::Mat enhanced;
cv::Mat kernel = (cv::Mat_<float>(3, 3) << 1, 1, 1, 1, -7, 1, 1, 1, 1);
cv::filter2D(image, enhanced, CV_32F, kernel);
cv::normalize(enhanced, enhanced, 0, 255, cv::NORM_MINMAX, CV_8UC1);
return enhanced;
}
cv::Mat equalizeHistogram(const cv::Mat& image)
{
cv::Mat equalized;
cv::cvtColor(image, equalized, cv::COLOR_BGR2GRAY);
cv::equalizeHist(equalized, equalized);
cv::cvtColor(equalized, equalized, cv::COLOR_GRAY2BGR);
return equalized;
}
cv::Mat adaptiveThresholding(const cv::Mat& image)
{
cv::Mat gray, thresholded;
cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
cv::adaptiveThreshold(gray, thresholded, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
cv::cvtColor(thresholded, thresholded, cv::COLOR_GRAY2BGR);
return thresholded;
}
void processFrame(const cv::Mat& frame)
void processFrame(cv::Mat& frame)
{
// Apply image processing steps
cv::Mat processedFrame = gammaCorrection(frame, 0.4);
processedFrame = sharpenImage(processedFrame);
processedFrame = upscaleImage(processedFrame, 3.0);
processedFrame = reduceNoise(processedFrame);
processedFrame = enhanceImage(processedFrame);
processedFrame = equalizeHistogram(processedFrame);
processedFrame = adaptiveThresholding(processedFrame);
cv::Mat processedFrame = enhanceImage(frame);
// Create ArUco marker detector
cv::Ptr<cv::aruco::Dictionary> dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<cv::Vec3d> rvecs, tvecs;
@ -138,13 +71,13 @@ void processFrame(const cv::Mat& frame)
// Draw detected markers
if (!markerIds.empty())
{
cv::aruco::drawDetectedMarkers(processedFrame, markerCorners, markerIds);
cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
ROS_INFO("Detected %zu ArUco markers", markerIds.size());
for (size_t i = 0; i < markerIds.size(); ++i)
{
ROS_INFO("Marker ID: %d", markerIds[i]);
cv::aruco::drawAxis(processedFrame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
// Print the rotation and translation vectors
ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
@ -183,7 +116,7 @@ void processFrame(const cv::Mat& frame)
}
// Display the result
cv::imshow("ArUco Detection", processedFrame);
cv::imshow("ArUco Detection", frame);
cv::waitKey(1);
}

53
test/src/arenaCalibration.cpp
View File

@ -10,6 +10,7 @@
#include <fstream>
cv::Mat cameraMatrix, distCoeffs;
std::map<int, cv::Point3f> fixedMarkerPositions;
void saveMarkerPositions(const std::string& filename, const std::map<int, cv::Point2f>& markerPositions)
{
@ -34,15 +35,14 @@ void saveTransformationMatrix(const std::string& filename, const cv::Mat& transf
cv::Mat enhanceImage(const cv::Mat& image)
{
cv::Mat enhanced;
cv::cvtColor(image, enhanced, cv::COLOR_BGR2GRAY);
cv::equalizeHist(enhanced, enhanced);
cv::GaussianBlur(enhanced, enhanced, cv::Size(5, 5), 0);
return enhanced;
cv::Mat gray, blurred, equalized, thresholded;
cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
cv::GaussianBlur(gray, blurred, cv::Size(5, 5), 0);
cv::equalizeHist(blurred, equalized);
cv::adaptiveThreshold(equalized, thresholded, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
return thresholded;
}
std::map<int, cv::Point3f> fixedMarkerPositions;
void loadMarkerPositions(const std::string& filename)
{
try
@ -73,7 +73,7 @@ void loadMarkerPositions(const std::string& filename)
}
}
void processFrame(const cv::Mat& frame, std::map<int, cv::Point2f>& markerPositions, cv::Mat& transformationMatrix, bool& calibrationDone)
void processFrame(cv::Mat& frame, std::map<int, cv::Point2f>& markerPositions, cv::Mat& transformationMatrix, bool& calibrationDone)
{
// Apply image processing steps
cv::Mat processedFrame = enhanceImage(frame);
@ -91,14 +91,14 @@ void processFrame(const cv::Mat& frame, std::map<int, cv::Point2f>& markerPositi
// Draw detected markers
if (!markerIds.empty())
{
cv::aruco::drawDetectedMarkers(processedFrame, markerCorners, markerIds);
cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
ROS_INFO("Detected %zu ArUco markers", markerIds.size());
for (size_t i = 0; i < markerIds.size(); ++i)
{
ROS_INFO("Marker ID: %d", markerIds[i]);
cv::aruco::drawAxis(processedFrame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
// Print the rotation and translation vectors
ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
@ -124,8 +124,19 @@ void processFrame(const cv::Mat& frame, std::map<int, cv::Point2f>& markerPositi
markerPositions[markerIds[i]] = cv::Point2f(tvecs[i][0], tvecs[i][1]);
}
// Compute the transformation matrix (example using homography)
if (markerIds.size() >= 3) // Need at least 3 points to compute homography
// Check if all required markers are detected
bool allMarkersDetected = true;
for (const auto& marker : fixedMarkerPositions)
{
if (markerPositions.find(marker.first) == markerPositions.end())
{
allMarkersDetected = false;
break;
}
}
// Compute the transformation matrix if all markers are detected
if (allMarkersDetected)
{
std::vector<cv::Point2f> srcPoints;
std::vector<cv::Point3f> dstPoints;
@ -163,6 +174,10 @@ void frameProcessingThread(cv::VideoCapture& cap, std::map<int, cv::Point2f>& ma
}
processFrame(frame, markerPositions, transformationMatrix, calibrationDone);
// Display the original frame with detected markers for debugging
cv::imshow("Arena Calibration", frame);
if (cv::waitKey(30) == 27) break; // Exit on ESC key
}
}
@ -200,20 +215,6 @@ void calibrateArena()
// Start the frame processing thread
std::thread processingThread(frameProcessingThread, std::ref(cap), std::ref(markerPositions), std::ref(transformationMatrix), std::ref(calibrationDone));
while (ros::ok() && !calibrationDone)
{
cap >> frame; // Capture a new frame
if (frame.empty())
{
ROS_ERROR("Captured empty frame");
continue;
}
// Display the original frame for debugging
cv::imshow("Arena Calibration", frame);
if (cv::waitKey(30) == 27) break; // Exit on ESC key
}
processingThread.join();
if (calibrationDone)