Add PerceptionFeatures library and implement ArucoDetector class

CHANGELOG.md

@@ -18,8 +18,10 @@ All notable changes to this project are documented in this file.
 - Implement LinearizedFrictionCone class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/244)
 - Added a check on whether the installed public headers have the correct folder structure (https://github.com/dic-iit/bipedal-locomotion-framework/pull/247)
 - Implement python bindings for VariablesHandler class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/234)
+- Implement `PerceptionFeatures` library and implement `ArucoDetector`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/159)
 - Implement FixedBaseDynamics class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/242)
 
+
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)
 - Move all the ContactDetectors related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/209)

README.md

@@ -64,6 +64,7 @@ The **bipedal-locomotion-framework** project consists of several components. The
 |     [YarpUtilities](./src/YarpUtilities)     | Utilities library for retrieving data and from YARP structures |          [`YARP`](https://www.yarp.it/git-master/)           |
 | [PerceptionInterface](./src/RobotInterface)  | Generic interface classes to adapt to perception data formats like images and point clouds | [`OpenCV`](https://github.com/opencv/opencv) [`PCL`](https://github.com/PointCloudLibrary/pcl) |
 |    [PerceptionCapture](./src/Perception)     |   Library containing driver classes for perception devices   | [`OpenCV`](https://github.com/opencv/opencv) [`PCL`](https://github.com/PointCloudLibrary/pcl) [`realsense2`](https://github.com/IntelRealSense/librealsense) |
+|    [PerceptionFeatures](./src/Perception)     |   Library containing perception algorithms useful for locomotion   | [`OpenCV`](https://github.com/opencv/opencv) |
 
 
 # :hammer: Build the suite

cmake/AddBipedalLocomotionUnitTest.cmake

@@ -15,7 +15,7 @@ if (FRAMEWORK_RUN_Valgrind_tests)
     else ()
         set(MEMORYCHECK_SUPPRESSIONS "")
     endif ()
-    set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}"  CACHE STRING "Options to pass to the memory checker")
+    set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --gen-suppressions=all --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}"  CACHE STRING "Options to pass to the memory checker")
     mark_as_advanced(MEMORYCHECK_COMMAND_OPTIONS)
     set(MEMCHECK_COMMAND_COMPLETE "${MEMORYCHECK_COMMAND} ${MEMORYCHECK_COMMAND_OPTIONS}")
     separate_arguments(MEMCHECK_COMMAND_COMPLETE)

cmake/valgrind-linux.supp

@@ -37,3 +37,51 @@
    fun:_dl_open
    fun:dlopen_doit
 }
+
+{
+   <ubuntu-20.04-opencv-tbb>
+   Memcheck:Leak
+   fun:malloc
+   fun:_ZN3tbb8internal23allocate_via_handler_v3Em
+}
+
+{
+   <ubuntu-20.04-opencv-aruco>
+   Memcheck:Leak
+   match-leak-kinds: possible
+   fun:calloc
+   fun:allocate_dtv
+   fun:_dl_allocate_tls
+   fun:allocate_stack
+   fun:pthread_create@@GLIBC_2.2.5
+   fun:_ZN2cv12WorkerThreadC1ERNS_10ThreadPoolEj
+   fun:_ZN2cv10ThreadPool12reconfigure_Ej
+   fun:_ZN2cv21parallel_for_pthreadsERKNS_5RangeERKNS_16ParallelLoopBodyEd
+   fun:_ZN2cv13parallel_for_ERKNS_5RangeERKNS_16ParallelLoopBodyEd
+   fun:_ZN2cv3hal8opt_AVX212cvtBGRtoGrayEPKhmPhmiiiib
+   fun:_ZN2cv3hal12cvtBGRtoGrayEPKhmPhmiiiib
+   fun:_ZN2cv16cvtColorBGR2GrayERKNS_11_InputArrayERKNS_12_OutputArrayEb
+   fun:_ZN2cv8cvtColorERKNS_11_InputArrayERKNS_12_OutputArrayEii
+   fun:_ZN2cv5arucoL14_convertToGreyERKNS_11_InputArrayERKNS_12_OutputArrayE
+}
+
+{
+   <ubuntu-20.04-opencv-aruco-ci>
+   Memcheck:Leak
+   match-leak-kinds: possible
+   fun:calloc
+   fun:allocate_dtv
+   fun:_dl_allocate_tls
+   fun:allocate_stack
+   fun:pthread_create@@GLIBC_2.2.5
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.2
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.2
+   obj:/usr/lib/x86_64-linux-gnu/libopencv_core.so.4.2.0
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.2
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.2
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.2
+   obj:/usr/lib/x86_64-linux-gnu/libopencv_core.so.4.2.0
+   obj:/usr/lib/x86_64-linux-gnu/libopencv_core.so.4.2.0
+   fun:_ZNK3tbb10interface78internal15task_arena_base16internal_executeERNS1_13delegate_baseE
+}
+

src/Perception/CMakeLists.txt

@@ -2,13 +2,23 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-if(FRAMEWORK_COMPILE_Perception AND FRAMEWORK_COMPILE_RealsenseCapture)
-  set(H_PREFIX include/BipedalLocomotion/Perception/Capture)
+if(FRAMEWORK_COMPILE_Perception)
+  if(FRAMEWORK_COMPILE_RealsenseCapture)
+    set(H_PREFIX include/BipedalLocomotion/Perception/Capture)
+    add_bipedal_locomotion_library(
+      NAME                   PerceptionCapture
+      SOURCES                src/RealSense.cpp
+      PUBLIC_HEADERS         ${H_PREFIX}/RealSense.h
+      PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging BipedalLocomotion::PerceptionInterface ${realsense2_LIBRARY}
+      INSTALLATION_FOLDER    Perception/Capture)
+  endif()
+
+  set(H_PREFIX include/BipedalLocomotion/Perception/Features)
   add_bipedal_locomotion_library(
-    NAME                   PerceptionCapture
-    SOURCES                src/RealSense.cpp
-    PUBLIC_HEADERS         ${H_PREFIX}/RealSense.h
-    PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging BipedalLocomotion::PerceptionInterface ${realsense2_LIBRARY}
-    INSTALLATION_FOLDER    Perception/Capture)
+    NAME                   PerceptionFeatures
+    SOURCES                src/ArucoDetector.cpp
+    PUBLIC_HEADERS         ${H_PREFIX}/ArucoDetector.h
+    SUBDIRECTORIES         tests/Perception/Features
+    PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::GenericContainer BipedalLocomotion::CommonConversions BipedalLocomotion::System ${OpenCV_LIBS} Eigen3::Eigen BipedalLocomotion::TextLogging
+    INSTALLATION_FOLDER    Perception/Features)
 endif()
