Fully removed OpenCV dependency

zed_wrapper/CMakeLists.txt

@@ -27,11 +27,6 @@ if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX
     SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
 endif()
 
-find_package(OpenCV 3 COMPONENTS core highgui imgproc)
-checkPackage("OPENCV_CORE" "OpenCV core not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
-checkPackage("OPENCV_HIGHGUI" "OpenCV highgui not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
-checkPackage("OPENCV_IMGPROC" "OpenCV imgproc not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
-
 find_package(CUDA)
 checkPackage("CUDA" "CUDA not found, install it from:\n https://developer.nvidia.com/cuda-downloads")
 
@@ -85,7 +80,6 @@ catkin_package(
     rosconsole
     sensor_msgs
     stereo_msgs
-    opencv3
     image_transport
     dynamic_reconfigure
     tf2_ros
@@ -111,14 +105,12 @@ include_directories(
         ${catkin_INCLUDE_DIRS}
         ${CUDA_INCLUDE_DIRS}
         ${ZED_INCLUDE_DIRS}
-        ${OpenCV_INCLUDE_DIRS}
         ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/include
         ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/include
 )
 
 link_directories(${ZED_LIBRARY_DIR})
 link_directories(${CUDA_LIBRARY_DIRS})
-link_directories(${OpenCV_LIBRARY_DIRS})
 
 ###############################################################################
 
@@ -136,7 +128,6 @@ set(LINK_LIBRARIES
   ${catkin_LIBRARIES}
   ${ZED_LIBRARIES}
   ${CUDA_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED}
-  ${OpenCV_LIBS}
   )
 
 add_library(ZEDWrapper ${TOOLS_SRC} ${NODELET_SRC})

zed_wrapper/README.md

@@ -25,12 +25,10 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package
    - rosconsole
    - sensor_msgs
    - stereo_msgs
-   - opencv3
    - image_transport
    - dynamic_reconfigure
    - urdf
 
-
 Open a terminal and build the package:
 
     cd ~/catkin_ws/src

zed_wrapper/package.xml

@@ -17,7 +17,6 @@
   <depend>rosconsole</depend>
   <depend>sensor_msgs</depend>
   <depend>stereo_msgs</depend>
-  <depend>opencv3</depend>
   <depend>image_transport</depend>
   <depend>dynamic_reconfigure</depend>
   <depend>nodelet</depend>

zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp

@@ -42,8 +42,6 @@
 #include <zed_wrapper/set_initial_pose.h>
 #include <zed_wrapper/reset_odometry.h>
 
-#include <opencv2/core/core.hpp>
-
 #include <mutex>
 #include <thread>
 #include <condition_variable>
@@ -344,10 +342,6 @@ namespace zed_wrapper {
         int camHeight;
         int matWidth;
         int matHeight;
-        cv::Mat leftImRGB;
-        cv::Mat rightImRGB;
-        cv::Mat confImRGB;
-        cv::Mat confMapFloat;
 
         // Mutex
         std::mutex dataMutex;

zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp

@@ -20,8 +20,6 @@
 #include "zed_wrapper_nodelet.hpp"
 #include "sl_tools.h"
 
-#include <opencv2/imgproc/imgproc.hpp>
-
 #ifndef NDEBUG
 #include <ros/console.h>
 #endif
@@ -1053,8 +1051,8 @@ namespace zed_wrapper {
         }
 
         if (rawParam) {
-            cv::Mat R_ = sl_tools::convertRodrigues(zedParam.R);
-            float* p = (float*)R_.data;
+            std::vector<float> R_ = sl_tools::convertRodrigues(zedParam.R);
+            float* p = R_.data();
             for (int i = 0; i < 9; i++) {
                 right_cam_info_msg->R[i] = p[i];
             }
@@ -1291,20 +1289,12 @@ namespace zed_wrapper {
         matHeight = static_cast<int>(camHeight * matResizeFactor);
         NODELET_DEBUG_STREAM("Data Mat size : " << matWidth << "x" << matHeight);
 
-        cv::Size cvSize(matWidth, matWidth);
-        leftImRGB = cv::Mat(cvSize, CV_8UC3);
-        rightImRGB = cv::Mat(cvSize, CV_8UC3);
-        confImRGB = cv::Mat(cvSize, CV_8UC3);
-        confMapFloat = cv::Mat(cvSize, CV_32FC1);
-
         // Create and fill the camera information messages
         fillCamInfo(zed, leftCamInfoMsg, rightCamInfoMsg, leftCamOptFrameId,
                     rightCamOptFrameId);
         fillCamInfo(zed, leftCamInfoRawMsg, rightCamInfoRawMsg, leftCamOptFrameId,
                     rightCamOptFrameId, true);
-        rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg; // the reference camera is
-        // the Left one (next to the
-        // ZED logo)
+        rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg;
         rgbCamInfoRawMsg = leftCamInfoRawMsg;
 
         sl::RuntimeParameters runParams;
@@ -1523,8 +1513,6 @@ namespace zed_wrapper {
                                         matWidth, matHeight);
 
                     // Need to flip sign, but cause of this is not sure
-                    //cv::Mat disparity = sl_tools::toCVMat(disparityZEDMat) * -1.0;
-
 #pragma parallel for
                     for (int y = 0; y < disparityZEDMat.getHeight(); y++) {
 #pragma parallel for

zed_wrapper/src/tools/include/sl_tools.h

@@ -23,7 +23,6 @@
 
 #include <string>
 #include <sl/Camera.hpp>
-#include <opencv2/core/core.hpp>
 #include <ros/time.h>
 
 namespace sl_tools {
@@ -38,7 +37,7 @@ namespace sl_tools {
     */
     sl::DeviceProperties getZEDFromSN(unsigned int serial_number);
 
-    cv::Mat convertRodrigues(sl::float3 r);
+    std::vector<float> convertRodrigues(sl::float3 r);
 
     /* \brief Test if a file exist
     * \param name : the path to the file

zed_wrapper/src/tools/src/sl_tools.cpp

@@ -47,22 +47,32 @@ namespace sl_tools {
         return prop;
     }
 
-    cv::Mat convertRodrigues(sl::float3 r) {
-        double theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z);
-        cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
+    std::vector<float>  convertRodrigues(sl::float3 r) {
+        float theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z);
+
+        //cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
+        std::vector<float> R = {1.0f, 0.0f, 0.0f,
+                                0.0f, 1.0f, 0.0f,
+                                0.0f, 0.0f, 1.0f
+                               };
 
         if (theta < DBL_EPSILON) {
             return R;
         } else {
-            double c = cos(theta);
-            double s = sin(theta);
-            double c1 = 1. - c;
-            double itheta = theta ? 1. / theta : 0.;
+            float c = cos(theta);
+            float s = sin(theta);
+            float c1 = 1.f - c;
+            float itheta = theta ? 1.f / theta : 0.f;
 
             r *= itheta;
 
-            cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F);
-            float* p = (float*) rrt.data;
+            //cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F);
+            std::vector<float> rrt = {1.0f, 0.0f, 0.0f,
+                                      0.0f, 1.0f, 0.0f,
+                                      0.0f, 0.0f, 1.0f
+                                     };
+
+            float* p = rrt.data();
             p[0] = r.x * r.x;
             p[1] = r.x * r.y;
             p[2] = r.x * r.z;
@@ -73,8 +83,12 @@ namespace sl_tools {
             p[7] = r.y * r.z;
             p[8] = r.z * r.z;
 
-            cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F);
-            p = (float*) r_x.data;
+            //cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F);
+            std::vector<float> r_x = {1.0f, 0.0f, 0.0f,
+                                      0.0f, 1.0f, 0.0f,
+                                      0.0f, 0.0f, 1.0f
+                                     };
+            p = r_x.data();
             p[0] = 0;
             p[1] = -r.z;
             p[2] = r.y;
@@ -86,8 +100,28 @@ namespace sl_tools {
             p[8] = 0;
 
             // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
-            R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x;
+
+            sl::Matrix3f eye;
+            eye.setIdentity();
+
+            sl::Matrix3f sl_R(R.data());
+            sl::Matrix3f sl_rrt(rrt.data());
+            sl::Matrix3f sl_r_x(r_x.data());
+
+            //R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x;
+            sl_R = eye * c + sl_rrt * c1 + sl_r_x * s;
+
+            R[0] = sl_R.r00;
+            R[1] = sl_R.r01;
+            R[2] = sl_R.r02;
+            R[3] = sl_R.r10;
+            R[4] = sl_R.r11;
+            R[5] = sl_R.r12;
+            R[6] = sl_R.r20;
+            R[7] = sl_R.r21;
+            R[8] = sl_R.r22;
         }
+
         return R;
     }