From 587f503e864bdcd3a4ff87aca0fd662f792e3ee3 Mon Sep 17 00:00:00 2001 From: Andreas Klintberg Date: Sat, 11 Apr 2020 19:11:29 -0700 Subject: [PATCH 1/3] working in opencv 3 for ros2 --- darknet_ros/CMakeLists.txt | 4 +- .../darknet_ros/YoloObjectDetector.hpp | 40 +++++++++---------- .../include/darknet_ros/image_interface.h | 10 ++++- darknet_ros/src/YoloObjectDetector.cpp | 24 +++++------ ...{image_interface.c => image_interface.cpp} | 39 +++++++++++++++--- 5 files changed, 74 insertions(+), 43 deletions(-) rename darknet_ros/src/{image_interface.c => image_interface.cpp} (52%) diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index fd402a124..ffcd2c428 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -44,7 +44,7 @@ if ( X11_FOUND ) endif ( X11_FOUND ) # Find rquired packeges -find_package(OpenCV REQUIRED) +find_package(OpenCV 3 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(ament_cmake REQUIRED) @@ -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 diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp index 1314455df..8a037c4f4 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -37,6 +37,7 @@ #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 @@ -44,22 +45,18 @@ #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 + #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 } -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 ipl_; float fps_ = 0; float demoThresh_ = 0; float demoHier_ = .5; @@ -249,7 +249,7 @@ class YoloObjectDetector : public rclcpp::Node void yolo(); - IplImageWithHeader_ getIplImageWithHeader(); + MatWithHeader_ getIplImageWithHeader(); bool getImageStatus(void); diff --git a/darknet_ros/include/darknet_ros/image_interface.h b/darknet_ros/include/darknet_ros/image_interface.h index 460549591..c70a65125 100644 --- a/darknet_ros/include/darknet_ros/image_interface.h +++ b/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 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 diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index 7dcc9609a..aed5b9a58 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -459,9 +459,9 @@ void *YoloObjectDetector::fetchInThread() { { std::shared_lock lock(mutexImageCallback_); - IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); - IplImage* ROS_img = imageAndHeader.image; - ipl_into_image(ROS_img, buff_[buffIndex_]); + MatWithHeader_ imageAndHeader = getIplImageWithHeader(); + 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 lock(mutexImageCallback_); - IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); - IplImage* ROS_img = imageAndHeader.image; - buff_[0] = ipl_to_image(ROS_img); + MatWithHeader_ imageAndHeader = getIplImageWithHeader(); + 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); + ipl_ = cv::Mat(buff_[0].h, buff_[0].w, CV_8UC(buff_[0].c)); int count = 0; @@ -611,10 +610,9 @@ void YoloObjectDetector::yolo() } -IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader() +MatWithHeader_ YoloObjectDetector::getIplImageWithHeader() { - 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 = ipl_; if (!publishDetectionImage(cv::Mat(cvImage))) { RCLCPP_DEBUG(get_logger(), "Detection image has not been broadcasted."); } diff --git a/darknet_ros/src/image_interface.c b/darknet_ros/src/image_interface.cpp similarity index 52% rename from darknet_ros/src/image_interface.c rename to darknet_ros/src/image_interface.cpp index 7fe9d9d53..c91aa8378 100644 --- a/darknet_ros/src/image_interface.c +++ b/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 From 418f1071b65edfeaa58209e132d6ae86f6880cce Mon Sep 17 00:00:00 2001 From: Andreas Klintberg Date: Sat, 11 Apr 2020 19:25:49 -0700 Subject: [PATCH 2/3] changed variable name --- .../include/darknet_ros/YoloObjectDetector.hpp | 4 ++-- darknet_ros/src/YoloObjectDetector.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp index 8a037c4f4..68f19b0fb 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -187,7 +187,7 @@ class YoloObjectDetector : public rclcpp::Node image buffLetter_[3]; int buffId_[3]; int buffIndex_ = 0; - cv::Mat ipl_; + cv::Mat mat_; float fps_ = 0; float demoThresh_ = 0; float demoHier_ = .5; @@ -249,7 +249,7 @@ class YoloObjectDetector : public rclcpp::Node void yolo(); - MatWithHeader_ getIplImageWithHeader(); + MatWithHeader_ getMatImageWithHeader(); bool getImageStatus(void); diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index aed5b9a58..3fd217fe4 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -459,7 +459,7 @@ void *YoloObjectDetector::fetchInThread() { { std::shared_lock lock(mutexImageCallback_); - MatWithHeader_ imageAndHeader = getIplImageWithHeader(); + MatWithHeader_ imageAndHeader = getMatImageWithHeader(); cv::Mat ROS_img = imageAndHeader.image; buff_[buffIndex_] = mat_to_image_(ROS_img); headerBuff_[buffIndex_] = imageAndHeader.header; @@ -554,7 +554,7 @@ void YoloObjectDetector::yolo() { std::shared_lock lock(mutexImageCallback_); - MatWithHeader_ imageAndHeader = getIplImageWithHeader(); + MatWithHeader_ imageAndHeader = getMatImageWithHeader(); cv::Mat ROS_img = imageAndHeader.image; buff_[0] = mat_to_image_(ROS_img); headerBuff_[0] = imageAndHeader.header; @@ -566,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_ = cv::Mat(buff_[0].h, buff_[0].w, CV_8UC(buff_[0].c)); + mat_ = cv::Mat(buff_[0].h, buff_[0].w, CV_8UC(buff_[0].c)); int count = 0; @@ -592,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 { @@ -610,7 +610,7 @@ void YoloObjectDetector::yolo() } -MatWithHeader_ YoloObjectDetector::getIplImageWithHeader() +MatWithHeader_ YoloObjectDetector::getMatImageWithHeader() { MatWithHeader_ header {camImageCopy_, imageHeader_}; return header; @@ -631,7 +631,7 @@ bool YoloObjectDetector::isNodeRunning(void) void *YoloObjectDetector::publishInThread() { // Publish image. - cv::Mat cvImage = ipl_; + cv::Mat cvImage = mat_; if (!publishDetectionImage(cv::Mat(cvImage))) { RCLCPP_DEBUG(get_logger(), "Detection image has not been broadcasted."); } From ccedd0da83e6cf3bc0f5e2896e467f7c3b0db13d Mon Sep 17 00:00:00 2001 From: Andreas Klintberg Date: Sat, 11 Apr 2020 19:27:26 -0700 Subject: [PATCH 3/3] remove enforce 3 --- darknet_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index ffcd2c428..f8855d1dd 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -44,7 +44,7 @@ if ( X11_FOUND ) endif ( X11_FOUND ) # Find rquired packeges -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(ament_cmake REQUIRED)