From b1d50517c6224546d7c19d1ba7f1cdfa35aee8f7 Mon Sep 17 00:00:00 2001 From: Thomas Bonnier Date: Fri, 13 Dec 2024 09:45:56 +0100 Subject: [PATCH] tt --- test/src/arenaCalibration.cpp | 179 ++++++++++++++++++++-------------- 1 file changed, 105 insertions(+), 74 deletions(-) diff --git a/test/src/arenaCalibration.cpp b/test/src/arenaCalibration.cpp index 4176d2d..e632e1a 100644 --- a/test/src/arenaCalibration.cpp +++ b/test/src/arenaCalibration.cpp @@ -126,6 +126,105 @@ void loadMarkerPositions(const std::string& filename) } } +void processFrame(const cv::Mat& frame, std::map& 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 dictionary = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + std::vector markerIds; + std::vector> markerCorners; + std::vector 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(2, 1), rotationMatrix.at(2, 2)); // Roll + eulerAngles[1] = atan2(-rotationMatrix.at(2, 0), sqrt(pow(rotationMatrix.at(2, 1), 2) + pow(rotationMatrix.at(2, 2), 2))); // Pitch + eulerAngles[2] = atan2(rotationMatrix.at(1, 0), rotationMatrix.at(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 srcPoints; + std::vector 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& 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(2, 1), rotationMatrix.at(2, 2)); // Roll - eulerAngles[1] = atan2(-rotationMatrix.at(2, 0), sqrt(pow(rotationMatrix.at(2, 1), 2) + pow(rotationMatrix.at(2, 2), 2))); // Pitch - eulerAngles[2] = atan2(rotationMatrix.at(1, 0), rotationMatrix.at(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 srcPoints; - std::vector 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