Ros2 OpenCV3

darknet_ros/CMakeLists.txt

@@ -82,7 +82,7 @@ include_directories(
 )
 
 set(PROJECT_LIB_FILES
-    src/YoloObjectDetector.cpp                    src/image_interface.c
+    src/YoloObjectDetector.cpp                    src/image_interface.cpp
 )
 
 set(DARKNET_CORE_FILES

darknet_ros/include/darknet_ros/YoloObjectDetector.hpp

@@ -37,29 +37,26 @@
 #include "darknet_ros_msgs/msg/bounding_box.hpp"
 #include "darknet_ros_msgs/msg/object_count.hpp"
 #include "darknet_ros_msgs/action/check_for_objects.hpp"
+#include "darknet_ros/image_interface.h"
 
 // Darknet.
 #ifdef GPU
 #include "cuda_runtime.h"
 #include "curand.h"
 #include "cublas_v2.h"
 #endif
-
 extern "C" {
-#include "network.h"
-#include "detection_layer.h"
-#include "region_layer.h"
-#include "cost_layer.h"
-#include "utils.h"
-#include "parser.h"
-#include "box.h"
-#include "darknet_ros/image_interface.h"
-#include <sys/time.h>
+  #include "network.h"
+  #include "detection_layer.h"
+  #include "region_layer.h"
+  #include "cost_layer.h"
+  #include "utils.h"
+  #include "parser.h"
+  #include "image.h"
+  #include "box.h"
+  #include <sys/time.h>
 }
 
-extern "C" void ipl_into_image(IplImage* src, image im);
-extern "C" image ipl_to_image(IplImage* src);
-extern "C" void show_image_cv(image p, const char *name, IplImage *disp);
 
 namespace darknet_ros {
 
@@ -70,11 +67,14 @@ typedef struct
   int num, Class;
 } RosBox_;
 
-typedef struct
-{
-  IplImage* image;
-  std_msgs::msg::Header header;
-} IplImageWithHeader_;
+struct MatWithHeader_ {
+    cv::Mat image;
+    std_msgs::msg::Header header;
+
+    MatWithHeader_() = default;
+    MatWithHeader_(cv::Mat img, std_msgs::msg::Header hdr):
+        image(img.clone()), header(hdr) {}
+};
 
 class YoloObjectDetector : public rclcpp::Node
 {
@@ -187,7 +187,7 @@ class YoloObjectDetector : public rclcpp::Node
   image buffLetter_[3];
   int buffId_[3];
   int buffIndex_ = 0;
-  IplImage * ipl_;
+  cv::Mat mat_;
   float fps_ = 0;
   float demoThresh_ = 0;
   float demoHier_ = .5;
@@ -249,7 +249,7 @@ class YoloObjectDetector : public rclcpp::Node
 
   void yolo();
 
-  IplImageWithHeader_ getIplImageWithHeader();
+  MatWithHeader_ getMatImageWithHeader();
 
   bool getImageStatus(void);
 

darknet_ros/include/darknet_ros/image_interface.h

@@ -9,10 +9,16 @@
 #ifndef IMAGE_INTERFACE_H
 #define IMAGE_INTERFACE_H
 
-#include "image.h"
+extern "C" {
+    #include "image.h"
+}
+
+#include <opencv2/core.hpp>
 
 static float get_pixel(image m, int x, int y, int c);
 image **load_alphabet_with_file(char *datafile);
-void generate_image(image p, IplImage *disp);
+void generate_image(image p, cv::Mat disp);
+image mat_to_image_(cv::Mat m);
+image* mat_to_image_(cv::Mat m, image* im);
 
 #endif

darknet_ros/src/YoloObjectDetector.cpp

@@ -459,9 +459,9 @@ void *YoloObjectDetector::fetchInThread()
 {
   {
     std::shared_lock<std::shared_mutex> lock(mutexImageCallback_);
-    IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
-    IplImage* ROS_img = imageAndHeader.image;
-    ipl_into_image(ROS_img, buff_[buffIndex_]);
+    MatWithHeader_ imageAndHeader = getMatImageWithHeader();
+    cv::Mat ROS_img = imageAndHeader.image;
+    buff_[buffIndex_] = mat_to_image_(ROS_img);
     headerBuff_[buffIndex_] = imageAndHeader.header;
     buffId_[buffIndex_] = actionId_;
   }
@@ -472,8 +472,7 @@ void *YoloObjectDetector::fetchInThread()
 
 void *YoloObjectDetector::displayInThread(void *ptr)
 {
-  show_image_cv(buff_[(buffIndex_ + 1)%3], "YOLO V3", ipl_);
-  int c = cv::waitKey(waitKeyDelay_);
+  int c = show_image_cv(buff_[(buffIndex_ + 1)%3], "YOLO V3", 1);
   if (c != -1) c = c%256;
   if (c == 27) {
       demoDone_ = 1;
@@ -555,9 +554,9 @@ void YoloObjectDetector::yolo()
 
   {
     std::shared_lock<std::shared_mutex> lock(mutexImageCallback_);
-    IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
-    IplImage* ROS_img = imageAndHeader.image;
-    buff_[0] = ipl_to_image(ROS_img);
+    MatWithHeader_ imageAndHeader = getMatImageWithHeader();
+    cv::Mat ROS_img = imageAndHeader.image;
+    buff_[0] = mat_to_image_(ROS_img);
     headerBuff_[0] = imageAndHeader.header;
   }
   buff_[1] = copy_image(buff_[0]);
@@ -567,7 +566,7 @@ void YoloObjectDetector::yolo()
   buffLetter_[0] = letterbox_image(buff_[0], net_->w, net_->h);
   buffLetter_[1] = letterbox_image(buff_[0], net_->w, net_->h);
   buffLetter_[2] = letterbox_image(buff_[0], net_->w, net_->h);
-  ipl_ = cvCreateImage(cvSize(buff_[0].w, buff_[0].h), IPL_DEPTH_8U, buff_[0].c);
+  mat_ = cv::Mat(buff_[0].h, buff_[0].w, CV_8UC(buff_[0].c));
 
   int count = 0;
 
@@ -593,7 +592,7 @@ void YoloObjectDetector::yolo()
       if (viewImage_) {
         displayInThread(0);
       } else {
-        generate_image(buff_[(buffIndex_ + 1)%3], ipl_);
+        generate_image(buff_[(buffIndex_ + 1)%3], mat_);
       }
       publishInThread();
     } else {
@@ -611,10 +610,9 @@ void YoloObjectDetector::yolo()
 
 }
 
-IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader()
+MatWithHeader_ YoloObjectDetector::getMatImageWithHeader()
 {
-  IplImage* ROS_img = new IplImage(camImageCopy_);
-  IplImageWithHeader_ header = {.image = ROS_img, .header = imageHeader_};
+  MatWithHeader_ header {camImageCopy_, imageHeader_};
   return header;
 }
 
@@ -633,7 +631,7 @@ bool YoloObjectDetector::isNodeRunning(void)
 void *YoloObjectDetector::publishInThread()
 {
   // Publish image.
-  cv::Mat cvImage = cv::cvarrToMat(ipl_);
+  cv::Mat cvImage = mat_;
   if (!publishDetectionImage(cv::Mat(cvImage))) {
     RCLCPP_DEBUG(get_logger(), "Detection image has not been broadcasted.");
   }

darknet_ros/src/image_interface.c → darknet_ros/src/image_interface.cpp

@@ -17,13 +17,13 @@ static float get_pixel(image m, int x, int y, int c)
 image **load_alphabet_with_file(char *datafile) {
   int i, j;
   const int nsize = 8;
-  image **alphabets = calloc(nsize, sizeof(image));
-  char* labels = "/labels/%d_%d.png";
+  image **alphabets = (image**)calloc(nsize, sizeof(image));
+  char const * labels = "/labels/%d_%d.png";
   char * files = (char *) malloc(1 + strlen(datafile)+ strlen(labels) );
   strcpy(files, datafile);
   strcat(files, labels);
   for(j = 0; j < nsize; ++j){
-    alphabets[j] = calloc(128, sizeof(image));
+    alphabets[j] = (image *)calloc(128, sizeof(image));
     for(i = 32; i < 127; ++i){
       char buff[256];
       sprintf(buff, files, i, j);
@@ -34,19 +34,46 @@ image **load_alphabet_with_file(char *datafile) {
 }
 
 #ifdef OPENCV
-void generate_image(image p, IplImage *disp)
+void generate_image(image p, cv::Mat disp)
 {
     int x,y,k;
     if(p.c == 3) rgbgr_image(p);
     //normalize_image(copy);
 
-    int step = disp->widthStep;
+    int step = disp.step[0];
     for(y = 0; y < p.h; ++y){
         for(x = 0; x < p.w; ++x){
             for(k= 0; k < p.c; ++k){
-                disp->imageData[y*step + x*p.c + k] = (unsigned char)(get_pixel(p,x,y,k)*255);
+                disp.data[y*step + x*p.c + k] = (unsigned char)(get_pixel(p,x,y,k)*255);
             }
         }
     }
 }
+
+image mat_to_image_(cv::Mat m)
+{
+    int h = m.rows;
+    int w = m.cols;
+    int c = m.channels();
+    image im = make_image(w, h, c);
+    return im;
+}
+
+image* mat_to_image_(cv::Mat m, image* im)
+{
+    int h = m.rows;
+    int w = m.cols;
+    int c = m.channels();
+    unsigned char *data = (unsigned char *)m.data;
+    int step = m.step;
+
+    for(int i = 0; i < h; ++i){
+        for(int k= 0; k < c; ++k){
+            for(int j = 0; j < w; ++j){
+                im->data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
+            }
+        }
+    }
+  return im;
+}	
 #endif