Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

OpenCV4 and Darknet by AlexeyAB #275

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion darknet
Submodule darknet updated 1236 files
63 changes: 12 additions & 51 deletions darknet_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
cmake_minimum_required(VERSION 2.8.12)
project(darknet_ros)

set(BUILD_AS_CPP ON)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../darknet ${CMAKE_BINARY_DIR}/darknet)

# Set c++11 cmake flags
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -fPIC ${CMAKE_C_FLAGS}")
Expand Down Expand Up @@ -39,7 +42,7 @@ endif()
message ( STATUS "Searching for X11..." )
find_package ( X11 REQUIRED )
if ( X11_FOUND )
include_directories ( ${X11_INCLUDE_DIR} )
include_directories (SYSTEM ${X11_INCLUDE_DIR} )
link_libraries ( ${X11_LIBRARIES} )
message ( STATUS " X11_INCLUDE_DIR: " ${X11_INCLUDE_DIR} )
message ( STATUS " X11_LIBRARIES: " ${X11_LIBRARIES} )
Expand All @@ -48,7 +51,7 @@ endif ( X11_FOUND )
# Find rquired packeges
find_package(Boost REQUIRED COMPONENTS thread)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(SYSTEM ${OpenCV_INCLUDE_DIRS})
find_package(catkin REQUIRED
COMPONENTS
cv_bridge
Expand Down Expand Up @@ -82,58 +85,16 @@ catkin_package(
)

include_directories(
${DARKNET_PATH}/src
${DARKNET_PATH}/include
include
)
include_directories(SYSTEM
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)

set(PROJECT_LIB_FILES
src/YoloObjectDetector.cpp src/image_interface.c
)

set(DARKNET_CORE_FILES
${DARKNET_PATH}/src/activation_layer.c ${DARKNET_PATH}/src/im2col.c
${DARKNET_PATH}/src/activations.c ${DARKNET_PATH}/src/image.c
${DARKNET_PATH}/src/avgpool_layer.c ${DARKNET_PATH}/src/layer.c
${DARKNET_PATH}/src/batchnorm_layer.c ${DARKNET_PATH}/src/list.c
${DARKNET_PATH}/src/blas.c ${DARKNET_PATH}/src/local_layer.c
${DARKNET_PATH}/src/box.c ${DARKNET_PATH}/src/lstm_layer.c
${DARKNET_PATH}/src/col2im.c ${DARKNET_PATH}/src/matrix.c
${DARKNET_PATH}/src/connected_layer.c ${DARKNET_PATH}/src/maxpool_layer.c
${DARKNET_PATH}/src/convolutional_layer.c ${DARKNET_PATH}/src/network.c
${DARKNET_PATH}/src/cost_layer.c ${DARKNET_PATH}/src/normalization_layer.c
${DARKNET_PATH}/src/crnn_layer.c ${DARKNET_PATH}/src/option_list.c
${DARKNET_PATH}/src/crop_layer.c ${DARKNET_PATH}/src/parser.c
${DARKNET_PATH}/src/cuda.c ${DARKNET_PATH}/src/region_layer.c
${DARKNET_PATH}/src/data.c ${DARKNET_PATH}/src/reorg_layer.c
${DARKNET_PATH}/src/deconvolutional_layer.c ${DARKNET_PATH}/src/rnn_layer.c
${DARKNET_PATH}/src/demo.c ${DARKNET_PATH}/src/route_layer.c
${DARKNET_PATH}/src/detection_layer.c ${DARKNET_PATH}/src/shortcut_layer.c
${DARKNET_PATH}/src/dropout_layer.c ${DARKNET_PATH}/src/softmax_layer.c
${DARKNET_PATH}/src/gemm.c ${DARKNET_PATH}/src/tree.c
${DARKNET_PATH}/src/gru_layer.c ${DARKNET_PATH}/src/utils.c
${DARKNET_PATH}/src/upsample_layer.c ${DARKNET_PATH}/src/logistic_layer.c
${DARKNET_PATH}/src/l2norm_layer.c ${DARKNET_PATH}/src/yolo_layer.c

${DARKNET_PATH}/examples/art.c ${DARKNET_PATH}/examples/lsd.c
${DARKNET_PATH}/examples/attention.c ${DARKNET_PATH}/examples/nightmare.c
${DARKNET_PATH}/examples/captcha.c ${DARKNET_PATH}/examples/regressor.c
${DARKNET_PATH}/examples/cifar.c ${DARKNET_PATH}/examples/rnn.c
${DARKNET_PATH}/examples/classifier.c ${DARKNET_PATH}/examples/segmenter.c
${DARKNET_PATH}/examples/coco.c ${DARKNET_PATH}/examples/super.c
${DARKNET_PATH}/examples/darknet.c ${DARKNET_PATH}/examples/tag.c
${DARKNET_PATH}/examples/detector.c ${DARKNET_PATH}/examples/yolo.c
${DARKNET_PATH}/examples/go.c
)

set(DARKNET_CUDA_FILES
${DARKNET_PATH}/src/activation_kernels.cu ${DARKNET_PATH}/src/crop_layer_kernels.cu
${DARKNET_PATH}/src/avgpool_layer_kernels.cu ${DARKNET_PATH}/src/deconvolutional_kernels.cu
${DARKNET_PATH}/src/blas_kernels.cu ${DARKNET_PATH}/src/dropout_layer_kernels.cu
${DARKNET_PATH}/src/col2im_kernels.cu ${DARKNET_PATH}/src/im2col_kernels.cu
${DARKNET_PATH}/src/convolutional_kernels.cu ${DARKNET_PATH}/src/maxpool_layer_kernels.cu
src/YoloObjectDetector.cpp
src/image_interface.cpp
)

if (CUDA_FOUND)
Expand All @@ -143,8 +104,7 @@ if (CUDA_FOUND)
)

