[Perception] avoid memcpy in ArucoDetector and fix docs

ami-iit · Mar 23, 2021 · daa3310 · daa3310
1 parent bc81352
commit daa3310
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 18 deletions.
diff --git a/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h b/src/Perception/include/BipedalLocomotion/Perception/Features/ArucoDetector.h
@@ -85,9 +85,11 @@ class ArucoDetector : public System::Advanceable<ArucoDetectorOutput>
     bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
 
     /**
-     * Set image for which markers need to be detecte
+     * 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);
@@ -119,9 +121,9 @@ class ArucoDetector : public System::Advanceable<ArucoDetectorOutput>
      * @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 getImgWithDetectedMarkers(cv::Mat& outputImg,
-                                   const bool& drawFrames = false, 
-                                   const double& axisLengthForDrawing = 0.1);
+    bool getImageWithDetectedMarkers(cv::Mat& outputImg,
+                                     const bool& drawFrames = false,
+                                     const double& axisLengthForDrawing = 0.1);
 
     /**
      * Determines the validity of the object retrieved with get()

diff --git a/src/Perception/src/ArucoDetector.cpp b/src/Perception/src/ArucoDetector.cpp
@@ -120,22 +120,21 @@ bool ArucoDetector::initialize(std::weak_ptr<IParametersHandler> handler)
         return false;
     }
 
-    std::vector<double> calibVec;
-    if (!handle->getParameter("camera_matrix",
-                               GenericContainer::make_vector(calibVec, GenericContainer::VectorResizeMode::Resizable)))
+    Eigen::Matrix<double, 9, 1> calibVec;
+    if (!handle->getParameter("camera_matrix", calibVec))
     {
         log()->error("{} "
         "The parameter handler could not find \" camera_matrix \" in the configuration file.",
         printPrefix);
         return false;
     }
 
+    Eigen::Matrix3d calibMat = Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(calibVec.data());
     m_pimpl->cameraMatrix = cv::Mat(3, 3, CV_64F);
-    std::memcpy(m_pimpl->cameraMatrix.data, calibVec.data(), calibVec.size()*sizeof(double));
+    cv::eigen2cv(calibMat, m_pimpl->cameraMatrix);
 
-    std::vector<double> distCoeffVec;
-    if (!handle->getParameter("distortion_coefficients",
-                               GenericContainer::make_vector(distCoeffVec, GenericContainer::VectorResizeMode::Resizable)))
+    Eigen::Matrix<double, 5, 1> distCoeffVec;
+    if (!handle->getParameter("distortion_coefficients", distCoeffVec))
     {
         log()->error("{} "
         "The parameter handler could not find \" distortion_coefficients \" in the configuration file.",
@@ -144,7 +143,7 @@ bool ArucoDetector::initialize(std::weak_ptr<IParametersHandler> handler)
     }
 
     m_pimpl->distCoeff = cv::Mat(5, 1, CV_64F);
-    std::memcpy(m_pimpl->distCoeff.data, distCoeffVec.data(), distCoeffVec.size()*sizeof(double));
+    cv::eigen2cv(distCoeffVec, m_pimpl->distCoeff);
 
     m_pimpl->initialized = true;
     return true;
@@ -250,9 +249,9 @@ bool ArucoDetector::getDetectedMarkerData(const int& id, ArucoMarkerData& marker
 }
 
 
-bool ArucoDetector::getImgWithDetectedMarkers(cv::Mat& outputImg,
-                                              const bool& drawFrames,
-                                              const double& axisLengthForDrawing)
+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)

diff --git a/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp b/src/Perception/tests/Perception/Features/ArucoDetectorTest.cpp
@@ -36,15 +36,17 @@ TEST_CASE("Aruco Detector")
     REQUIRE(detector.advance());
 
     cv::Mat outputImage;
-    REQUIRE(detector.getImgWithDetectedMarkers(outputImage, /*drawFrames=*/ true, /*axisLengthForDrawing=*/ 0.3));
+    REQUIRE(detector.getImageWithDetectedMarkers(outputImage,
+                                                 /*drawFrames=*/ true,
+                                                 /*axisLengthForDrawing=*/ 0.3));
 
     // Marker 2 is detected in the sample image
     ArucoMarkerData marker2;
     REQUIRE(detector.getDetectedMarkerData(/*id=*/ 2, marker2));
     REQUIRE(marker2.id == 2);
 
-    /* // uncomment this block to view the output image
+     // uncomment this block to view the output image
        cv::imshow("outputImage", outputImage);
        cv::waitKey();
-    */
+
 }