Merge branch '111-aruco_tests_examples'

roboticslab-uc3m · Jul 23, 2024 · 8fb126f · 8fb126f
2 parents b04c1b0 + efb5036
commit 8fb126f
Show file tree

Hide file tree

Showing 9 changed files with 293 additions and 1 deletion.
diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt
@@ -2,6 +2,7 @@ option(ENABLE_examples "Enable/disable C++ examples" OFF)
 
 if(ENABLE_examples)
     if(TARGET ROBOTICSLAB::VisionInterfaces)
+        add_subdirectory(exampleArucoDetector)
         add_subdirectory(exampleColorRegionDetector)
         add_subdirectory(exampleDnnDetector)
         add_subdirectory(exampleHaarDetector)

diff --git a/examples/cpp/exampleArucoDetector/CMakeLists.txt b/examples/cpp/exampleArucoDetector/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.16)
+
+project(exampleArucoDetector LANGUAGES CXX)
+
+if(NOT YARP_FOUND)
+    find_package(YARP 3.8 REQUIRED COMPONENTS os dev sig)
+endif()
+
+if(NOT TARGET ROBOTICSLAB::VisionInterfaces)
+    find_package(ROBOTICSLAB_VISION REQUIRED)
+endif()
+
+add_executable(exampleArucoDetector exampleArucoDetector.cpp)
+
+target_link_libraries(exampleArucoDetector YARP::YARP_os
+                                           YARP::YARP_init
+                                           YARP::YARP_dev
+                                           YARP::YARP_sig
+                                           ROBOTICSLAB::VisionInterfaces)
diff --git a/examples/cpp/exampleArucoDetector/exampleArucoDetector.cpp b/examples/cpp/exampleArucoDetector/exampleArucoDetector.cpp
@@ -0,0 +1,72 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+/**
+ * @ingroup vision_examples
+ * @defgroup exampleArucoDetector exampleArucoDetector
+ * @brief exampleArucoDetector
+ */
+
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Property.h>
+#include <yarp/os/ResourceFinder.h>
+
+#include <yarp/dev/PolyDriver.h>
+
+#include <yarp/sig/Image.h>
+#include <yarp/sig/ImageFile.h>
+
+#include <IDetector.hpp>
+
+int main(int argc, char const *argv[])
+{
+    yarp::os::Property detectorOptions {{"device", yarp::os::Value("ArucoDetector")}};
+    yarp::dev::PolyDriver detectorDevice(detectorOptions);
+    roboticslab::IDetector *iDetector;
+
+    if (!detectorDevice.isValid() || !detectorDevice.view(iDetector))
+    {
+        yError() << "Device not available";
+        return 1;
+    }
+
+    yarp::os::ResourceFinder rf;
+    rf.setDefaultContext("ArucoDetector");
+    std::string qrFullName = rf.findFileByName("tests/aruco.png");
+
+    yarp::sig::ImageOf<yarp::sig::PixelRgb> yarpImgRgb;
+
+    if (!yarp::sig::file::read(yarpImgRgb, qrFullName, yarp::sig::file::FORMAT_PNG))
+    {
+        yError() << "Image file not available";
+        return 1;
+    }
+
+    yInfo() << "detect()";
+
+    yarp::os::Bottle detectedObjects;
+
+    if (!iDetector->detect(yarpImgRgb, detectedObjects))
+    {
+        yError() << "Detector failed";
+        return 1;
+    }
+
+    for (auto i = 0; i < detectedObjects.size(); i++)
+    {
+        const auto * detectedObject = detectedObjects.get(i).asDict();
+
+        auto tlX = detectedObject->find("tlx").asFloat32();
+        auto tlY = detectedObject->find("tly").asFloat32();
+        auto trX = detectedObject->find("trx").asFloat32();
+        auto trY = detectedObject->find("try").asFloat32();
+        auto blX = detectedObject->find("blx").asFloat32();
+        auto blY = detectedObject->find("bly").asFloat32();
+        auto brX = detectedObject->find("brx").asFloat32();
+        auto brY = detectedObject->find("bry").asFloat32();
+        auto text = detectedObject->find("text").asInt32();
+
+        yInfo("aruco%d [[%f,%f],[%f,%f],[%f,%f],[%f,%f]]: \"%d\"", i, tlX, tlY, trX, trY, brX, brY, blX, blY, text);
+    }
+
+    return 0;
+}
diff --git a/examples/python/exampleArucoDetector.py b/examples/python/exampleArucoDetector.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+
+import yarp
+import roboticslab_vision
+
+detectorOptions = yarp.Property()
+detectorOptions.put("device", "ArucoDetector")
+detectorDevice = yarp.PolyDriver(detectorOptions)
+
+if not detectorDevice.isValid():
+    print("Device not available")
+    raise SystemExit
+
+iDetector = roboticslab_vision.viewIDetector(detectorDevice)
+
+rf = yarp.ResourceFinder()
+rf.setDefaultContext("ArucoDetector")
+arucoFullName = rf.findFileByName("tests/aruco.png")
+yarpImgRgb = yarp.ImageRgb()
+
+if not yarp.read(yarpImgRgb, arucoFullName, yarp.FORMAT_PNG):
+    print("Image file not available")
+    raise SystemExit
+
+print("detect()")
+detectedObjects = yarp.Bottle()
+
+if not iDetector.detect(yarpImgRgb, detectedObjects):
+    print("Detector failed")
+    raise SystemExit
+
+for i in range(detectedObjects.size()):
+    tlX = detectedObjects.get(i).asDict().find("tlx").asFloat32()
+    tlY = detectedObjects.get(i).asDict().find("tly").asFloat32()
+    trX = detectedObjects.get(i).asDict().find("trx").asFloat32()
+    trY = detectedObjects.get(i).asDict().find("try").asFloat32()
+    brX = detectedObjects.get(i).asDict().find("brx").asFloat32()
+    brY = detectedObjects.get(i).asDict().find("bry").asFloat32()
+    blX = detectedObjects.get(i).asDict().find("blx").asFloat32()
+    blY = detectedObjects.get(i).asDict().find("bly").asFloat32()
+    text = detectedObjects.get(i).asDict().find("text").asInt32()
+
+    print("aruco%d [[%f,%f],[%f,%f],[%f,%f],[%f,%f]]: \"%d\"" % (i, tlX, tlY, trX, trY, brX, brY, blX, blY, text))
diff --git a/libraries/YarpPlugins/ArucoDetector/ArucoDetector.cpp b/libraries/YarpPlugins/ArucoDetector/ArucoDetector.cpp
@@ -70,7 +70,8 @@ bool ArucoDetector::detect(const yarp::sig::Image& inYarpImg, yarp::os::Bottle&
             {"brx", yarp::os::Value(br.x)},
             {"bry", yarp::os::Value(br.y)},
             {"blx", yarp::os::Value(bl.x)},
-            {"bly", yarp::os::Value(bl.y)}
+            {"bly", yarp::os::Value(bl.y)},
+            {"text", yarp::os::Value(markerIds[i])}
         };
     }
 

