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

tt

This commit is contained in:
Thomas BONNIER 2024-12-13 09:45:56 +01:00
parent 0f80eccfd3
commit b1d50517c6
1 changed files with 105 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

179
test/src/arenaCalibration.cpp
View File

@ -126,6 +126,105 @@ void loadMarkerPositions(const std::string& filename)
}
}
void processFrame(const cv::Mat& frame, std::map<int, cv::Point2f>& markerPositions, cv::Mat& transformationMatrix, bool& calibrationDone)
{
// 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);
// Create ArUco marker detector
cv::Ptr<cv::aruco::Dictionary> dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<cv::Vec3d> rvecs, tvecs;
// Detect markers in the image
cv::aruco::detectMarkers(processedFrame, dictionary, markerCorners, markerIds);
// Draw detected markers
if (!markerIds.empty())
{
cv::aruco::drawDetectedMarkers(processedFrame, 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);
// Print the rotation and translation vectors
ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
ROS_INFO_STREAM("Translation Vector: " << tvecs[i]);
// Calculate the distance from the camera to the marker
double distance = cv::norm(tvecs[i]);
ROS_INFO("Distance from camera: %f meters", distance);
// Convert rotation vector to rotation matrix
cv::Mat rotationMatrix;
cv::Rodrigues(rvecs[i], rotationMatrix);
// Extract Euler angles from the rotation matrix
cv::Vec3d eulerAngles;
eulerAngles[0] = atan2(rotationMatrix.at<double>(2, 1), rotationMatrix.at<double>(2, 2)); // Roll
eulerAngles[1] = atan2(-rotationMatrix.at<double>(2, 0), sqrt(pow(rotationMatrix.at<double>(2, 1), 2) + pow(rotationMatrix.at<double>(2, 2), 2))); // Pitch
eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
// Save the marker position
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
{
std::vector<cv::Point2f> srcPoints;
std::vector<cv::Point3f> dstPoints;
for (const auto& marker : markerPositions)
{
if (fixedMarkerPositions.find(marker.first) != fixedMarkerPositions.end())
{
srcPoints.push_back(marker.second);
dstPoints.push_back(fixedMarkerPositions[marker.first]);
}
}
if (srcPoints.size() >= 3)
{
transformationMatrix = cv::findHomography(srcPoints, dstPoints);
calibrationDone = true;
}
}
}
else
{
ROS_INFO("No ArUco markers detected");
}
}
void frameProcessingThread(cv::VideoCapture& cap, std::map<int, cv::Point2f>& markerPositions, cv::Mat& transformationMatrix, bool& calibrationDone)
{
while (ros::ok() && !calibrationDone)
{
cv::Mat frame;
cap >> frame; // Capture a new frame
if (frame.empty())
{
ROS_ERROR("Captured empty frame");
continue;
}
processFrame(frame, markerPositions, transformationMatrix, calibrationDone);
}
}
void calibrateArena()
{
// Create ArUco marker detector
@ -157,6 +256,9 @@ void calibrateArena()
bool calibrationDone = false;
// 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
@ -166,84 +268,13 @@ void calibrateArena()
continue;
}
// Apply image processing steps
frame = gammaCorrection(frame, 0.4);
frame = sharpenImage(frame);
frame = upscaleImage(frame, 3.0);
frame = reduceNoise(frame);
frame = enhanceImage(frame);
frame = equalizeHistogram(frame);
frame = adaptiveThresholding(frame);
// Detect markers in the image
cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds);
// Draw detected markers
if (!markerIds.empty())
{
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(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
// Print the rotation and translation vectors
ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
ROS_INFO_STREAM("Translation Vector: " << tvecs[i]);
// Calculate the distance from the camera to the marker
double distance = cv::norm(tvecs[i]);
ROS_INFO("Distance from camera: %f meters", distance);
// Convert rotation vector to rotation matrix
cv::Mat rotationMatrix;
cv::Rodrigues(rvecs[i], rotationMatrix);
// Extract Euler angles from the rotation matrix
cv::Vec3d eulerAngles;
eulerAngles[0] = atan2(rotationMatrix.at<double>(2, 1), rotationMatrix.at<double>(2, 2)); // Roll
eulerAngles[1] = atan2(-rotationMatrix.at<double>(2, 0), sqrt(pow(rotationMatrix.at<double>(2, 1), 2) + pow(rotationMatrix.at<double>(2, 2), 2))); // Pitch
eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
// Save the marker position
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
{
std::vector<cv::Point2f> srcPoints;
std::vector<cv::Point3f> dstPoints;
for (const auto& marker : markerPositions)
{
if (fixedMarkerPositions.find(marker.first) != fixedMarkerPositions.end())
{
srcPoints.push_back(marker.second);
dstPoints.push_back(fixedMarkerPositions[marker.first]);
}
}
if (srcPoints.size() >= 3)
{
transformationMatrix = cv::findHomography(srcPoints, dstPoints);
calibrationDone = true;
}
}
}
else
{
ROS_INFO("No ArUco markers detected");
}
// Display the result
// 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)
{
// Save the transformation matrix to a configuration file