diff --git a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h index f8295e1bae..7d45316c47 100644 --- a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h +++ b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h @@ -13,6 +13,9 @@ #include #include +#include // Include for Board definition +#include + #include #include @@ -25,15 +28,20 @@ namespace Perception /** * Aruco marker identifiers */ -struct ArucoMarkerData +struct ArucoData { /** * 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 +51,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,7 +63,12 @@ struct ArucoMarkerData */ struct ArucoDetectorOutput { - std::unordered_map markers; + /** + * Map of detected markers/boards. + * Key: Marker ID (or special ID for the board) + * Value: ArucoData for the corresponding marker/board + */ + std::unordered_map markers; double timeNow{-1.0}; }; @@ -78,6 +91,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 +104,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 +120,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); + bool getDetectedMarkerData(const int& id, ArucoData& arucoData); /** - * 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 +159,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..33e24c0078 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,10 +8,39 @@ using namespace BipedalLocomotion::GenericContainer; using namespace BipedalLocomotion::Perception; using namespace BipedalLocomotion::Conversions; -#include #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: @@ -28,10 +50,12 @@ 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*/ 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 */ @@ -52,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 */ @@ -79,6 +108,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() @@ -121,14 +153,22 @@ 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) = 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 " @@ -163,6 +203,63 @@ bool ArucoDetector::initialize(std::weak_ptr handler) m_pimpl->distCoeff = cv::Mat(5, 1, CV_64F); cv::eigen2cv(distCoeffVec, m_pimpl->distCoeff); + // Read optional board parameters + int markersX; + int markersY; + double markerSeparation; + + if (handle->getParameter("markers_x", markersX)) + { + m_pimpl->markersX = markersX; + } + + if (handle->getParameter("markers_y", markersY)) + { + m_pimpl->markersY = markersY; + } + + 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("{} 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(m_pimpl->markersX.value(), m_pimpl->markersY.value()); + float markerLength = m_pimpl->markerLength; + float markerDistance = m_pimpl->markerSeparation.value(); // 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->board = cv::makePtr( + cv::Size(boardSize.width, boardSize.height), + markerLength, + markerDistance, + *m_pimpl->dictionary); + } + else + { + m_pimpl->board = nullptr; // Ensure board is null if not using it + } + m_pimpl->initialized = true; return true; } @@ -201,41 +298,84 @@ bool ArucoDetector::advance() } m_pimpl->resetBuffers(); - std::vector> detectedmarkerCorners; + 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) { - 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++) + 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, + 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 + ArucoData boardData{0, + {}, // No corners for board m_pimpl->poseEig}; - m_pimpl->out.markers[m_pimpl->currentDetectedMarkerIds[idx]] = markerData; + m_pimpl->out.markers[0] = boardData; + } + else + { + 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; @@ -251,14 +391,14 @@ bool ArucoDetector::isOutputValid() const return true; } -bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoMarkerData& 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; } @@ -266,8 +406,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 +422,34 @@ 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++) + // 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)) { - cv::drawFrameAxes(outputImg, - m_pimpl->cameraMatrix, - m_pimpl->distCoeff, - m_pimpl->currentDetectedMarkersRotVecs[idx], - m_pimpl->currentDetectedMarkersTransVecs[idx], - axisLengthForDrawing); + 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, + rvec, // rotation vector + tvec, // translation vector + axisLengthForDrawing); } return true; @@ -307,4 +463,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..6d21613ea9 100644 --- a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp +++ b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp @@ -14,40 +14,76 @@ #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("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 + 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}); + 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(20); // 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 + ArucoData 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