diff --git a/share/CMakeLists.txt b/share/CMakeLists.txt
@@ -37,6 +37,13 @@ if(ENABLE_QrDetector)
     endif()
 endif()
 
+if(ENABLE_ArucoDetector)
+    if(ENABLE_tests)
+        yarp_install(DIRECTORY contexts/ArucoDetector/tests
+                     DESTINATION ${ROBOTICSLAB-VISION_CONTEXTS_INSTALL_DIR}/ArucoDetector)
+    endif()
+endif()
+
 if(ENABLE_sceneReconstruction)
     yarp_install(DIRECTORY contexts/sceneReconstruction
                  DESTINATION ${ROBOTICSLAB-VISION_CONTEXTS_INSTALL_DIR})

diff --git a/share/contexts/ArucoDetector/tests/aruco.png b/share/contexts/ArucoDetector/tests/aruco.png
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -10,6 +10,18 @@ if(ENABLE_tests)
     add_subdirectory(${GTestSources_SOURCE_DIR} ${CMAKE_BINARY_DIR}/googletest)
     include_directories(${GTestSources_INCLUDE_DIR})
 
+    if(ENABLE_ArucoDetector)
+        add_executable(testArucoDetector testArucoDetector.cpp)
+
+        target_link_libraries(testArucoDetector YARP::YARP_os
+                                                YARP::YARP_dev
+                                                YARP::YARP_sig
+                                                ROBOTICSLAB::VisionInterfaces
+                                                gtest_main)
+
+        gtest_discover_tests(testArucoDetector)
+    endif()
+
     if(ENABLE_ColorRegionDetector)
         add_executable(testColorRegionDetector testColorRegionDetector.cpp)
 

