From f3053b0176955f2ad417addecb442a1a91115563 Mon Sep 17 00:00:00 2001 From: PinkWink Date: Thu, 23 Sep 2021 10:53:30 +0900 Subject: [PATCH 1/4] Added yolov4-tiny --- darknet_ros/CMakeLists.txt | 32 +- darknet_ros/config/yolov4-tiny.yaml | 90 ++++++ darknet_ros/launch/yolo_v4-tiny.launch | 16 + .../yolo_network_config/cfg/yolov4-tiny.cfg | 281 ++++++++++++++++++ 4 files changed, 407 insertions(+), 12 deletions(-) create mode 100644 darknet_ros/config/yolov4-tiny.yaml create mode 100644 darknet_ros/launch/yolo_v4-tiny.launch create mode 100644 darknet_ros/yolo_network_config/cfg/yolov4-tiny.cfg diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index ef6f3924a..8dc0624de 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -228,20 +228,20 @@ install(DIRECTORY config launch yolo_network_config # Download yolov2-tiny.weights set(PATH "${CMAKE_CURRENT_SOURCE_DIR}/yolo_network_config/weights") -set(FILE "${PATH}/yolov2-tiny.weights") -message(STATUS "Checking and downloading yolov2-tiny.weights if needed ...") -if (NOT EXISTS "${FILE}") - message(STATUS "... file does not exist. Downloading now ...") - execute_process(COMMAND wget -q https://github.com/leggedrobotics/darknet_ros/releases/download/1.1.4/yolov2-tiny.weights -P ${PATH}) -endif() +#set(FILE "${PATH}/yolov2-tiny.weights") +#message(STATUS "Checking and downloading yolov2-tiny.weights if needed ...") +#if (NOT EXISTS "${FILE}") +# message(STATUS "... file does not exist. Downloading now ...") +# execute_process(COMMAND wget -q https://github.com/leggedrobotics/darknet_ros/releases/download/1.1.4/yolov2-tiny.weights -P ${PATH}) +#endif() # Download yolov3.weights -set(FILE "${PATH}/yolov3.weights") -message(STATUS "Checking and downloading yolov3.weights if needed ...") -if (NOT EXISTS "${FILE}") - message(STATUS "... file does not exist. Downloading now ...") - execute_process(COMMAND wget -q https://github.com/leggedrobotics/darknet_ros/releases/download/1.1.4/yolov3.weights -P ${PATH}) -endif() +#set(FILE "${PATH}/yolov3.weights") +#message(STATUS "Checking and downloading yolov3.weights if needed ...") +#if (NOT EXISTS "${FILE}") +# message(STATUS "... file does not exist. Downloading now ...") +# execute_process(COMMAND wget -q https://github.com/leggedrobotics/darknet_ros/releases/download/1.1.4/yolov3.weights -P ${PATH}) +#endif() # Download yolov4.weights set(FILE "${PATH}/yolov4.weights") @@ -251,6 +251,14 @@ if (NOT EXISTS "${FILE}") execute_process(COMMAND wget -q https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights -P ${PATH}) endif() +# Download yolov4-tiny.weights +set(FILE "${PATH}/yolov4-tiny.weights") +message(STATUS "Checking and downloading yolov4-tiny.weights if needed ...") +if (NOT EXISTS "${FILE}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND wget -q https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v4_pre/yolov4-tiny.weights -P ${PATH}) +endif() + ############# ## Testing ## ############# diff --git a/darknet_ros/config/yolov4-tiny.yaml b/darknet_ros/config/yolov4-tiny.yaml new file mode 100644 index 000000000..be54094b5 --- /dev/null +++ b/darknet_ros/config/yolov4-tiny.yaml @@ -0,0 +1,90 @@ +yolo_model: + + config_file: + name: yolov4-tiny.cfg + weight_file: + name: yolov4-tiny.weights + threshold: + value: 0.3 + detection_classes: + names: + - person + - bicycle + - car + - motorbike + - aeroplane + - bus + - train + - truck + - boat + - traffic light + - fire hydrant + - stop sign + - parking meter + - bench + - bird + - cat + - dog + - horse + - sheep + - cow + - elephant + - bear + - zebra + - giraffe + - backpack + - umbrella + - handbag + - tie + - suitcase + - frisbee + - skis + - snowboard + - sports ball + - kite + - baseball bat + - baseball glove + - skateboard + - surfboard + - tennis racket + - bottle + - wine glass + - cup + - fork + - knife + - spoon + - bowl + - banana + - apple + - sandwich + - orange + - broccoli + - carrot + - hot dog + - pizza + - donut + - cake + - chair + - sofa + - pottedplant + - bed + - diningtable + - toilet + - tvmonitor + - laptop + - mouse + - remote + - keyboard + - cell phone + - microwave + - oven + - toaster + - sink + - refrigerator + - book + - clock + - vase + - scissors + - teddy bear + - hair drier + - toothbrush diff --git a/darknet_ros/launch/yolo_v4-tiny.launch b/darknet_ros/launch/yolo_v4-tiny.launch new file mode 100644 index 000000000..d309d9b35 --- /dev/null +++ b/darknet_ros/launch/yolo_v4-tiny.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/darknet_ros/yolo_network_config/cfg/yolov4-tiny.cfg b/darknet_ros/yolo_network_config/cfg/yolov4-tiny.cfg new file mode 100644 index 000000000..025f34f1a --- /dev/null +++ b/darknet_ros/yolo_network_config/cfg/yolov4-tiny.cfg @@ -0,0 +1,281 @@ +[net] +# Testing +#batch=1 +#subdivisions=1 +# Training +batch=64 +subdivisions=1 +width=416 +height=416 +channels=3 +momentum=0.9 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.00261 +burn_in=1000 +max_batches = 500200 +policy=steps +steps=400000,450000 +scales=.1,.1 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers=-1 +groups=2 +group_id=1 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1,-2 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -6,-1 + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers=-1 +groups=2 +group_id=1 + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1,-2 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -6,-1 + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers=-1 +groups=2 +group_id=1 + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1,-2 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -6,-1 + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +################################## + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + + +[yolo] +mask = 3,4,5 +anchors = 10,14, 23,27, 37,58, 81,82, 135,169, 344,319 +classes=80 +num=6 +jitter=.3 +scale_x_y = 1.05 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +ignore_thresh = .7 +truth_thresh = 1 +random=0 +resize=1.5 +nms_kind=greedynms +beta_nms=0.6 + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = -1, 23 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + +[yolo] +mask = 1,2,3 +anchors = 10,14, 23,27, 37,58, 81,82, 135,169, 344,319 +classes=80 +num=6 +jitter=.3 +scale_x_y = 1.05 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +ignore_thresh = .7 +truth_thresh = 1 +random=0 +resize=1.5 +nms_kind=greedynms +beta_nms=0.6 \ No newline at end of file From 3d4d2399bd9ea1eafabd2ed3842cb9c9faf8f28c Mon Sep 17 00:00:00 2001 From: kyuhyong Date: Thu, 14 Sep 2023 15:12:18 +0900 Subject: [PATCH 2/4] Switch to opencv4 --- .gitignore | 1 + darknet_ros/CMakeLists.txt | 14 ++-- darknet_ros/config/yolov4-tiny_dehumid.yaml | 12 +++ .../darknet_ros/YoloObjectDetector.hpp | 32 ++++--- ...{image_interface.h => image_interface.hpp} | 8 +- darknet_ros/src/YoloObjectDetector.cpp | 83 ++++++++++++++----- ...{image_interface.c => image_interface.cpp} | 6 +- 7 files changed, 109 insertions(+), 47 deletions(-) create mode 100644 .gitignore create mode 100644 darknet_ros/config/yolov4-tiny_dehumid.yaml rename darknet_ros/include/darknet_ros/{image_interface.h => image_interface.hpp} (66%) rename darknet_ros/src/{image_interface.c => image_interface.cpp} (85%) diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..6c07c8043 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +darknet_ros/yolo_network_config/cfg/yolov4-tiny_dehumid.cfg diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index 8dc0624de..a859d7046 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 2.8.12) project(darknet_ros) # Set c++11 cmake flags -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}") +#set(CMAKE_CXX_STANDARD 17) set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -fPIC ${CMAKE_C_FLAGS}") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -23,12 +24,13 @@ if (CUDA_FOUND) CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS}; -O3 - -gencode arch=compute_30,code=sm_30 - -gencode arch=compute_35,code=sm_35 - -gencode arch=compute_50,code=[sm_50,compute_50] - -gencode arch=compute_52,code=[sm_52,compute_52] + #-gencode arch=compute_30,code=sm_30 + #-gencode arch=compute_35,code=sm_35 + #-gencode arch=compute_50,code=[sm_50,compute_50] + #-gencode arch=compute_52,code=[sm_52,compute_52] -gencode arch=compute_61,code=sm_61 -gencode arch=compute_62,code=sm_62 + -gencode arch=compute_86,code=[sm_86,compute_86] ) add_definitions(-DGPU) else() @@ -90,7 +92,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/config/yolov4-tiny_dehumid.yaml b/darknet_ros/config/yolov4-tiny_dehumid.yaml new file mode 100644 index 000000000..47ce6e309 --- /dev/null +++ b/darknet_ros/config/yolov4-tiny_dehumid.yaml @@ -0,0 +1,12 @@ +yolo_model: + + config_file: + name: yolov4-tiny_dehumid.cfg + weight_file: + name: yolov4-tiny_dehumid_final2.weights + threshold: + value: 0.8 + detection_classes: + names: + - pass + - no_cover diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp index 88c0a5126..967a35393 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -51,7 +51,7 @@ extern "C" { #include #include "box.h" #include "cost_layer.h" -#include "darknet_ros/image_interface.h" +#include "darknet_ros/image_interface.hpp" #include "detection_layer.h" #include "network.h" #include "parser.h" @@ -59,8 +59,10 @@ extern "C" { #include "utils.h" } -extern "C" void ipl_into_image(IplImage* src, image im); -extern "C" image ipl_to_image(IplImage* src); +//extern "C" void ipl_into_image(IplImage* src, image im); +//extern "C" image ipl_to_image(IplImage* src); +extern "C" cv::Mat image_to_mat(image im); +extern "C" image mat_to_image(cv::Mat m); // extern "C" void show_image_cv(image p, const char* name, IplImage* disp); namespace darknet_ros { @@ -71,10 +73,15 @@ typedef struct { int num, Class; } RosBox_; +// typedef struct { +// IplImage* image; +// std_msgs::Header header; +// } IplImageWithHeader_; + typedef struct { - IplImage* image; + cv::Mat image; std_msgs::Header header; -} IplImageWithHeader_; +} CvMatWithHeader_; class YoloObjectDetector { public: @@ -176,7 +183,8 @@ class YoloObjectDetector { image buffLetter_[3]; int buffId_[3]; int buffIndex_ = 0; - IplImage* ipl_; + //IplImage* ipl_; + cv::Mat disp_; float fps_ = 0; float demoThresh_ = 0; float demoHier_ = .5; @@ -202,16 +210,16 @@ class YoloObjectDetector { std_msgs::Header imageHeader_; cv::Mat camImageCopy_; - boost::shared_mutex mutexImageCallback_; + std::shared_mutex mutexImageCallback_; bool imageStatus_ = false; - boost::shared_mutex mutexImageStatus_; + std::shared_mutex mutexImageStatus_; bool isNodeRunning_ = true; - boost::shared_mutex mutexNodeStatus_; + std::shared_mutex mutexNodeStatus_; int actionId_; - boost::shared_mutex mutexActionStatus_; + std::shared_mutex mutexActionStatus_; // double getWallTime(); @@ -236,13 +244,15 @@ class YoloObjectDetector { void yolo(); - IplImageWithHeader_ getIplImageWithHeader(); + //IplImageWithHeader_ getIplImageWithHeader(); + CvMatWithHeader_ getCvMatWithHeader(); bool getImageStatus(void); bool isNodeRunning(void); void* publishInThread(); + void generate_image_cp(image p, cv::Mat& disp); }; } /* namespace darknet_ros*/ diff --git a/darknet_ros/include/darknet_ros/image_interface.h b/darknet_ros/include/darknet_ros/image_interface.hpp similarity index 66% rename from darknet_ros/include/darknet_ros/image_interface.h rename to darknet_ros/include/darknet_ros/image_interface.hpp index eb55df8b4..23081421b 100644 --- a/darknet_ros/include/darknet_ros/image_interface.h +++ b/darknet_ros/include/darknet_ros/image_interface.hpp @@ -6,14 +6,14 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#ifndef IMAGE_INTERFACE_H -#define IMAGE_INTERFACE_H +#ifndef IMAGE_INTERFACE_HPP +#define IMAGE_INTERFACE_HPP #include "image.h" -#include "opencv2/core/types_c.h" +//#include "opencv2/core/types_c.h" 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); #endif diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index 14e897f87..ced92255b 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -39,7 +39,7 @@ YoloObjectDetector::YoloObjectDetector(ros::NodeHandle nh) YoloObjectDetector::~YoloObjectDetector() { { - boost::unique_lock lockNodeStatus(mutexNodeStatus_); + std::unique_lock lockNodeStatus(mutexNodeStatus_); isNodeRunning_ = false; } yoloThread_.join(); @@ -151,8 +151,8 @@ void YoloObjectDetector::init() { std::string checkForObjectsActionName; nodeHandle_.param("actions/camera_reading/topic", checkForObjectsActionName, std::string("check_for_objects")); checkForObjectsActionServer_.reset(new CheckForObjectsActionServer(nodeHandle_, checkForObjectsActionName, false)); - checkForObjectsActionServer_->registerGoalCallback(boost::bind(&YoloObjectDetector::checkForObjectsActionGoalCB, this)); - checkForObjectsActionServer_->registerPreemptCallback(boost::bind(&YoloObjectDetector::checkForObjectsActionPreemptCB, this)); + checkForObjectsActionServer_->registerGoalCallback(std::bind(&YoloObjectDetector::checkForObjectsActionGoalCB, this)); + checkForObjectsActionServer_->registerPreemptCallback(std::bind(&YoloObjectDetector::checkForObjectsActionPreemptCB, this)); checkForObjectsActionServer_->start(); } @@ -170,12 +170,12 @@ void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) { if (cam_image) { { - boost::unique_lock lockImageCallback(mutexImageCallback_); + std::unique_lock lockImageCallback(mutexImageCallback_); imageHeader_ = msg->header; camImageCopy_ = cam_image->image.clone(); } { - boost::unique_lock lockImageStatus(mutexImageStatus_); + std::unique_lock lockImageStatus(mutexImageStatus_); imageStatus_ = true; } frameWidth_ = cam_image->image.size().width; @@ -187,7 +187,7 @@ void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) { void YoloObjectDetector::checkForObjectsActionGoalCB() { ROS_DEBUG("[YoloObjectDetector] Start check for objects action."); - boost::shared_ptr imageActionPtr = checkForObjectsActionServer_->acceptNewGoal(); + std::shared_ptr imageActionPtr = checkForObjectsActionServer_->acceptNewGoal(); sensor_msgs::Image imageAction = imageActionPtr->image; cv_bridge::CvImagePtr cam_image; @@ -201,15 +201,15 @@ void YoloObjectDetector::checkForObjectsActionGoalCB() { if (cam_image) { { - boost::unique_lock lockImageCallback(mutexImageCallback_); + std::unique_lock lockImageCallback(mutexImageCallback_); camImageCopy_ = cam_image->image.clone(); } { - boost::unique_lock lockImageCallback(mutexActionStatus_); + std::unique_lock lockImageCallback(mutexActionStatus_); actionId_ = imageActionPtr->id; } { - boost::unique_lock lockImageStatus(mutexImageStatus_); + std::unique_lock lockImageStatus(mutexImageStatus_); imageStatus_ = true; } frameWidth_ = cam_image->image.size().width; @@ -370,10 +370,13 @@ void* YoloObjectDetector::detectInThread() { void* YoloObjectDetector::fetchInThread() { { - boost::shared_lock lock(mutexImageCallback_); - IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); - IplImage* ROS_img = imageAndHeader.image; - ipl_into_image(ROS_img, buff_[buffIndex_]); + std::shared_lock lock(mutexImageCallback_); + //IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); + //IplImage* ROS_img = imageAndHeader.image; + //ipl_into_image(ROS_img, buff_[buffIndex_]); + CvMatWithHeader_ imageAndHeader = getCvMatWithHeader(); + free_image(buff_[buffIndex_]); + buff_[buffIndex_] = mat_to_image(imageAndHeader.image); headerBuff_[buffIndex_] = imageAndHeader.header; buffId_[buffIndex_] = actionId_; } @@ -382,6 +385,14 @@ void* YoloObjectDetector::fetchInThread() { return 0; } +float get_pixel_cp(image m, int x, int y, int c) +{ + assert(x < m.w && y < m.h && c < m.c); + return m.data[c*m.h*m.w + y*m.w + x]; +} + +int windows = 0; + void* YoloObjectDetector::displayInThread(void* ptr) { show_image_cv(buff_[(buffIndex_ + 1) % 3], "YOLO V4"); int c = cv::waitKey(waitKeyDelay_); @@ -459,10 +470,14 @@ void YoloObjectDetector::yolo() { roiBoxes_ = (darknet_ros::RosBox_*)calloc(l.w * l.h * l.n, sizeof(darknet_ros::RosBox_)); { - boost::shared_lock lock(mutexImageCallback_); - IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); - IplImage* ROS_img = imageAndHeader.image; - buff_[0] = ipl_to_image(ROS_img); + // boost::shared_lock lock(mutexImageCallback_); + // IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); + // IplImage* ROS_img = imageAndHeader.image; + // buff_[0] = ipl_to_image(ROS_img); + // headerBuff_[0] = imageAndHeader.header; + std::shared_lock lock(mutexImageCallback_); + CvMatWithHeader_ imageAndHeader = getCvMatWithHeader(); + buff_[0] = mat_to_image(imageAndHeader.image); headerBuff_[0] = imageAndHeader.header; } buff_[1] = copy_image(buff_[0]); @@ -472,7 +487,8 @@ 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_ = cvCreateImage(cvSize(buff_[0].w, buff_[0].h), IPL_DEPTH_8U, buff_[0].c); + disp_ = image_to_mat(buff_[0]); int count = 0; @@ -498,7 +514,7 @@ void YoloObjectDetector::yolo() { if (viewImage_) { displayInThread(0); } else { - generate_image(buff_[(buffIndex_ + 1) % 3], ipl_); + generate_image_cp(buff_[(buffIndex_ + 1) % 3], disp_); } publishInThread(); } else { @@ -515,25 +531,33 @@ void YoloObjectDetector::yolo() { } } +CvMatWithHeader_ YoloObjectDetector::getCvMatWithHeader() { + CvMatWithHeader_ header = {.image = camImageCopy_, .header = imageHeader_}; + return header; +} + +/* IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader() { IplImage* ROS_img = new IplImage(camImageCopy_); IplImageWithHeader_ header = {.image = ROS_img, .header = imageHeader_}; return header; } +*/ bool YoloObjectDetector::getImageStatus(void) { - boost::shared_lock lock(mutexImageStatus_); + std::shared_lock lock(mutexImageStatus_); return imageStatus_; } bool YoloObjectDetector::isNodeRunning(void) { - boost::shared_lock lock(mutexNodeStatus_); + std::shared_lock lock(mutexNodeStatus_); return isNodeRunning_; } void* YoloObjectDetector::publishInThread() { // Publish image. - cv::Mat cvImage = cv::cvarrToMat(ipl_); + //cv::Mat cvImage = cv::cvarrToMat(ipl_); + cv::Mat cvImage = disp_; if (!publishDetectionImage(cv::Mat(cvImage))) { ROS_DEBUG("Detection image has not been broadcasted."); } @@ -603,5 +627,18 @@ void* YoloObjectDetector::publishInThread() { return 0; } - +void YoloObjectDetector::generate_image_cp(image p, cv::Mat& disp) { + int x, y, k; + if (p.c == 3) rgbgr_image(p); + // normalize_image(copy); + + int step = disp.step; + for (y = 0; y < p.h; ++y) { + for (x = 0; x < p.w; ++x) { + for (k = 0; k < p.c; ++k) { + disp.data[y * step + x * p.c + k] = (unsigned char)(get_pixel_cp(p, x, y, k) * 255); + } + } + } +} } /* namespace darknet_ros*/ diff --git a/darknet_ros/src/image_interface.c b/darknet_ros/src/image_interface.cpp similarity index 85% rename from darknet_ros/src/image_interface.c rename to darknet_ros/src/image_interface.cpp index e1ab068cb..e5bf9a1ac 100644 --- a/darknet_ros/src/image_interface.c +++ b/darknet_ros/src/image_interface.cpp @@ -6,7 +6,7 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include "darknet_ros/image_interface.h" +#include "darknet_ros/image_interface.hpp" static float get_pixel(image m, int x, int y, int c) { assert(x < m.w && y < m.h && c < m.c); @@ -33,7 +33,7 @@ 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); @@ -42,7 +42,7 @@ void generate_image(image p, IplImage* disp) { 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); } } } From 6e6e5d4d47697fb4d32111552e6e7b9c7178566b Mon Sep 17 00:00:00 2001 From: kyuhyong Date: Thu, 14 Sep 2023 16:34:23 +0900 Subject: [PATCH 3/4] Fix std->boost --- darknet_ros/CMakeLists.txt | 10 +++++---- .../darknet_ros/YoloObjectDetector.hpp | 16 +++++++++----- .../include/darknet_ros/image_interface.hpp | 4 ++++ darknet_ros/src/YoloObjectDetector.cpp | 22 +++++++++---------- darknet_ros/src/image_interface.cpp | 8 +++---- 5 files changed, 36 insertions(+), 24 deletions(-) diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index a859d7046..23f296579 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -2,8 +2,9 @@ cmake_minimum_required(VERSION 2.8.12) project(darknet_ros) # Set c++11 cmake flags -set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") #set(CMAKE_CXX_STANDARD 17) +#set(CMAKE_CXX_STANDARD_REQUIRED True) set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -fPIC ${CMAKE_C_FLAGS}") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -28,8 +29,8 @@ if (CUDA_FOUND) #-gencode arch=compute_35,code=sm_35 #-gencode arch=compute_50,code=[sm_50,compute_50] #-gencode arch=compute_52,code=[sm_52,compute_52] - -gencode arch=compute_61,code=sm_61 - -gencode arch=compute_62,code=sm_62 + #-gencode arch=compute_61,code=sm_61 + #-gencode arch=compute_62,code=sm_62 -gencode arch=compute_86,code=[sm_86,compute_86] ) add_definitions(-DGPU) @@ -92,7 +93,8 @@ include_directories( ) set(PROJECT_LIB_FILES - src/YoloObjectDetector.cpp src/image_interface.cpp + 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 967a35393..322b399d6 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -51,12 +51,18 @@ extern "C" { #include #include "box.h" #include "cost_layer.h" -#include "darknet_ros/image_interface.hpp" +//#include "darknet_ros/image_interface.hpp" #include "detection_layer.h" #include "network.h" #include "parser.h" #include "region_layer.h" #include "utils.h" + +#include "image_opencv.h" +#include "image.h" +#include "darknet.h" + +#include "image_interface.hpp" } //extern "C" void ipl_into_image(IplImage* src, image im); @@ -210,16 +216,16 @@ class YoloObjectDetector { std_msgs::Header imageHeader_; cv::Mat camImageCopy_; - std::shared_mutex mutexImageCallback_; + boost::shared_mutex mutexImageCallback_; bool imageStatus_ = false; - std::shared_mutex mutexImageStatus_; + boost::shared_mutex mutexImageStatus_; bool isNodeRunning_ = true; - std::shared_mutex mutexNodeStatus_; + boost::shared_mutex mutexNodeStatus_; int actionId_; - std::shared_mutex mutexActionStatus_; + boost::shared_mutex mutexActionStatus_; // double getWallTime(); diff --git a/darknet_ros/include/darknet_ros/image_interface.hpp b/darknet_ros/include/darknet_ros/image_interface.hpp index 23081421b..f4ee3d8d2 100644 --- a/darknet_ros/include/darknet_ros/image_interface.hpp +++ b/darknet_ros/include/darknet_ros/image_interface.hpp @@ -9,6 +9,10 @@ #ifndef IMAGE_INTERFACE_HPP #define IMAGE_INTERFACE_HPP +#include "opencv2/highgui/highgui_c.h" +#include "opencv2/imgproc/imgproc_c.h" +#include "opencv2/core/version.hpp" + #include "image.h" //#include "opencv2/core/types_c.h" diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index ced92255b..3cc181fa5 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -39,7 +39,7 @@ YoloObjectDetector::YoloObjectDetector(ros::NodeHandle nh) YoloObjectDetector::~YoloObjectDetector() { { - std::unique_lock lockNodeStatus(mutexNodeStatus_); + boost::unique_lock lockNodeStatus(mutexNodeStatus_); isNodeRunning_ = false; } yoloThread_.join(); @@ -170,12 +170,12 @@ void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) { if (cam_image) { { - std::unique_lock lockImageCallback(mutexImageCallback_); + boost::unique_lock lockImageCallback(mutexImageCallback_); imageHeader_ = msg->header; camImageCopy_ = cam_image->image.clone(); } { - std::unique_lock lockImageStatus(mutexImageStatus_); + boost::unique_lock lockImageStatus(mutexImageStatus_); imageStatus_ = true; } frameWidth_ = cam_image->image.size().width; @@ -187,7 +187,7 @@ void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) { void YoloObjectDetector::checkForObjectsActionGoalCB() { ROS_DEBUG("[YoloObjectDetector] Start check for objects action."); - std::shared_ptr imageActionPtr = checkForObjectsActionServer_->acceptNewGoal(); + boost::shared_ptr imageActionPtr = checkForObjectsActionServer_->acceptNewGoal(); sensor_msgs::Image imageAction = imageActionPtr->image; cv_bridge::CvImagePtr cam_image; @@ -201,15 +201,15 @@ void YoloObjectDetector::checkForObjectsActionGoalCB() { if (cam_image) { { - std::unique_lock lockImageCallback(mutexImageCallback_); + boost::unique_lock lockImageCallback(mutexImageCallback_); camImageCopy_ = cam_image->image.clone(); } { - std::unique_lock lockImageCallback(mutexActionStatus_); + boost::unique_lock lockImageCallback(mutexActionStatus_); actionId_ = imageActionPtr->id; } { - std::unique_lock lockImageStatus(mutexImageStatus_); + boost::unique_lock lockImageStatus(mutexImageStatus_); imageStatus_ = true; } frameWidth_ = cam_image->image.size().width; @@ -370,7 +370,7 @@ void* YoloObjectDetector::detectInThread() { void* YoloObjectDetector::fetchInThread() { { - std::shared_lock lock(mutexImageCallback_); + boost::shared_lock lock(mutexImageCallback_); //IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); //IplImage* ROS_img = imageAndHeader.image; //ipl_into_image(ROS_img, buff_[buffIndex_]); @@ -475,7 +475,7 @@ void YoloObjectDetector::yolo() { // IplImage* ROS_img = imageAndHeader.image; // buff_[0] = ipl_to_image(ROS_img); // headerBuff_[0] = imageAndHeader.header; - std::shared_lock lock(mutexImageCallback_); + boost::shared_lock lock(mutexImageCallback_); CvMatWithHeader_ imageAndHeader = getCvMatWithHeader(); buff_[0] = mat_to_image(imageAndHeader.image); headerBuff_[0] = imageAndHeader.header; @@ -545,12 +545,12 @@ IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader() { */ bool YoloObjectDetector::getImageStatus(void) { - std::shared_lock lock(mutexImageStatus_); + boost::shared_lock lock(mutexImageStatus_); return imageStatus_; } bool YoloObjectDetector::isNodeRunning(void) { - std::shared_lock lock(mutexNodeStatus_); + boost::shared_lock lock(mutexNodeStatus_); return isNodeRunning_; } diff --git a/darknet_ros/src/image_interface.cpp b/darknet_ros/src/image_interface.cpp index e5bf9a1ac..728358559 100644 --- a/darknet_ros/src/image_interface.cpp +++ b/darknet_ros/src/image_interface.cpp @@ -1,5 +1,5 @@ /* - * image_interface.c + * image_interface.cpp * * Created on: Dec 19, 2016 * Author: Marko Bjelonic @@ -16,13 +16,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)); + image** alphabets = (image**)calloc(nsize, sizeof(image)); char* 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); @@ -38,7 +38,7 @@ void generate_image(image p, cv::Mat& disp) { if (p.c == 3) rgbgr_image(p); // normalize_image(copy); - int step = disp->widthStep; + int step = disp.step; for (y = 0; y < p.h; ++y) { for (x = 0; x < p.w; ++x) { for (k = 0; k < p.c; ++k) { From a00093f4d2a9c519c3c1ee966306e3fa83bb7ad7 Mon Sep 17 00:00:00 2001 From: kyuhyong Date: Fri, 15 Sep 2023 12:22:20 +0900 Subject: [PATCH 4/4] Fixed link error, gtk --- darknet_ros/CMakeLists.txt | 4 ++-- darknet_ros/config/ros.yaml | 2 +- .../darknet_ros/YoloObjectDetector.hpp | 5 +++-- .../include/darknet_ros/image_interface.hpp | 2 +- darknet_ros/launch/yolo_v3.launch | 2 +- darknet_ros/launch/yolo_v4-tiny.launch | 2 +- darknet_ros/launch/yolo_v4.launch | 2 +- darknet_ros/src/YoloObjectDetector.cpp | 19 +++++++++++++++++++ darknet_ros/src/image_interface.cpp | 19 ------------------- 9 files changed, 29 insertions(+), 28 deletions(-) diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index 23f296579..ad8009fc2 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -3,10 +3,10 @@ project(darknet_ros) # Set c++11 cmake flags set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") -#set(CMAKE_CXX_STANDARD 17) -#set(CMAKE_CXX_STANDARD_REQUIRED True) set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -fPIC ${CMAKE_C_FLAGS}") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +find_package(PkgConfig REQUIRED) +pkg_check_modules(gtk+ REQUIRED gtk+-3.0) # Define path of darknet folder here. find_path(DARKNET_PATH diff --git a/darknet_ros/config/ros.yaml b/darknet_ros/config/ros.yaml index 04abce530..534e3a7bc 100644 --- a/darknet_ros/config/ros.yaml +++ b/darknet_ros/config/ros.yaml @@ -28,6 +28,6 @@ publishers: image_view: - enable_opencv: true + enable_opencv: false wait_key_delay: 1 enable_console_output: true diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp index 322b399d6..dab9ffcdf 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -51,7 +51,6 @@ extern "C" { #include #include "box.h" #include "cost_layer.h" -//#include "darknet_ros/image_interface.hpp" #include "detection_layer.h" #include "network.h" #include "parser.h" @@ -62,7 +61,7 @@ extern "C" { #include "image.h" #include "darknet.h" -#include "image_interface.hpp" +#include "darknet_ros/image_interface.hpp" } //extern "C" void ipl_into_image(IplImage* src, image im); @@ -248,6 +247,8 @@ class YoloObjectDetector { void setupNetwork(char* cfgfile, char* weightfile, char* datafile, float thresh, char** names, int classes, int delay, char* prefix, int avg_frames, float hier, int w, int h, int frames, int fullscreen); + image** load_alphabet_with_file(char* datafile); + void yolo(); //IplImageWithHeader_ getIplImageWithHeader(); diff --git a/darknet_ros/include/darknet_ros/image_interface.hpp b/darknet_ros/include/darknet_ros/image_interface.hpp index f4ee3d8d2..0a2fd66f8 100644 --- a/darknet_ros/include/darknet_ros/image_interface.hpp +++ b/darknet_ros/include/darknet_ros/image_interface.hpp @@ -17,7 +17,7 @@ //#include "opencv2/core/types_c.h" static float get_pixel(image m, int x, int y, int c); -image** load_alphabet_with_file(char* datafile); +//image** load_alphabet_with_file(char* datafile); //Moved to YoloObejctDetector.cpp void generate_image(image p, cv::Mat& disp); #endif diff --git a/darknet_ros/launch/yolo_v3.launch b/darknet_ros/launch/yolo_v3.launch index b39b44656..00f388c19 100644 --- a/darknet_ros/launch/yolo_v3.launch +++ b/darknet_ros/launch/yolo_v3.launch @@ -4,7 +4,7 @@ - + diff --git a/darknet_ros/launch/yolo_v4-tiny.launch b/darknet_ros/launch/yolo_v4-tiny.launch index d309d9b35..5dc4a3f80 100644 --- a/darknet_ros/launch/yolo_v4-tiny.launch +++ b/darknet_ros/launch/yolo_v4-tiny.launch @@ -4,7 +4,7 @@ - + diff --git a/darknet_ros/launch/yolo_v4.launch b/darknet_ros/launch/yolo_v4.launch index 275d78a18..6f659db80 100644 --- a/darknet_ros/launch/yolo_v4.launch +++ b/darknet_ros/launch/yolo_v4.launch @@ -4,7 +4,7 @@ - + diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index 3cc181fa5..9f33738d4 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -443,6 +443,25 @@ void YoloObjectDetector::setupNetwork(char* cfgfile, char* weightfile, char* dat set_batch_network(net_, 1); } +image** YoloObjectDetector::load_alphabet_with_file(char* datafile) { + int i, j; + const int nsize = 8; + image** alphabets = (image**)calloc(nsize, sizeof(image)); + char* 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] = (image*)calloc(128, sizeof(image)); + for (i = 32; i < 127; ++i) { + char buff[256]; + sprintf(buff, files, i, j); + alphabets[j][i] = load_image_color(buff, 0, 0); + } + } + return alphabets; +} + void YoloObjectDetector::yolo() { const auto wait_duration = std::chrono::milliseconds(2000); while (!getImageStatus()) { diff --git a/darknet_ros/src/image_interface.cpp b/darknet_ros/src/image_interface.cpp index 728358559..62f99820b 100644 --- a/darknet_ros/src/image_interface.cpp +++ b/darknet_ros/src/image_interface.cpp @@ -13,25 +13,6 @@ static float get_pixel(image m, int x, int y, int c) { return m.data[c * m.h * m.w + y * m.w + x]; } -image** load_alphabet_with_file(char* datafile) { - int i, j; - const int nsize = 8; - image** alphabets = (image**)calloc(nsize, sizeof(image)); - char* 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] = (image*)calloc(128, sizeof(image)); - for (i = 32; i < 127; ++i) { - char buff[256]; - sprintf(buff, files, i, j); - alphabets[j][i] = load_image_color(buff, 0, 0); - } - } - return alphabets; -} - #ifdef OPENCV void generate_image(image p, cv::Mat& disp) { int x, y, k;