cuda_add_library(${PROJECT_NAME}_lib
${PROJECT_LIB_FILES} ${DARKNET_CORE_FILES}
${DARKNET_CUDA_FILES}
${PROJECT_LIB_FILES}
)

target_link_libraries(${PROJECT_NAME}_lib
Expand All @@ -161,7 +121,7 @@ if (CUDA_FOUND)
else()

add_library(${PROJECT_NAME}_lib
${PROJECT_LIB_FILES} ${DARKNET_CORE_FILES}
${PROJECT_LIB_FILES}
)

add_executable(${PROJECT_NAME}
Expand All @@ -178,6 +138,7 @@ target_link_libraries(${PROJECT_NAME}_lib
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
${OpenCV_LIBS}
dark
)

target_link_libraries(${PROJECT_NAME}
Expand Down
14 changes: 6 additions & 8 deletions darknet_ros/include/darknet_ros/YoloObjectDetector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,21 +45,19 @@
#include "curand.h"
#endif

extern "C" {
#include <sys/time.h>
#include "box.h"
#include "cost_layer.h"
#include "darknet_ros/image_interface.h"
#include "detection_layer.h"
#include "network.h"
#include "parser.h"
#include "region_layer.h"
#include "utils.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);
#include "image_opencv.h"
#include "blas.h"

#include "darknet_ros/image_interface.h"

namespace darknet_ros {

Expand All @@ -70,7 +68,7 @@ typedef struct {
} RosBox_;

typedef struct {
IplImage* image;
cv::Mat image;
std_msgs::Header header;
} IplImageWithHeader_;

Expand Down Expand Up @@ -174,7 +172,7 @@ class YoloObjectDetector {
image buffLetter_[3];
int buffId_[3];
int buffIndex_ = 0;
IplImage* ipl_;
cv::Mat ipl_;
float fps_ = 0;
float demoThresh_ = 0;
float demoHier_ = .5;
Expand Down
4 changes: 3 additions & 1 deletion darknet_ros/include/darknet_ros/image_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@

#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);

#endif
23 changes: 11 additions & 12 deletions darknet_ros/src/YoloObjectDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -286,7 +286,7 @@ detection* YoloObjectDetector::avgPredictions(network* net, int* nboxes) {
count += l.outputs;
}
}
detection* dets = get_network_boxes(net, buff_[0].w, buff_[0].h, demoThresh_, demoHier_, 0, 1, nboxes);
detection* dets = get_network_boxes(net, buff_[0].w, buff_[0].h, demoThresh_, demoHier_, 0, 1, nboxes, 0);
return dets;
}

Expand All @@ -296,7 +296,7 @@ void* YoloObjectDetector::detectInThread() {

layer l = net_->layers[net_->n - 1];
float* X = buffLetter_[(buffIndex_ + 2) % 3].data;
float* prediction = network_predict(net_, X);
float* prediction = network_predict(*net_, X);

rememberNetwork(net_);
detection* dets = 0;
Expand All @@ -312,7 +312,7 @@ void* YoloObjectDetector::detectInThread() {
printf("Objects:\n\n");
}
image display = buff_[(buffIndex_ + 2) % 3];
draw_detections(display, dets, nboxes, demoThresh_, demoNames_, demoAlphabet_, demoClasses_);
draw_detections_v3(display, dets, nboxes, demoThresh_, demoNames_, demoAlphabet_, demoClasses_, false);

// extract the bounding boxes and send them to ROS
int i, j;
Expand Down Expand Up @@ -369,8 +369,8 @@ void* YoloObjectDetector::fetchInThread() {
{
boost::shared_lock<boost::shared_mutex> lock(mutexImageCallback_);
IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
IplImage* ROS_img = imageAndHeader.image;
ipl_into_image(ROS_img, buff_[buffIndex_]);
cv::Mat ROS_img = imageAndHeader.image;
buff_[buffIndex_] = mat_to_image(ROS_img);
headerBuff_[buffIndex_] = imageAndHeader.header;
buffId_[buffIndex_] = actionId_;
}
Expand All @@ -380,7 +380,7 @@ void* YoloObjectDetector::fetchInThread() {
}

void* YoloObjectDetector::displayInThread(void* ptr) {
show_image_cv(buff_[(buffIndex_ + 1) % 3], "YOLO V3", ipl_);
show_image_cv(buff_[(buffIndex_ + 1) % 3], "YOLO V3"); //, ipl_);
int c = cv::waitKey(waitKeyDelay_);
if (c != -1) c = c % 256;
if (c == 27) {
Expand Down Expand Up @@ -458,8 +458,8 @@ void YoloObjectDetector::yolo() {
{
boost::shared_lock<boost::shared_mutex> lock(mutexImageCallback_);
IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
IplImage* ROS_img = imageAndHeader.image;
buff_[0] = ipl_to_image(ROS_img);
cv::Mat ROS_img = imageAndHeader.image;
buff_[0] = mat_to_image(ROS_img);
headerBuff_[0] = imageAndHeader.header;
}
buff_[1] = copy_image(buff_[0]);
Expand All @@ -469,7 +469,6 @@ 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);

int count = 0;

Expand All @@ -495,7 +494,7 @@ void YoloObjectDetector::yolo() {
if (viewImage_) {
displayInThread(0);
} else {
generate_image(buff_[(buffIndex_ + 1) % 3], ipl_);
ipl_ = image_to_mat(buff_[(buffIndex_ + 1) % 3]);
}
publishInThread();
} else {
Expand All @@ -513,7 +512,7 @@ void YoloObjectDetector::yolo() {
}

IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader() {
IplImage* ROS_img = new IplImage(camImageCopy_);
cv::Mat ROS_img = camImageCopy_;
IplImageWithHeader_ header = {.image = ROS_img, .header = imageHeader_};
return header;
}
Expand All @@ -530,7 +529,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))) {
ROS_DEBUG("Detection image has not been broadcasted.");
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -33,16 +33,16 @@ 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);
}
}
}
Expand Down