diff --git a/tests/testArucoDetector.cpp b/tests/testArucoDetector.cpp
@@ -0,0 +1,137 @@
+#include "gtest/gtest.h"
+
+#include <unordered_set>
+
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Property.h>
+#include <yarp/os/ResourceFinder.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/sig/Image.h>
+#include <yarp/sig/ImageFile.h>
+
+#include "IDetector.hpp"
+
+namespace roboticslab::test
+{
+
+struct PropertyHasher
+{
+    std::size_t operator()(const yarp::os::Property & prop) const
+    {
+        return std::hash<std::string>()(prop.toString());
+    }
+};
+
+struct PropertyComparer
+{
+    bool operator()(const yarp::os::Property & lhs, const yarp::os::Property & rhs) const
+    {
+        return lhs.toString() == rhs.toString();
+    }
+};
+
+/**
+ * @ingroup vision_tests
+ * @brief Tests @ref ArucoDetector
+ */
+class ArucoDetectorTest : public testing::Test
+{
+public:
+    void SetUp() override
+    {
+        yarp::os::Property deviceOptions {{"device", yarp::os::Value("ArucoDetector")}};
+
+        if (!detectorDevice.open(deviceOptions))
+        {
+            yError() << "Failed to open ArucoDetector device";
+            return;
+        }
+
+        if (!detectorDevice.view(iDetector))
+        {
+            yError() << "Problems acquiring detector interface";
+            return;
+        }
+    }
+
+    void TearDown() override
+    {
+    }
+
+protected:
+    roboticslab::IDetector * iDetector;
+    yarp::dev::PolyDriver detectorDevice;
+    static std::unordered_set<yarp::os::Property, PropertyHasher, PropertyComparer> expectedValues;
+};
+
+decltype(ArucoDetectorTest::expectedValues) ArucoDetectorTest::expectedValues = {
+    {
+        {"tlx", yarp::os::Value(431.0)},
+        {"tly", yarp::os::Value(1164.0)},
+        {"trx", yarp::os::Value(904.0)},
+        {"try", yarp::os::Value(1165.0)},
+        {"brx", yarp::os::Value(903.0)},
+        {"bry", yarp::os::Value(1637.0)},
+        {"blx", yarp::os::Value(430.0)},
+        {"bly", yarp::os::Value(1636.0)},
+        {"text", yarp::os::Value(5)}
+    },
+    {
+        {"tlx", yarp::os::Value(1301.0)},
+        {"tly", yarp::os::Value(515.0)},
+        {"trx", yarp::os::Value(2341.0)},
+        {"try", yarp::os::Value(515.0)},
+        {"brx", yarp::os::Value(2340.0)},
+        {"bry", yarp::os::Value(1557.0)},
+        {"blx", yarp::os::Value(1300.0)},
+        {"bly", yarp::os::Value(1555.0)},
+        {"text", yarp::os::Value(20)}
+    },
+    {
+        {"tlx", yarp::os::Value(334.0)},
+        {"tly", yarp::os::Value(298.0)},
+        {"trx", yarp::os::Value(997.0)},
+        {"try", yarp::os::Value(299.0)},
+        {"brx", yarp::os::Value(996.0)},
+        {"bry", yarp::os::Value(962.0)},
+        {"blx", yarp::os::Value(333.0)},
+        {"bly", yarp::os::Value(961.0)},
+        {"text", yarp::os::Value(10)}
+    }
+};
+
+TEST_F(ArucoDetectorTest, ArucoDetector1)
+{
+    yarp::sig::ImageOf<yarp::sig::PixelRgb> yarpImgRgb;
+    yarpImgRgb.resize(300, 200);
+
+    yarp::os::Bottle detectedObjects;
+    ASSERT_TRUE(iDetector->detect(yarpImgRgb, detectedObjects));
+    ASSERT_EQ(detectedObjects.size(), 0);
+}
+
+TEST_F(ArucoDetectorTest, ArucoDetector2)
+{
+    yarp::os::ResourceFinder rf;
+    rf.setVerbose(false);
+    rf.setDefaultContext("ArucoDetector");
+    std::string arucoFullName = rf.findFileByName("tests/aruco.png");
+    ASSERT_FALSE(arucoFullName.empty());
+
+    yarp::sig::ImageOf<yarp::sig::PixelRgb> yarpImgRgb;
+    ASSERT_TRUE(yarp::sig::file::read(yarpImgRgb, arucoFullName, yarp::sig::file::FORMAT_PNG));
+
+    yarp::os::Bottle detectedObjects;
+    ASSERT_TRUE(iDetector->detect(yarpImgRgb, detectedObjects));
+
+    ASSERT_GE(detectedObjects.size(), 1);
+    ASSERT_LE(detectedObjects.size(), 3);
+
+    for (auto i = 0; i < detectedObjects.size(); i++)
+    {
+        const auto * detectedObject = detectedObjects.get(i).asDict();
+        ASSERT_TRUE(expectedValues.find(*detectedObject) != expectedValues.end());
+    }
+}
+
+} // namespace roboticslab::test