-

src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h

@@ -0,0 +1,143 @@
+/**
+ * @file ArucoDetector.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_PERECEPTION_FEATURES_ARUCO_H
+#define BIPEDAL_LOCOMOTION_PERECEPTION_FEATURES_ARUCO_H
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+#include <Eigen/Dense>
+#include <opencv2/core.hpp>
+
+#include <memory>
+#include <unordered_map>
+
+namespace BipedalLocomotion
+{
+namespace Perception
+{
+
+/**
+ * Aruco marker identifiers
+ */
+struct ArucoMarkerData
+{
+    /**
+     * Marker ID
+     */
+    int id{-1};
+
+    /**
+     * Marker corners in camera coordinates
+     * in the order
+     * - top left
+     * - top right
+     * - bottom right
+     * - bottom left
+     */
+    std::vector<cv::Point2f> corners;
+
+    /**
+     * Pose of the marker in camera frame
+     * cam_H_marker
+     */
+    Eigen::Matrix4d pose;
+};
+
+
+/**
+ * Aruco detector output
+ */
+struct ArucoDetectorOutput
+{
+    std::unordered_map<int, ArucoMarkerData> markers;
+    double timeNow{-1.0};
+};
+
+class ArucoDetector : public System::Advanceable<ArucoDetectorOutput>
+{
+public:
+    ArucoDetector();
+    ~ArucoDetector();
+
+    /**
+     * Initialize the detector
+     * @note The following parameter are required by the filter:
+     * - "marker_dictionary" OpenCV predefined aruco marker dictionary 
+     * (available options: "4X4_50", "4X4_100", "4X4_250", "4X4_1000",
+     *                     "5X5_50", "5X5_100", "5X5_250", "5X5_1000",
+     *                     "6X6_50", "6X6_100", "6X6_250", "6X6_1000",
+     *                     "7X7_50", "7X7_100", "7X7_250", "7X7_1000",
+     *                     "ARUCO_ORIGINAL")
+     * options coherent with https://docs.opencv.org/master/d9/d6a/group__aruco.html#gac84398a9ed9dd01306592dd616c2c975
+     * - "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
+     * @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.
+     */
+    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
+
+    /**
+     * Set image for which markers 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
+     *                    or time delay synchronization
+     * @return True in case of success, false otherwise
+     */
+    bool setImage(const cv::Mat& inputImg, double timeNow);
+
+    /**
+     * Compute one step of the detector
+     * @return True in case of success, false otherwise
+     */
+    bool advance() final;
+
+    /**
+     * Get the detected markers' data from the current step
+     * @return A struct containing a map container of detected markers.
+     */
+    const ArucoDetectorOutput& get() const final;
+
+    /**
+     * Get the detected marker data
+     * @param[in] id marker id
+     * @param[in] markerData detected marker identifiers data
+     * @return True in case of success, false if marker was not detected
+     */
+    bool getDetectedMarkerData(const int& id, ArucoMarkerData& markerData);
+
+    /**
+     * Get the image with drawn detected markers
+     * @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] axisLengthForDrawing axis length for drawing the frame axes, 0.1 by default
+     * @return True in case of success, false if no marker was detected
+     */
+    bool getImageWithDetectedMarkers(cv::Mat& outputImg,
+                                     const bool& drawFrames = false,
+                                     const double& axisLengthForDrawing = 0.1);
+
+    /**
+     * Determines the validity of the object retrieved with get()
+     * @return True if the object is valid, false otherwise.
+     */
+    bool isValid() const final;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> m_pimpl;
+};
+
+} // namespace Perception
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_PERECEPTION_FEATURES_ARUCO_H
+