From 3141b541501e33d8ae766ff201d9ca22a276a218 Mon Sep 17 00:00:00 2001 From: mebbaid Date: Wed, 5 Feb 2025 17:43:25 +0100 Subject: [PATCH 1/5] enable pose estimation for aruco boards --- .../Perception/Features/ArucoDetector.h | 40 +++-- src/Perception/src/ArucoDetector.cpp | 146 ++++++++++++++---- .../Perception/Features/ArucoDetectorTest.cpp | 79 +++++++--- 3 files changed, 198 insertions(+), 67 deletions(-) diff --git a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h index f8295e1bae..30374156df 100644 --- a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h +++ b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h @@ -13,6 +13,7 @@ #include #include +#include // Include for Board definition #include #include @@ -29,11 +30,16 @@ struct ArucoMarkerData { /** * Marker ID + * Note: When detecting boards, a special ID (e.g., 0) can be used + * to represent the board's pose. */ int id{-1}; /** - * Marker corners in camera coordinates + * Marker corners in camera coordinates. + * For boards, this may contain the corners of one of the markers + * on the board or be left empty, depending on how you want to + * represent the board's visual information. * in the order * - top left * - top right @@ -43,7 +49,7 @@ struct ArucoMarkerData std::vector corners; /** - * Pose of the marker in camera frame + * Pose of the marker/board in camera frame * cam_H_marker */ Eigen::Matrix4d pose; @@ -55,6 +61,11 @@ struct ArucoMarkerData */ struct ArucoDetectorOutput { + /** + * Map of detected markers/boards. + * Key: Marker ID (or special ID for the board) + * Value: ArucoMarkerData for the corresponding marker/board + */ std::unordered_map markers; double timeNow{-1.0}; }; @@ -78,6 +89,12 @@ class ArucoDetector : public System::Source * - "marker_length" marker length in m * - "camera_matrix" 9d vector representing the camera calbration matrix in row major order * - "distortion_coefficients" 5d vector containing camera distortion coefficients + * + * For board detection, also require: + * - "markers_x": Number of markers in the X direction of the board. + * - "markers_y": Number of markers in the Y direction of the board. + * - "marker_separation": Separation between markers on the board (in meters). + * * @param[in] handlerWeak weak pointer to a ParametersHandler::IParametersHandler interface * @tparameter Derived particular implementation of the IParameterHandler * @return True in case of success, false otherwise. @@ -85,7 +102,7 @@ class ArucoDetector : public System::Source bool initialize(std::weak_ptr handler) final; /** - * Set image for which markers need to be detected + * Set image for which markers/boards need to be detected * @param[in] inputImg image as OpenCV mat * @param[in] timeNow current time in chosen time units * it is useful for bookkeeping @@ -101,25 +118,26 @@ class ArucoDetector : public System::Source bool advance() final; /** - * Get the detected markers' data from the current step + * Get the detected markers'/boards' data from the current step * @return A struct containing a map container of detected markers. */ const ArucoDetectorOutput& getOutput() const final; /** - * Get the detected marker data - * @param[in] id marker id + * Get the detected marker/board data + * @param[in] id marker/board id. Use a special ID (e.g., 0) to retrieve + * the board's pose if board detection is enabled. * @param[in] markerData detected marker identifiers data - * @return True in case of success, false if marker was not detected + * @return True in case of success, false if marker/board was not detected */ bool getDetectedMarkerData(const int& id, ArucoMarkerData& markerData); /** - * Get the image with drawn detected markers + * Get the image with drawn detected markers/board pose. * @param[in] outputImg image with detected markers drawn on it - * @param[in] drawFrames draw also estimated marker poses, set to false by default + * @param[in] drawFrames draw also estimated marker/board poses, set to false by default * @param[in] axisLengthForDrawing axis length for drawing the frame axes, 0.1 by default - * @return True in case of success, false if no marker was detected + * @return True in case of success, false if no marker/board was detected */ bool getImageWithDetectedMarkers(cv::Mat& outputImg, const bool& drawFrames = false, @@ -139,4 +157,4 @@ class ArucoDetector : public System::Source } // namespace Perception } // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_PERECEPTION_FEATURES_ARUCO_H +#endif // BIPEDAL_LOCOMOTION_PERECEPTION_FEATURES_ARUCO_H \ No newline at end of file diff --git a/src/Perception/src/ArucoDetector.cpp b/src/Perception/src/ArucoDetector.cpp index b07d987279..49d62c3938 100644 --- a/src/Perception/src/ArucoDetector.cpp +++ b/src/Perception/src/ArucoDetector.cpp @@ -1,10 +1,3 @@ -/** - * @file ArucoDetector.cpp - * @authors Prashanth Ramadoss - * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - #include #include #include @@ -15,7 +8,6 @@ using namespace BipedalLocomotion::GenericContainer; using namespace BipedalLocomotion::Perception; using namespace BipedalLocomotion::Conversions; -#include #include #include @@ -32,6 +24,7 @@ class ArucoDetector::Impl double markerLength; /**< marker length*/ cv::Mat cameraMatrix; /**< camera calibration matrix*/ cv::Mat distCoeff; /**< camera distortion coefficients*/ + cv::Ptr board; /**< ArUco board configuration */ ArucoDetectorOutput out; /**< container with detected markers data */ cv::Mat currentImg; /**< currently set image */ @@ -163,6 +156,43 @@ bool ArucoDetector::initialize(std::weak_ptr handler) m_pimpl->distCoeff = cv::Mat(5, 1, CV_64F); cv::eigen2cv(distCoeffVec, m_pimpl->distCoeff); + // Board configuration parameters (read from file or hardcode) + int markersX; + int markersY; + double markerSeparation; + + if (!handle->getParameter("markers_x", markersX)) + { + log()->error("{} The parameter handler could not find \"markers_x\" in the configuration file.", printPrefix); + return false; + } + if (!handle->getParameter("markers_y", markersY)) + { + log()->error("{} The parameter handler could not find \"markers_y\" in the configuration file.", printPrefix); + return false; + } + if (!handle->getParameter("marker_separation", markerSeparation)) + { + log()->error("{} The parameter handler could not find \"marker_separation\" in the configuration file.", printPrefix); + return false; + } + + + // Create the board object + cv::Size boardSize(markersX, markersY); + float markerLength = m_pimpl->markerLength; + float markerDistance = markerSeparation; // distance in meters + + // Create the board layout (assuming sequential IDs) + std::vector markerIds; + for (int i = 0; i < boardSize.width * boardSize.height; ++i) + { + markerIds.push_back(i); // Sequential IDs + } + // Define the board using the layout and the dictionary + m_pimpl->board = cv::aruco::GridBoard::create(boardSize.width, boardSize.height, markerLength, markerDistance, m_pimpl->dictionary); + + m_pimpl->initialized = true; return true; } @@ -201,7 +231,8 @@ bool ArucoDetector::advance() } m_pimpl->resetBuffers(); - std::vector> detectedmarkerCorners; + + // Detect markers cv::aruco::detectMarkers(m_pimpl->currentImg, m_pimpl->dictionary, m_pimpl->currentDetectedMarkerCorners, @@ -209,27 +240,59 @@ bool ArucoDetector::advance() if (m_pimpl->currentDetectedMarkerIds.size() > 0) { - cv::aruco::estimatePoseSingleMarkers(m_pimpl->currentDetectedMarkerCorners, - m_pimpl->markerLength, - m_pimpl->cameraMatrix, - m_pimpl->distCoeff, - m_pimpl->currentDetectedMarkersRotVecs, - m_pimpl->currentDetectedMarkersTransVecs); - - for (std::size_t idx = 0; idx < m_pimpl->currentDetectedMarkerIds.size(); idx++) + // Estimate board pose (assuming you have an ArUco board) + cv::Vec3d rvec, tvec; + int valid_corners = cv::aruco::estimatePoseBoard(m_pimpl->currentDetectedMarkerCorners, + m_pimpl->currentDetectedMarkerIds, + m_pimpl->board, // Assuming you have board object + m_pimpl->cameraMatrix, + m_pimpl->distCoeff, + rvec, + tvec); + + if (valid_corners > 0) { - cv::Rodrigues(m_pimpl->currentDetectedMarkersRotVecs[idx], m_pimpl->R); + // Board detected + // Convert to Eigen matrices + cv::Rodrigues(rvec, m_pimpl->R); cv::cv2eigen(m_pimpl->R, m_pimpl->Reig); - m_pimpl->teig << m_pimpl->currentDetectedMarkersTransVecs[idx](0), - m_pimpl->currentDetectedMarkersTransVecs[idx](1), - m_pimpl->currentDetectedMarkersTransVecs[idx](2); + m_pimpl->teig << tvec[0], tvec[1], tvec[2]; m_pimpl->poseEig = toEigenPose(m_pimpl->Reig, m_pimpl->teig); - ArucoMarkerData markerData{m_pimpl->currentDetectedMarkerIds[idx], - m_pimpl->currentDetectedMarkerCorners[idx], + + // Store the board's pose as marker id 0 + ArucoMarkerData markerData{0, + {}, // No corners for board m_pimpl->poseEig}; - m_pimpl->out.markers[m_pimpl->currentDetectedMarkerIds[idx]] = markerData; + m_pimpl->out.markers[0] = markerData; } + else + { + // Board not detected (or not enough markers) + // Estimate pose of single markers + cv::aruco::estimatePoseSingleMarkers(m_pimpl->currentDetectedMarkerCorners, + m_pimpl->markerLength, + m_pimpl->cameraMatrix, + m_pimpl->distCoeff, + m_pimpl->currentDetectedMarkersRotVecs, + m_pimpl->currentDetectedMarkersTransVecs); + + for (std::size_t idx = 0; idx < m_pimpl->currentDetectedMarkerIds.size(); idx++) + { + cv::Rodrigues(m_pimpl->currentDetectedMarkersRotVecs[idx], m_pimpl->R); + cv::cv2eigen(m_pimpl->R, m_pimpl->Reig); + m_pimpl->teig << m_pimpl->currentDetectedMarkersTransVecs[idx](0), + m_pimpl->currentDetectedMarkersTransVecs[idx](1), + m_pimpl->currentDetectedMarkersTransVecs[idx](2); + + m_pimpl->poseEig = toEigenPose(m_pimpl->Reig, m_pimpl->teig); + ArucoMarkerData markerData{m_pimpl->currentDetectedMarkerIds[idx], + m_pimpl->currentDetectedMarkerCorners[idx], + m_pimpl->poseEig}; + m_pimpl->out.markers[m_pimpl->currentDetectedMarkerIds[idx]] = markerData; + } + } + m_pimpl->out.timeNow = m_pimpl->currentTime; } @@ -266,8 +329,7 @@ bool ArucoDetector::getImageWithDetectedMarkers(cv::Mat& outputImg, const bool& drawFrames, const double& axisLengthForDrawing) { - std::size_t nrDetectedMarkers = m_pimpl->currentDetectedMarkerIds.size(); - if (m_pimpl->currentImg.empty() || nrDetectedMarkers <= 0) + if (m_pimpl->currentImg.empty()) { return false; } @@ -283,17 +345,33 @@ bool ArucoDetector::getImageWithDetectedMarkers(cv::Mat& outputImg, m_pimpl->currentDetectedMarkerCorners, m_pimpl->currentDetectedMarkerIds); - if (drawFrames) + if (drawFrames && m_pimpl->out.markers.find(0) != m_pimpl->out.markers.end()) // Assuming board ID is 0 { - for (std::size_t idx = 0; idx < nrDetectedMarkers; idx++) - { - cv::drawFrameAxes(outputImg, + // Draw coordinate axes on the board + cv::Vec3d rvec, tvec; + + // Convert Eigen pose back to OpenCV rvec and tvec + Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity(); + Eigen::Vector3d translationVector = Eigen::Vector3d::Zero(); + if (m_pimpl->out.markers.count(0)) { + rotationMatrix = m_pimpl->out.markers.at(0).pose.block<3,3>(0,0); + translationVector = m_pimpl->out.markers.at(0).pose.block<3,1>(0,3); + } + + + cv::eigen2cv(rotationMatrix, m_pimpl->R); // Convert Eigen rotation to OpenCV Mat + cv::Rodrigues(m_pimpl->R, rvec); // Convert rotation matrix to rotation vector + + tvec[0] = translationVector(0); + tvec[1] = translationVector(1); + tvec[2] = translationVector(2); + + cv::drawFrameAxes(outputImg, m_pimpl->cameraMatrix, m_pimpl->distCoeff, - m_pimpl->currentDetectedMarkersRotVecs[idx], - m_pimpl->currentDetectedMarkersTransVecs[idx], + rvec, // rotation vector + tvec, // translation vector axisLengthForDrawing); - } } return true; @@ -307,4 +385,4 @@ void ArucoDetector::Impl::resetBuffers() currentDetectedMarkersTransVecs.clear(); out.markers.clear(); out.timeNow = -1.0; -} +} \ No newline at end of file diff --git a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp index 3a128064eb..af3e103ee5 100644 --- a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp +++ b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp @@ -14,40 +14,75 @@ #include #include +#include +#include +#include + using namespace BipedalLocomotion::Perception; using namespace BipedalLocomotion::ParametersHandler; -TEST_CASE("Aruco Detector") +TEST_CASE("Aruco Detector - Live Camera Feed with Boards") { std::shared_ptr parameterHandler = std::make_shared(); - parameterHandler->setParameter("marker_dictionary", "4X4_50"); - parameterHandler->setParameter("marker_length", 0.806); - parameterHandler->setParameter("camera_matrix", std::vector{922.309448242188,0,664.546813964844,0,922.194458007813,348.770141601563,0,0,1}); + parameterHandler->setParameter("marker_dictionary", "6X6_250"); + parameterHandler->setParameter("marker_length", 0.05); // Reduced marker length to test with small markers + parameterHandler->setParameter("markers_x", 5); // Number of markers in X direction + parameterHandler->setParameter("markers_y", 4); // Number of markers in Y direction + parameterHandler->setParameter("marker_separation", 0.02); // Marker separation in meters + parameterHandler->setParameter("camera_matrix", std::vector{600, 0, 320, 0, 600, 240, 0, 0, 1}); // Example Values, replace with calibrated ones parameterHandler->setParameter("distortion_coefficients", std::vector{0.0, 0.0, 0.0, 0.0, 0.0}); + std::cout << "Parameters set" << std::endl; - // // Instantiate the estimator + // Instantiate the detector ArucoDetector detector; REQUIRE(detector.initialize(parameterHandler)); - auto imgName = getSampleImagePath(); - auto inputImg = cv::imread(imgName); + // Initialize video capture + cv::VideoCapture cap(0); // Open the default camera + REQUIRE(cap.isOpened()); + + cv::namedWindow("Aruco Detection - Live with Boards", cv::WINDOW_AUTOSIZE); + + auto startTime = std::chrono::steady_clock::now(); + auto endTime = startTime + std::chrono::seconds(10); // Run for 10 seconds + + while (std::chrono::steady_clock::now() < endTime) + { + cv::Mat frame; + cap >> frame; + REQUIRE(!frame.empty()); + + REQUIRE(detector.setImage(frame, 0.0)); // Time is not important for this test + REQUIRE(detector.advance()); - REQUIRE(detector.setImage(inputImg, 0.1)); - REQUIRE(detector.advance()); + cv::Mat outputImage; + if (!detector.getImageWithDetectedMarkers(outputImage, true, 0.05)) // Draw frames and reduce axisLength + { + outputImage = frame.clone(); + } - cv::Mat outputImage; - REQUIRE(detector.getImageWithDetectedMarkers(outputImage, - /*drawFrames=*/ true, - /*axisLengthForDrawing=*/ 0.3)); + // Check for board detection + ArucoMarkerData boardData; + bool boardDetected = detector.getDetectedMarkerData(0, boardData); // Check for board with id 0 + if (boardDetected) + { + std::cout << "Board Detected! Pose:" << std::endl << boardData.pose << std::endl; + } + else + { + std::cout << "No Board Detected." << std::endl; + } - // Marker 2 is detected in the sample image - ArucoMarkerData marker2; - REQUIRE(detector.getDetectedMarkerData(/*id=*/ 2, marker2)); - REQUIRE(marker2.id == 2); + cv::imshow("Aruco Detection - Live with Boards", outputImage); - /* // uncomment this block to view the output image - cv::imshow("outputImage", outputImage); - cv::waitKey(); - */ + // Small delay to allow the GUI to update and process key presses + if (cv::waitKey(1) == 'q') + { + break; // exit if q is pressed + } + } -} + // Cleanup + cap.release(); + cv::destroyAllWindows(); +} \ No newline at end of file From 019c2224af0ffe24d35108ed5fa2664ef377f822 Mon Sep 17 00:00:00 2001 From: mebbaid Date: Wed, 5 Feb 2025 18:39:21 +0100 Subject: [PATCH 2/5] updated unit test --- .../tests/Perception/Features/ArucoDetectorTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp index af3e103ee5..f7d83c8164 100644 --- a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp +++ b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp @@ -25,9 +25,9 @@ TEST_CASE("Aruco Detector - Live Camera Feed with Boards") { std::shared_ptr parameterHandler = std::make_shared(); parameterHandler->setParameter("marker_dictionary", "6X6_250"); - parameterHandler->setParameter("marker_length", 0.05); // Reduced marker length to test with small markers + parameterHandler->setParameter("marker_length", 0.1); // Reduced marker length to test with small markers parameterHandler->setParameter("markers_x", 5); // Number of markers in X direction - parameterHandler->setParameter("markers_y", 4); // Number of markers in Y direction + parameterHandler->setParameter("markers_y", 7); // Number of markers in Y direction parameterHandler->setParameter("marker_separation", 0.02); // Marker separation in meters parameterHandler->setParameter("camera_matrix", std::vector{600, 0, 320, 0, 600, 240, 0, 0, 1}); // Example Values, replace with calibrated ones parameterHandler->setParameter("distortion_coefficients", std::vector{0.0, 0.0, 0.0, 0.0, 0.0}); From 0ecf1271421a4677a40bda061e70d84975a3759b Mon Sep 17 00:00:00 2001 From: mebbaid Date: Thu, 6 Feb 2025 11:04:06 +0100 Subject: [PATCH 3/5] add parameter is_board --- .../Perception/Features/ArucoDetector.h | 8 +- src/Perception/src/ArucoDetector.cpp | 186 +++++++++++------- .../Perception/Features/ArucoDetectorTest.cpp | 5 +- 3 files changed, 120 insertions(+), 79 deletions(-) diff --git a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h index 30374156df..87780b45ba 100644 --- a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h +++ b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h @@ -26,7 +26,7 @@ namespace Perception /** * Aruco marker identifiers */ -struct ArucoMarkerData +struct ArucoData { /** * Marker ID @@ -64,9 +64,9 @@ struct ArucoDetectorOutput /** * Map of detected markers/boards. * Key: Marker ID (or special ID for the board) - * Value: ArucoMarkerData for the corresponding marker/board + * Value: ArucoData for the corresponding marker/board */ - std::unordered_map markers; + std::unordered_map markers; double timeNow{-1.0}; }; @@ -130,7 +130,7 @@ class ArucoDetector : public System::Source * @param[in] markerData detected marker identifiers data * @return True in case of success, false if marker/board was not detected */ - bool getDetectedMarkerData(const int& id, ArucoMarkerData& markerData); + bool getDetectedMarkerData(const int& id, ArucoData& arucoData); /** * Get the image with drawn detected markers/board pose. diff --git a/src/Perception/src/ArucoDetector.cpp b/src/Perception/src/ArucoDetector.cpp index 49d62c3938..916817ddf4 100644 --- a/src/Perception/src/ArucoDetector.cpp +++ b/src/Perception/src/ArucoDetector.cpp @@ -20,6 +20,7 @@ class ArucoDetector::Impl void resetBuffers(); // parameters + bool isBoard{false}; /**< flag to check if we want to detect a board */ cv::Ptr dictionary; /**< container with detected markers data */ double markerLength; /**< marker length*/ cv::Mat cameraMatrix; /**< camera calibration matrix*/ @@ -72,6 +73,9 @@ class ArucoDetector::Impl {"7X7_1000", cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_7X7_1000}, {"ARUCO_ORIGINAL", cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_ARUCO_ORIGINAL}}; + + // Helper method for single marker pose estimation + void estimateSingleMarkersPose(); }; ArucoDetector::ArucoDetector() @@ -117,11 +121,20 @@ bool ArucoDetector::initialize(std::weak_ptr handler) // instead of a cv::Ptr #if (CV_VERSION_MAJOR >= 5) || (CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7) m_pimpl->dictionary = cv::makePtr(); - *(m_pimpl->dictionary) = cv::aruco::getPredefinedDictionary(m_pimpl->availableDict.at(dictName)); + *(m_pimpl->dictionary) + = cv::aruco::getPredefinedDictionary(m_pimpl->availableDict.at(dictName)); #else m_pimpl->dictionary = cv::aruco::getPredefinedDictionary(m_pimpl->availableDict.at(dictName)); #endif + if (!handle->getParameter("is_board", m_pimpl->isBoard)) + { + log()->error("{} The parameter handler could not find \" is_board \" in the " + "configuration file.", + printPrefix); + return false; + } + if (!handle->getParameter("marker_length", m_pimpl->markerLength)) { log()->error("{} The parameter handler could not find \" marker_length \" in the " @@ -156,42 +169,58 @@ bool ArucoDetector::initialize(std::weak_ptr handler) m_pimpl->distCoeff = cv::Mat(5, 1, CV_64F); cv::eigen2cv(distCoeffVec, m_pimpl->distCoeff); - // Board configuration parameters (read from file or hardcode) - int markersX; - int markersY; - double markerSeparation; - - if (!handle->getParameter("markers_x", markersX)) + // Board configuration parameters + if (m_pimpl->isBoard) { - log()->error("{} The parameter handler could not find \"markers_x\" in the configuration file.", printPrefix); - return false; - } - if (!handle->getParameter("markers_y", markersY)) - { - log()->error("{} The parameter handler could not find \"markers_y\" in the configuration file.", printPrefix); - return false; - } - if (!handle->getParameter("marker_separation", markerSeparation)) - { - log()->error("{} The parameter handler could not find \"marker_separation\" in the configuration file.", printPrefix); - return false; - } + int markersX; + int markersY; + double markerSeparation; + if (!handle->getParameter("markers_x", markersX)) + { + log()->error("{} The parameter handler could not find \"markers_x\" in the " + "configuration file, required for board detection.", + printPrefix); + return false; + } + if (!handle->getParameter("markers_y", markersY)) + { + log()->error("{} The parameter handler could not find \"markers_y\" in the " + "configuration file, required for board detection.", + printPrefix); + return false; + } - // Create the board object - cv::Size boardSize(markersX, markersY); - float markerLength = m_pimpl->markerLength; - float markerDistance = markerSeparation; // distance in meters + if (!handle->getParameter("marker_separation", markerSeparation)) + { + log()->error("{} The parameter handler could not find \"marker_separation\" in the " + "configuration file, required for board detection.", + printPrefix); + return false; + } - // Create the board layout (assuming sequential IDs) - std::vector markerIds; - for (int i = 0; i < boardSize.width * boardSize.height; ++i) + // Create the board object + cv::Size boardSize(markersX, markersY); + float markerLength = m_pimpl->markerLength; + float markerDistance = markerSeparation; // distance in meters + + // Create the board layout (assuming sequential IDs) + std::vector markerIds; + for (int i = 0; i < boardSize.width * boardSize.height; ++i) + { + markerIds.push_back(i); // Sequential IDs + } + // Define the board using the layout and the dictionary + m_pimpl->board = cv::aruco::GridBoard::create(boardSize.width, + boardSize.height, + markerLength, + markerDistance, + m_pimpl->dictionary); + } + else { - markerIds.push_back(i); // Sequential IDs + m_pimpl->board = nullptr; // Ensure board is null if not using it } - // Define the board using the layout and the dictionary - m_pimpl->board = cv::aruco::GridBoard::create(boardSize.width, boardSize.height, markerLength, markerDistance, m_pimpl->dictionary); - m_pimpl->initialized = true; return true; @@ -232,19 +261,23 @@ bool ArucoDetector::advance() m_pimpl->resetBuffers(); - // Detect markers cv::aruco::detectMarkers(m_pimpl->currentImg, m_pimpl->dictionary, m_pimpl->currentDetectedMarkerCorners, m_pimpl->currentDetectedMarkerIds); - if (m_pimpl->currentDetectedMarkerIds.size() > 0) + if (m_pimpl->isBoard) { - // Estimate board pose (assuming you have an ArUco board) + if (m_pimpl->board == nullptr) { + log()->error("{} Board is enabled, but board object is null. This should not happen. Check initialize method.", printPrefix); + return false; // Or handle the error in another appropriate way. + } + + // Estimate board pose cv::Vec3d rvec, tvec; int valid_corners = cv::aruco::estimatePoseBoard(m_pimpl->currentDetectedMarkerCorners, m_pimpl->currentDetectedMarkerIds, - m_pimpl->board, // Assuming you have board object + m_pimpl->board, m_pimpl->cameraMatrix, m_pimpl->distCoeff, rvec, @@ -261,44 +294,50 @@ bool ArucoDetector::advance() m_pimpl->poseEig = toEigenPose(m_pimpl->Reig, m_pimpl->teig); // Store the board's pose as marker id 0 - ArucoMarkerData markerData{0, + ArucoData markerData{0, {}, // No corners for board m_pimpl->poseEig}; m_pimpl->out.markers[0] = markerData; } else { - // Board not detected (or not enough markers) - // Estimate pose of single markers - cv::aruco::estimatePoseSingleMarkers(m_pimpl->currentDetectedMarkerCorners, - m_pimpl->markerLength, - m_pimpl->cameraMatrix, - m_pimpl->distCoeff, - m_pimpl->currentDetectedMarkersRotVecs, - m_pimpl->currentDetectedMarkersTransVecs); - - for (std::size_t idx = 0; idx < m_pimpl->currentDetectedMarkerIds.size(); idx++) - { - cv::Rodrigues(m_pimpl->currentDetectedMarkersRotVecs[idx], m_pimpl->R); - cv::cv2eigen(m_pimpl->R, m_pimpl->Reig); - m_pimpl->teig << m_pimpl->currentDetectedMarkersTransVecs[idx](0), - m_pimpl->currentDetectedMarkersTransVecs[idx](1), - m_pimpl->currentDetectedMarkersTransVecs[idx](2); - - m_pimpl->poseEig = toEigenPose(m_pimpl->Reig, m_pimpl->teig); - ArucoMarkerData markerData{m_pimpl->currentDetectedMarkerIds[idx], - m_pimpl->currentDetectedMarkerCorners[idx], - m_pimpl->poseEig}; - m_pimpl->out.markers[m_pimpl->currentDetectedMarkerIds[idx]] = markerData; - } + log()->debug("{} Board not detected (or not enough markers). Falling back to single marker estimation.", printPrefix); + m_pimpl->estimateSingleMarkersPose(); } - - m_pimpl->out.timeNow = m_pimpl->currentTime; + } else { + // isBoard is false, so estimate single marker poses + m_pimpl->estimateSingleMarkersPose(); } + m_pimpl->out.timeNow = m_pimpl->currentTime; return true; } +void ArucoDetector::Impl::estimateSingleMarkersPose() { + // Estimate pose of single markers + cv::aruco::estimatePoseSingleMarkers(currentDetectedMarkerCorners, + markerLength, + cameraMatrix, + distCoeff, + currentDetectedMarkersRotVecs, + currentDetectedMarkersTransVecs); + + for (std::size_t idx = 0; idx < currentDetectedMarkerIds.size(); idx++) + { + cv::Rodrigues(currentDetectedMarkersRotVecs[idx], R); + cv::cv2eigen(R, Reig); + teig << currentDetectedMarkersTransVecs[idx](0), + currentDetectedMarkersTransVecs[idx](1), + currentDetectedMarkersTransVecs[idx](2); + + poseEig = toEigenPose(Reig, teig); + ArucoData markerData{currentDetectedMarkerIds[idx], + currentDetectedMarkerCorners[idx], + poseEig}; + out.markers[currentDetectedMarkerIds[idx]] = markerData; + } +} + const ArucoDetectorOutput& ArucoDetector::getOutput() const { return m_pimpl->out; @@ -314,7 +353,7 @@ bool ArucoDetector::isOutputValid() const return true; } -bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoMarkerData& markerData) +bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoData& markerData) { if (m_pimpl->out.markers.find(id) == m_pimpl->out.markers.end()) { @@ -345,7 +384,8 @@ bool ArucoDetector::getImageWithDetectedMarkers(cv::Mat& outputImg, m_pimpl->currentDetectedMarkerCorners, m_pimpl->currentDetectedMarkerIds); - if (drawFrames && m_pimpl->out.markers.find(0) != m_pimpl->out.markers.end()) // Assuming board ID is 0 + if (drawFrames && m_pimpl->out.markers.find(0) != m_pimpl->out.markers.end()) // Assuming board + // ID is 0 { // Draw coordinate axes on the board cv::Vec3d rvec, tvec; @@ -353,25 +393,25 @@ bool ArucoDetector::getImageWithDetectedMarkers(cv::Mat& outputImg, // Convert Eigen pose back to OpenCV rvec and tvec Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity(); Eigen::Vector3d translationVector = Eigen::Vector3d::Zero(); - if (m_pimpl->out.markers.count(0)) { - rotationMatrix = m_pimpl->out.markers.at(0).pose.block<3,3>(0,0); - translationVector = m_pimpl->out.markers.at(0).pose.block<3,1>(0,3); + if (m_pimpl->out.markers.count(0)) + { + rotationMatrix = m_pimpl->out.markers.at(0).pose.block<3, 3>(0, 0); + translationVector = m_pimpl->out.markers.at(0).pose.block<3, 1>(0, 3); } - cv::eigen2cv(rotationMatrix, m_pimpl->R); // Convert Eigen rotation to OpenCV Mat - cv::Rodrigues(m_pimpl->R, rvec); // Convert rotation matrix to rotation vector + cv::Rodrigues(m_pimpl->R, rvec); // Convert rotation matrix to rotation vector tvec[0] = translationVector(0); tvec[1] = translationVector(1); tvec[2] = translationVector(2); cv::drawFrameAxes(outputImg, - m_pimpl->cameraMatrix, - m_pimpl->distCoeff, - rvec, // rotation vector - tvec, // translation vector - axisLengthForDrawing); + m_pimpl->cameraMatrix, + m_pimpl->distCoeff, + rvec, // rotation vector + tvec, // translation vector + axisLengthForDrawing); } return true; diff --git a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp index f7d83c8164..6d21613ea9 100644 --- a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp +++ b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp @@ -24,6 +24,7 @@ using namespace BipedalLocomotion::ParametersHandler; TEST_CASE("Aruco Detector - Live Camera Feed with Boards") { std::shared_ptr parameterHandler = std::make_shared(); + parameterHandler->setParameter("is_board", true); parameterHandler->setParameter("marker_dictionary", "6X6_250"); parameterHandler->setParameter("marker_length", 0.1); // Reduced marker length to test with small markers parameterHandler->setParameter("markers_x", 5); // Number of markers in X direction @@ -44,7 +45,7 @@ TEST_CASE("Aruco Detector - Live Camera Feed with Boards") cv::namedWindow("Aruco Detection - Live with Boards", cv::WINDOW_AUTOSIZE); auto startTime = std::chrono::steady_clock::now(); - auto endTime = startTime + std::chrono::seconds(10); // Run for 10 seconds + auto endTime = startTime + std::chrono::seconds(20); // Run for 10 seconds while (std::chrono::steady_clock::now() < endTime) { @@ -62,7 +63,7 @@ TEST_CASE("Aruco Detector - Live Camera Feed with Boards") } // Check for board detection - ArucoMarkerData boardData; + ArucoData boardData; bool boardDetected = detector.getDetectedMarkerData(0, boardData); // Check for board with id 0 if (boardDetected) { From 4a5654d2608f59a29a683112cfad0c713af71ba4 Mon Sep 17 00:00:00 2001 From: mebbaid Date: Thu, 6 Feb 2025 11:49:54 +0100 Subject: [PATCH 4/5] use optional params --- src/Perception/src/ArucoDetector.cpp | 93 +++++++++++++++++++--------- 1 file changed, 63 insertions(+), 30 deletions(-) diff --git a/src/Perception/src/ArucoDetector.cpp b/src/Perception/src/ArucoDetector.cpp index 916817ddf4..46edf35b79 100644 --- a/src/Perception/src/ArucoDetector.cpp +++ b/src/Perception/src/ArucoDetector.cpp @@ -11,6 +11,36 @@ using namespace BipedalLocomotion::Conversions; #include #include +#ifndef __cpp_lib_optional // Check if std::optional is available +#include // For std::unique_ptr + +template +class Optional +{ +public: + Optional() : m_hasValue(false) {} + Optional(const T& value) : m_value(value), m_hasValue(true) {} + + bool has_value() const { return m_hasValue; } + T value() const + { + if (!m_hasValue) + { + throw std::runtime_error("Accessing an empty Optional value"); + } + return m_value; + } + +private: + bool m_hasValue; + T m_value; +}; + +#else // std::optional is available +#include +using Optional = std::optional; +#endif + class ArucoDetector::Impl { public: @@ -46,6 +76,11 @@ class ArucoDetector::Impl Eigen::Vector3d teig; /**< placeholder translation vector as Eigen object*/ Eigen::Matrix4d poseEig; /**< placeholder pose matrix as Eigen object*/ + // Optional board parameters + Optional markersX; + Optional markersY; + Optional markerSeparation; + /** * Utility map for choosing Aruco marker dictionary depending on user parameter */ @@ -118,7 +153,6 @@ bool ArucoDetector::initialize(std::weak_ptr handler) } // In OpenCV 4.7.0 getPredefinedDictionary started returning a cv::aruco::Dictionary -// instead of a cv::Ptr #if (CV_VERSION_MAJOR >= 5) || (CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7) m_pimpl->dictionary = cv::makePtr(); *(m_pimpl->dictionary) @@ -169,40 +203,39 @@ bool ArucoDetector::initialize(std::weak_ptr handler) m_pimpl->distCoeff = cv::Mat(5, 1, CV_64F); cv::eigen2cv(distCoeffVec, m_pimpl->distCoeff); - // Board configuration parameters - if (m_pimpl->isBoard) + // Read optional board parameters + int markersX; + int markersY; + double markerSeparation; + + if (handle->getParameter("markers_x", markersX)) { - int markersX; - int markersY; - double markerSeparation; + m_pimpl->markersX = markersX; + } - if (!handle->getParameter("markers_x", markersX)) - { - log()->error("{} The parameter handler could not find \"markers_x\" in the " - "configuration file, required for board detection.", - printPrefix); - return false; - } - if (!handle->getParameter("markers_y", markersY)) - { - log()->error("{} The parameter handler could not find \"markers_y\" in the " - "configuration file, required for board detection.", - printPrefix); - return false; - } + if (handle->getParameter("markers_y", markersY)) + { + m_pimpl->markersY = markersY; + } - if (!handle->getParameter("marker_separation", markerSeparation)) + if (handle->getParameter("marker_separation", markerSeparation)) + { + m_pimpl->markerSeparation = markerSeparation; + } + + // Board configuration + if (m_pimpl->isBoard) + { + if (!m_pimpl->markersX.has_value() || !m_pimpl->markersY.has_value() || !m_pimpl->markerSeparation.has_value()) { - log()->error("{} The parameter handler could not find \"marker_separation\" in the " - "configuration file, required for board detection.", - printPrefix); + log()->error("{} is_board is true, but not all board parameters (markers_x, markers_y, marker_separation) were provided.", printPrefix); return false; } // Create the board object - cv::Size boardSize(markersX, markersY); + cv::Size boardSize(m_pimpl->markersX.value(), m_pimpl->markersY.value()); float markerLength = m_pimpl->markerLength; - float markerDistance = markerSeparation; // distance in meters + float markerDistance = m_pimpl->markerSeparation.value(); // distance in meters // Create the board layout (assuming sequential IDs) std::vector markerIds; @@ -294,10 +327,10 @@ bool ArucoDetector::advance() m_pimpl->poseEig = toEigenPose(m_pimpl->Reig, m_pimpl->teig); // Store the board's pose as marker id 0 - ArucoData markerData{0, + ArucoData boardData{0, {}, // No corners for board m_pimpl->poseEig}; - m_pimpl->out.markers[0] = markerData; + m_pimpl->out.markers[0] = boardData; } else { @@ -353,14 +386,14 @@ bool ArucoDetector::isOutputValid() const return true; } -bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoData& markerData) +bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoData& Data) { if (m_pimpl->out.markers.find(id) == m_pimpl->out.markers.end()) { return false; } - markerData = m_pimpl->out.markers.at(id); + Data = m_pimpl->out.markers.at(id); return true; } From 5f1a76e0f1944b72a5ac5c9bf47e52cfa474e294 Mon Sep 17 00:00:00 2001 From: ergocub Date: Tue, 12 Aug 2025 18:36:35 +0200 Subject: [PATCH 5/5] fix compilation error --- .../Perception/Features/ArucoDetector.h | 2 ++ src/Perception/src/ArucoDetector.cpp | 15 ++++++++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h index 87780b45ba..7d45316c47 100644 --- a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h +++ b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h @@ -14,6 +14,8 @@ #include #include #include // Include for Board definition +#include + #include #include diff --git a/src/Perception/src/ArucoDetector.cpp b/src/Perception/src/ArucoDetector.cpp index 46edf35b79..33e24c0078 100644 --- a/src/Perception/src/ArucoDetector.cpp +++ b/src/Perception/src/ArucoDetector.cpp @@ -244,11 +244,16 @@ bool ArucoDetector::initialize(std::weak_ptr handler) markerIds.push_back(i); // Sequential IDs } // Define the board using the layout and the dictionary - m_pimpl->board = cv::aruco::GridBoard::create(boardSize.width, - boardSize.height, - markerLength, - markerDistance, - m_pimpl->dictionary); + // m_pimpl->board = cv::aruco::GridBoard::create(boardSize.width, + // boardSize.height, + // markerLength, + // markerDistance, + // m_pimpl->dictionary); + m_pimpl->board = cv::makePtr( + cv::Size(boardSize.width, boardSize.height), + markerLength, + markerDistance, + *m_pimpl->dictionary); } else {