├── .gitmodules ├── CMakeLists.txt ├── README.md ├── doc ├── hopenet_graph.png ├── posenet.png ├── rcnn.png ├── retinaface.png ├── yolact.png ├── yolo.png └── yolov5.png ├── include └── ros_ncnn │ ├── gpu_support.h │ ├── ncnn_config.h.in │ ├── ncnn_fast_rcnn.h │ ├── ncnn_hopenet.h │ ├── ncnn_pfld.h │ ├── ncnn_posenet.h │ ├── ncnn_retinaface.h │ ├── ncnn_ultraface.h │ ├── ncnn_utils.h │ ├── ncnn_yolact.h │ ├── ncnn_yolo.h │ └── ncnn_yolov5.h ├── launch ├── hopenet.launch ├── retinaface.launch ├── ultraface.launch ├── yolact.launch ├── yolo.launch └── yolov5.launch ├── msg ├── Euler.msg ├── FaceObject.msg ├── Object.msg ├── Rectangle.msg └── Vector2D.msg ├── package.xml └── src ├── faster_rcnn_node.cpp ├── hopenet_node.cpp ├── ncnn_fast_rcnn.cpp ├── ncnn_hopenet.cpp ├── ncnn_pfld.cpp ├── ncnn_posenet.cpp ├── ncnn_retinaface.cpp ├── ncnn_ultraface.cpp ├── ncnn_yolact.cpp ├── ncnn_yolo.cpp ├── ncnn_yolov5.cpp ├── pfld_node.cpp ├── posenet_node.cpp ├── retinaface_node.cpp ├── ultraface_node.cpp ├── yolact_node.cpp ├── yolo_node.cpp └── yolov5_node.cpp /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "assets"] 2 | path = assets 3 | url = https://github.com/nilseuropa/ncnn_models 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_ncnn) 3 | 4 | add_compile_options(-std=c++11 -DUSE_AVX_INSTRUCTIONS=ON) 5 | 6 | option(GPU_SUPPORT "Enable GPU support (Vulkan)" OFF) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | roslib 11 | cv_bridge 12 | image_transport 13 | image_geometry 14 | sensor_msgs 15 | message_generation 16 | ) 17 | 18 | add_message_files( 19 | FILES 20 | Vector2D.msg 21 | Rectangle.msg 22 | FaceObject.msg 23 | Object.msg 24 | Euler.msg 25 | ) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | ) 31 | 32 | catkin_package( 33 | CATKIN_DEPENDS 34 | roscpp 35 | roslib 36 | cv_bridge 37 | image_transport 38 | image_geometry 39 | sensor_msgs 40 | message_generation 41 | ) 42 | 43 | include_directories( 44 | "include" 45 | ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | find_package(ncnn REQUIRED) 49 | if (${ncnn_FOUND}) 50 | message("-- NCNN found.") 51 | message("-- NCNN_VULKAN flag is ${NCNN_VULKAN}") 52 | if (${NCNN_VULKAN}) 53 | message("-- AUTO-ENABLING GPU_SUPPORT") 54 | set(GPU_SUPPORT ON) 55 | endif() 56 | include_directories(${ncnn_INCLUDE}) 57 | endif() 58 | 59 | find_package(OpenCV REQUIRED COMPONENTS 60 | core highgui imgproc imgcodecs 61 | ) 62 | include_directories( 63 | ${OpenCV_INCLUDE_DIRS} 64 | ) 65 | 66 | if (GPU_SUPPORT) 67 | message("-- GPU support is ENABLED") 68 | find_package(Vulkan) # REQUIRES ncnn to be built with vulkan 69 | if (${VULKAN_FOUND}) 70 | message("-- Vulkan found.") 71 | else() 72 | message("-- ERROR: AUTO-DISABLING GPU_SUPPORT, because Vulkan was not found") 73 | set(GPU_SUPPORT OFF) 74 | endif() 75 | else() 76 | message("-- GPU support is DISABLED") 77 | endif() 78 | 79 | # NCNN config header exports GPU_SUPPORT definition towards source files 80 | configure_file("include/ros_ncnn/ncnn_config.h.in" "ros_ncnn/ncnn_config.h") 81 | include_directories(${CMAKE_CURRENT_BINARY_DIR}) 82 | 83 | # PFLD 84 | add_executable(pfld_node src/pfld_node.cpp src/ncnn_pfld.cpp) 85 | add_dependencies( pfld_node ros_ncnn_generate_messages_cpp) 86 | target_link_libraries(pfld_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 87 | 88 | # HOPENET 89 | add_executable(hopenet_node src/hopenet_node.cpp src/ncnn_hopenet.cpp) 90 | add_dependencies( hopenet_node ros_ncnn_generate_messages_cpp) 91 | target_link_libraries(hopenet_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 92 | 93 | # RETINAFACE 94 | add_executable(retinaface_node src/retinaface_node.cpp src/ncnn_retinaface.cpp) 95 | add_dependencies( retinaface_node ros_ncnn_generate_messages_cpp) 96 | target_link_libraries(retinaface_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 97 | 98 | # ULTRAFACE 99 | add_executable(ultraface_node src/ultraface_node.cpp src/ncnn_ultraface.cpp) 100 | add_dependencies( ultraface_node ros_ncnn_generate_messages_cpp) 101 | target_link_libraries(ultraface_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 102 | 103 | # YOLACT 104 | add_executable(yolact_node src/yolact_node.cpp src/ncnn_yolact.cpp) 105 | add_dependencies( yolact_node ros_ncnn_generate_messages_cpp) 106 | target_link_libraries(yolact_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 107 | 108 | # YOLO v2/v3 109 | add_executable(yolo_node src/yolo_node.cpp src/ncnn_yolo.cpp) 110 | add_dependencies( yolo_node ros_ncnn_generate_messages_cpp) 111 | target_link_libraries(yolo_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 112 | 113 | # YOLO v5 114 | add_executable(yolov5_node src/yolov5_node.cpp src/ncnn_yolov5.cpp) 115 | add_dependencies( yolov5_node ros_ncnn_generate_messages_cpp) 116 | target_link_libraries(yolov5_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 117 | 118 | # POSENET 119 | add_executable(posenet_node src/posenet_node.cpp src/ncnn_posenet.cpp) 120 | add_dependencies( posenet_node ros_ncnn_generate_messages_cpp) 121 | target_link_libraries(posenet_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 122 | 123 | # FASTER RCNN 124 | add_executable(faster_rcnn_node src/faster_rcnn_node.cpp src/ncnn_fast_rcnn.cpp) 125 | add_dependencies( faster_rcnn_node ros_ncnn_generate_messages_cpp) 126 | target_link_libraries(faster_rcnn_node ${catkin_LIBRARIES} ncnn ${OpenCV_LIBS}) 127 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS_NCNN # 2 | 3 | This is a ROS package for NCNN, a high-performance neural network inference framework *- by Tencent -* optimized for mobile platforms: 4 | 5 | - ARM NEON assembly level optimization 6 | - Sophisticated memory management and data structure design, very low memory footprint 7 | - Supports multi-core parallel computing acceleration 8 | - Supports GPU acceleration via the next-generation low-overhead Vulkan API 9 | - The overall library size is less than 700K, and can be easily reduced to less than 300K 10 | - Extensible model design, supports 8bit quantization and half-precision floating point storage 11 | - Can import caffe/pytorch/mxnet/onnx models 12 | 13 | 14 | 15 | ## Setting up ## 16 | 17 | ### Library ### 18 | 19 | - [Build for NVIDIA Jetson](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-nvidia-jetson) 20 | - [Build for Linux x86](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-linux-x86) 21 | - [Build for Windows x64 using VS2017](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-windows-x64-using-visual-studio-community-2017) 22 | - [Build for MacOSX](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-macosx) 23 | - [Build for Raspberry Pi 3](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-raspberry-pi-3) 24 | - [Build for ARM Cortex-A family with cross-compiling](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-arm-cortex-a-family-with-cross-compiling) 25 | - [Build for Android](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-android) 26 | - [Build for iOS on MacOSX with xcode](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-ios-on-macosx-with-xcode) 27 | - [Build for iOS on Linux with cctools-port](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-ios-on-linux-with-cctools-port) 28 | - [Build for Hisilicon platform with cross-compiling](https://github.com/Tencent/ncnn/wiki/how-to-build#build-for-hisilicon-platform-with-cross-compiling) 29 | 30 | 31 | 32 | ## ROS package ## 33 | 34 | * Clone this repository into your catkin workspace. 35 | * Initialize and update submodule `ncnn-assets` *( this is a collection of some popular models )* 36 | * Compile the workspace. 37 | * CMake script is going to autodetect whether the **ncnn library** is built with **Vulkan** or not. _( All nodes will utilize the GPU if Vulkan is enabled. )_ 38 | 39 | #### General launch parameters #### 40 | ```xml 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | ``` 52 | 53 | ### YOLACT ### 54 | ![](doc/yolact.png) 55 | 56 | **Publisher** 57 | ```yaml 58 | # Object message 59 | Header header 60 | Rectangle boundingbox # Vector2D position and size 61 | string label 62 | float32 probability 63 | ``` 64 | **Params** 65 | * _probability_threshold_ - default 0.5 - above which objects are published 66 | 67 | ### YOLO v2 / v3 68 | ![](doc/yolo.png) 69 | The `assets` repository has multiple YOLO networks, choose the parameter and model file before launch. _( Default is YOLO-3 on MobileNet-2 )_ 70 | 71 | **Publisher** 72 | ```yaml 73 | # Object message 74 | Header header 75 | Rectangle boundingbox # Vector2D position and size 76 | string label 77 | float32 probability 78 | ``` 79 | **Params** 80 | * _model_file_ - YOLO network model file 81 | * _param_file_ - YOLO network parameter file 82 | * _probability_threshold_ - default 0.5 - above which objects are published 83 | 84 | ### YOLO v5 85 | ![](doc/yolov5.png) 86 | 87 | **Publisher** 88 | ```yaml 89 | # Object message 90 | Header header 91 | Rectangle boundingbox # Vector2D position and size 92 | string label 93 | float32 probability 94 | ``` 95 | **Params** 96 | * _model_file_ - YOLO network model file 97 | * _param_file_ - YOLO network parameter file 98 | * _probability_threshold_ - default 0.5 - above which objects are published 99 | 100 | ### RetinaFace ### 101 | ![](doc/retinaface.png) 102 | 103 | **Publisher** 104 | ```yaml 105 | # FaceObject message 106 | Header header 107 | Rectangle boundingbox # Vector2D position and size 108 | Vector2D[5] landmark # 5x 2x float32 109 | float32 probability 110 | ``` 111 | **Params** 112 | * _probability_threshold_ - default 0.5 - above which face objects are published 113 | 114 | ### HopeNet ### 115 | Using RetinaFace as face detector: 116 | ![](doc/hopenet_graph.png) 117 | 118 | **Publisher** 119 | ```yaml 120 | # Euler angles 121 | float32 roll 122 | float32 pitch 123 | float32 yaw 124 | ``` 125 | 126 | ### PoseNet ### 127 | ![](doc/posenet.png) 128 | 129 | ### Faster R-CNN ### 130 | Don't forget to uncompress `ZF_faster_rcnn_final.bin.zip` in assets directory first. _( but again, R-CNN is the past and that's neither a cat nor a bird right there... that's my best friend )_ 131 | ![](doc/rcnn.png) 132 | 133 | ## :construction: To do ## 134 | 135 | * General model loader node _( with layer to topic mapping through NDS file )_ 136 | * Dynamic reconfiguration for some params _( e.g. probability thresholds )_ 137 | 138 | ## :v: Acknowledgements ## 139 | _Special thanks to **[Nihui](https://github.com/nihui)** for her wonderful work._ 140 | -------------------------------------------------------------------------------- /doc/hopenet_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/hopenet_graph.png -------------------------------------------------------------------------------- /doc/posenet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/posenet.png -------------------------------------------------------------------------------- /doc/rcnn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/rcnn.png -------------------------------------------------------------------------------- /doc/retinaface.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/retinaface.png -------------------------------------------------------------------------------- /doc/yolact.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/yolact.png -------------------------------------------------------------------------------- /doc/yolo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/yolo.png -------------------------------------------------------------------------------- /doc/yolov5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nilseuropa/ros_ncnn/a7e08d3804ae582f0e6bf726a83d5e4c37d87c56/doc/yolov5.png -------------------------------------------------------------------------------- /include/ros_ncnn/gpu_support.h: -------------------------------------------------------------------------------- 1 | #ifndef _NODE_GPU_SUPPORT_HEADER_ 2 | #define _NODE_GPU_SUPPORT_HEADER_ 3 | 4 | static ncnn::VulkanDevice* g_vkdev = 0; 5 | static ncnn::VkAllocator* g_blob_vkallocator = 0; 6 | static ncnn::VkAllocator* g_staging_vkallocator = 0; 7 | 8 | // Check GPU info, override selection with 1st discrete device if the selected gpu_device is non-discrete 9 | int selectGPU(int gpu_device){ 10 | /* 11 | int gpus = ncnn::get_gpu_count(); 12 | ncnn::GpuInfo gpu_info; 13 | int first_discrete = -1; 14 | std::string gpu_type_s, selected_gpu_type_s = "UNKNOWN"; 15 | bool selected_discrete = false; 16 | bool has_discrete = false; 17 | for (int g=0; g rect; 13 | int label; 14 | float prob; 15 | }; 16 | 17 | static const char* class_names[] = {"background", 18 | "aeroplane", "bicycle", "bird", "boat", 19 | "bottle", "bus", "car", "cat", "chair", 20 | "cow", "diningtable", "dog", "horse", 21 | "motorbike", "person", "pottedplant", 22 | "sheep", "sofa", "train", "tvmonitor"}; 23 | 24 | class ncnnFastRcnn 25 | { 26 | 27 | public: 28 | 29 | ncnn::Net neuralnet; 30 | 31 | int detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads); 32 | void draw(const cv::Mat& bgr, const std::vector& objects, double dT); 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_hopenet.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_HOPENET_HEADER_ 2 | #define _ROS_NCNN_HOPENET_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | struct HeadPose 11 | { 12 | float roll; 13 | float pitch; 14 | float yaw; 15 | }; 16 | 17 | class ncnnHopeNet { 18 | 19 | private: 20 | float idx_tensor[66]; 21 | cv::Mat faceGrayResized; 22 | 23 | public: 24 | 25 | ncnn::Net neuralnet; 26 | 27 | void initialize(); 28 | void softmax(float* z, size_t el); 29 | double getAngle(float* prediction, size_t len); 30 | int detect(const cv::Mat& bgr, cv::Rect roi, HeadPose& euler_angles); 31 | void draw(); 32 | 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_pfld.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_PFLD_HEADER_ 2 | #define _ROS_NCNN_PFLD_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | class ncnnPFLD { 11 | 12 | private: 13 | const int num_landmarks = 106 * 2; 14 | float landmarks[212]; 15 | uint8_t num_threads = 8; 16 | const float norm_vals[3] = {1 / 255.f, 1 / 255.f, 1 / 255.f}; 17 | 18 | public: 19 | 20 | ncnn::Net neuralnet; 21 | 22 | int detect(const cv::Mat& bgr, cv::Rect roi); 23 | void draw(const cv::Mat& bgr, cv::Rect roi); 24 | void set_threads(uint8_t num); 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_posenet.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_POSENET_HEADER_ 2 | #define _ROS_NCNN_POSENET_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | struct KeyPoint 11 | { 12 | cv::Point2f p; 13 | float prob; 14 | }; 15 | 16 | class ncnnPoseNet { 17 | 18 | public: 19 | 20 | ncnn::Net neuralnet; 21 | 22 | int detect(const cv::Mat& bgr, std::vector& keypoints, uint8_t n_threads); 23 | void draw(const cv::Mat& bgr, const std::vector& keypoints, double dT); 24 | 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_retinaface.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_RETINAFACE_HEADER_ 2 | #define _ROS_NCNN_RETINAFACE_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | struct FaceObject 11 | { 12 | cv::Rect_ rect; 13 | cv::Point2f landmark[5]; 14 | float prob; 15 | }; 16 | 17 | class ncnnRetinaface { 18 | 19 | public: 20 | 21 | ncnn::Net neuralnet; 22 | 23 | int detect(const cv::Mat& bgr, std::vector& faceobjects, uint8_t n_threads); 24 | void draw(const cv::Mat& bgr, const std::vector& faceobjects, double dT); 25 | 26 | }; 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_ultraface.h: -------------------------------------------------------------------------------- 1 | // Created by vealocia on 2019/10/17. 2 | // Copyright © 2019 vealocia. All rights reserved. 3 | // Modifications by nilseuropa 4 | // 5 | 6 | #ifndef _ROS_NCNN_ULTRAFACE_HEADER_ 7 | #define _ROS_NCNN_ULTRAFACE_HEADER_ 8 | 9 | #include "platform.h" 10 | #include "net.h" 11 | #include "cpu.h" 12 | #include "ros_ncnn/ncnn_utils.h" 13 | 14 | #pragma once 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define num_featuremap 4 21 | #define hard_nms 1 22 | #define blending_nms 2 /* mix nms was been proposaled in paper blaze face, aims to minimize the temporal jitter*/ 23 | 24 | typedef struct FaceInfo { 25 | float x1; 26 | float y1; 27 | float x2; 28 | float y2; 29 | // float width; 30 | // float height; 31 | float score; 32 | // float *landmarks; 33 | } FaceInfo; 34 | 35 | class ncnnUltraFace { 36 | public: 37 | 38 | void init(int input_width, int input_length, float probability_threshold, float iou_threshold); 39 | int detect(const cv::Mat& bgr, std::vector& face_list, uint8_t n_threads); 40 | void draw(const cv::Mat& bgr, const std::vector& face_info, double dT); 41 | 42 | ncnn::Net neuralnet; 43 | 44 | private: 45 | void generateBBox(std::vector &bbox_collection, ncnn::Mat scores, ncnn::Mat boxes, float score_threshold, int num_anchors); 46 | void nms(std::vector &input, std::vector &output, int type = blending_nms); 47 | 48 | private: 49 | 50 | int num_thread; 51 | int image_w; 52 | int image_h; 53 | 54 | int in_w; 55 | int in_h; 56 | int num_anchors; 57 | 58 | float score_threshold; 59 | float iou_threshold; 60 | 61 | const float mean_vals[3] = {127, 127, 127}; 62 | const float norm_vals[3] = {1.0 / 128, 1.0 / 128, 1.0 / 128}; 63 | 64 | const float center_variance = 0.1; 65 | const float size_variance = 0.2; 66 | const std::vector> min_boxes = { 67 | {10.0f, 16.0f, 24.0f}, 68 | {32.0f, 48.0f}, 69 | {64.0f, 96.0f}, 70 | {128.0f, 192.0f, 256.0f}}; 71 | const std::vector strides = {8.0, 16.0, 32.0, 64.0}; 72 | std::vector> featuremap_size; 73 | std::vector> shrinkage_size; 74 | std::vector w_h_list; 75 | 76 | std::vector> priors = {}; 77 | }; 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_UTILS_HEADER_ 2 | #define _ROS_NCNN_UTILS_HEADER_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #if CV_VERSION_MAJOR >= 4 13 | #include 14 | #define CV_LOAD_IMAGE_COLOR cv::IMREAD_COLOR 15 | #define CV_BGR2GRAY cv::COLOR_BGR2GRAY 16 | #endif // CV_VERSION_MAJOR >= 4 17 | 18 | template 19 | inline float intersection_area(const T& a, const T& b) { 20 | cv::Rect_ inter = a.rect & b.rect; 21 | return inter.area(); 22 | }; 23 | 24 | template 25 | void qsort_descent_inplace(std::vector& objects, int left, int right) { 26 | int i = left; 27 | int j = right; 28 | float p = objects[(left + right) / 2].prob; 29 | 30 | while (i <= j) 31 | { 32 | while (objects[i].prob > p) 33 | i++; 34 | 35 | while (objects[j].prob < p) 36 | j--; 37 | 38 | if (i <= j) 39 | { 40 | // swap 41 | std::swap(objects[i], objects[j]); 42 | 43 | i++; 44 | j--; 45 | } 46 | } 47 | 48 | #pragma omp parallel sections 49 | { 50 | #pragma omp section 51 | { 52 | if (left < j) qsort_descent_inplace(objects, left, j); 53 | } 54 | #pragma omp section 55 | { 56 | if (i < right) qsort_descent_inplace(objects, i, right); 57 | } 58 | } 59 | }; 60 | 61 | template 62 | static void qsort_descent_inplace(std::vector& objects) 63 | { 64 | if (objects.empty()) 65 | return; 66 | 67 | qsort_descent_inplace(objects, 0, objects.size() - 1); 68 | }; 69 | 70 | 71 | template 72 | static void nms_sorted_bboxes(const std::vector& objects, std::vector& picked, float nms_threshold) 73 | { 74 | picked.clear(); 75 | 76 | const int n = objects.size(); 77 | 78 | std::vector areas(n); 79 | for (int i = 0; i < n; i++) 80 | { 81 | areas[i] = objects[i].rect.area(); 82 | } 83 | 84 | for (int i = 0; i < n; i++) 85 | { 86 | const T& a = objects[i]; 87 | 88 | int keep = 1; 89 | for (int j = 0; j < (int)picked.size(); j++) 90 | { 91 | const T& b = objects[picked[j]]; 92 | 93 | // intersection over union 94 | float inter_area = intersection_area(a, b); 95 | float union_area = areas[i] + areas[picked[j]] - inter_area; 96 | // float IoU = inter_area / union_area 97 | if (inter_area / union_area > nms_threshold) 98 | keep = 0; 99 | } 100 | 101 | if (keep) 102 | picked.push_back(i); 103 | } 104 | }; 105 | 106 | template 107 | T linear_map(T x, T in_min, T in_max, T out_min, T out_max) { 108 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 109 | }; 110 | 111 | // template 112 | // void printArray(T* array, uint len) 113 | // { 114 | // for (uint i=1; i rect; 13 | int label; 14 | float prob; 15 | std::vector maskdata; 16 | cv::Mat mask; 17 | }; 18 | 19 | static const char* class_names[] = {"background", 20 | "person", "bicycle", "car", "motorcycle", "airplane", "bus", 21 | "train", "truck", "boat", "traffic light", "fire hydrant", 22 | "stop sign", "parking meter", "bench", "bird", "cat", "dog", 23 | "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", 24 | "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", 25 | "skis", "snowboard", "sports ball", "kite", "baseball bat", 26 | "baseball glove", "skateboard", "surfboard", "tennis racket", 27 | "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", 28 | "banana", "apple", "sandwich", "orange", "broccoli", "carrot", 29 | "hot dog", "pizza", "donut", "cake", "chair", "couch", 30 | "potted plant", "bed", "dining table", "toilet", "tv", "laptop", 31 | "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", 32 | "toaster", "sink", "refrigerator", "book", "clock", "vase", 33 | "scissors", "teddy bear", "hair drier", "toothbrush"}; 34 | 35 | class ncnnYolact { 36 | 37 | public: 38 | 39 | ncnn::Net neuralnet; 40 | 41 | int detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads); 42 | void draw(const cv::Mat& bgr, const std::vector& objects, double dT); 43 | 44 | }; 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_yolo.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_YOLO_HEADER_ 2 | #define _ROS_NCNN_YOLO_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | struct Object 11 | { 12 | cv::Rect_ rect; 13 | int label; 14 | float prob; 15 | }; 16 | 17 | static const char* class_names[] = {"background", 18 | "aeroplane", "bicycle", "bird", "boat", 19 | "bottle", "bus", "car", "cat", "chair", 20 | "cow", "diningtable", "dog", "horse", 21 | "motorbike", "person", "pottedplant", 22 | "sheep", "sofa", "train", "tvmonitor"}; 23 | 24 | class ncnnYolo { 25 | 26 | public: 27 | 28 | ncnn::Net neuralnet; 29 | 30 | int detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads); 31 | void draw(const cv::Mat& bgr, const std::vector& objects, double dT); 32 | 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/ros_ncnn/ncnn_yolov5.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_NCNN_YOLOV5_HEADER_ 2 | #define _ROS_NCNN_YOLOV5_HEADER_ 3 | 4 | #include "platform.h" 5 | #include "net.h" 6 | #include "cpu.h" 7 | 8 | #include "ros_ncnn/ncnn_utils.h" 9 | 10 | typedef struct { 11 | std::string name; 12 | int stride; 13 | std::vector anchors; 14 | } 15 | YoloLayerData; 16 | 17 | struct Object 18 | { 19 | cv::Rect_ rect; 20 | int label; 21 | float prob; 22 | }; 23 | 24 | static const char* class_names[] = {"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light", 25 | "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow", 26 | "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee", 27 | "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard", 28 | "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", 29 | "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch", 30 | "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone", 31 | "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", 32 | "hair drier", "toothbrush"}; 33 | 34 | class ncnnYoloV5 { 35 | public: 36 | 37 | ncnn::Net neuralnet; 38 | 39 | int detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads); 40 | void draw(const cv::Mat& bgr, const std::vector& objects, double dT); 41 | 42 | private: 43 | static std::vector decode_infer(ncnn::Mat &data, int stride,const cv::Size& frame_size, int net_size,int num_classes,const std::vector& anchors,float threshold); 44 | static void nms(std::vector& result,float nms_threshold); 45 | 46 | int num_class = 80; 47 | 48 | std::vector layers{ 49 | {"394",32,{{116,90},{156,198},{373,326}}}, 50 | {"375",16,{{30,61},{62,45},{59,119}}}, 51 | {"output",8,{{10,13},{16,30},{33,23}}}, 52 | }; 53 | 54 | }; 55 | 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /launch/hopenet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/retinaface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/ultraface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/yolact.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/yolo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/yolov5.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /msg/Euler.msg: -------------------------------------------------------------------------------- 1 | # Euler angles 2 | float32 roll 3 | float32 pitch 4 | float32 yaw 5 | -------------------------------------------------------------------------------- /msg/FaceObject.msg: -------------------------------------------------------------------------------- 1 | # Face bounding box with marker positions 2 | Header header 3 | Rectangle boundingbox 4 | Vector2D[] landmark 5 | float32 probability 6 | -------------------------------------------------------------------------------- /msg/Object.msg: -------------------------------------------------------------------------------- 1 | # Face bounding box with marker positions 2 | Header header 3 | Rectangle boundingbox 4 | string label 5 | float32 probability 6 | -------------------------------------------------------------------------------- /msg/Rectangle.msg: -------------------------------------------------------------------------------- 1 | # openCV style rectangle with size and 2D position 2 | Vector2D position 3 | Vector2D size 4 | -------------------------------------------------------------------------------- /msg/Vector2D.msg: -------------------------------------------------------------------------------- 1 | # simple 2D vector 2 | float32 x 3 | float32 y 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_ncnn 4 | 0.5.0 5 | ROS_NCNN Package 6 | 7 | nilseuropa 8 | GNU 9 | 10 | catkin 11 | 12 | roscpp 13 | roslib 14 | cv_bridge 15 | image_transport 16 | image_geometry 17 | sensor_msgs 18 | message_generation 19 | 20 | roscpp 21 | roslib 22 | cv_bridge 23 | image_transport 24 | image_geometry 25 | sensor_msgs 26 | message_generation 27 | 28 | message_runtime 29 | roscpp 30 | roslib 31 | cv_bridge 32 | image_transport 33 | image_geometry 34 | sensor_msgs 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/faster_rcnn_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | //////////////////////////////////// 15 | #include "ros_ncnn/ncnn_fast_rcnn.h" 16 | ncnnFastRcnn engine; 17 | //////////////////////////////////// 18 | 19 | std::vector objects; 20 | cv_bridge::CvImagePtr cv_ptr; 21 | ros::Time last_time; 22 | bool display_output; 23 | 24 | void print_objects(const std::vector& objects){ 25 | for (size_t i = 0; i < objects.size(); i++) 26 | { 27 | const Object& obj = objects[i]; 28 | if (obj.prob > 0.15) 29 | { 30 | ROS_INFO("%d = %.5f at %.2f %.2f %.2f x %.2f", obj.label, obj.prob, 31 | obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 32 | } 33 | } 34 | } 35 | 36 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 37 | { 38 | try { 39 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 40 | engine.detect(cv_ptr->image, objects, n_threads); 41 | print_objects(objects); 42 | 43 | if (display_output) { 44 | ros::Time current_time = ros::Time::now(); 45 | engine.draw(cv_ptr->image, objects, (current_time-last_time).toSec()); 46 | last_time = current_time; 47 | } 48 | } 49 | catch (cv_bridge::Exception& e) { 50 | ROS_ERROR("CV bridge exception: %s", e.what()); 51 | return; 52 | } 53 | } 54 | 55 | int main(int argc, char** argv) 56 | { 57 | ros::init(argc, argv, "yolact_node"); /**/ 58 | ros::NodeHandle nhLocal("~"); 59 | ros::NodeHandle n; 60 | std::string node_name = ros::this_node::getName(); 61 | int gpu_device; 62 | nhLocal.param("gpu_device", gpu_device, 0); 63 | 64 | #ifndef GPU_SUPPORT 65 | ROS_WARN_STREAM(node_name << " running on CPU"); 66 | #endif 67 | #ifdef GPU_SUPPORT 68 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 69 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 70 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 71 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 72 | engine.neuralnet.opt.use_vulkan_compute = true; 73 | engine.neuralnet.set_vulkan_device(g_vkdev); 74 | #endif 75 | 76 | const std::string package_name = "ros_ncnn"; 77 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 78 | ROS_INFO("Assets path: %s", path.c_str()); 79 | engine.neuralnet.load_param((path+("ZF_faster_rcnn_final.param")).c_str()); /**/ 80 | engine.neuralnet.load_model((path+("ZF_faster_rcnn_final.bin")).c_str()); /**/ 81 | 82 | nhLocal.param("display_output", display_output, true); 83 | 84 | int num_threads; 85 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 86 | image_transport::ImageTransport it(n); 87 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 88 | 89 | #ifdef GPU_SUPPORT 90 | ncnn::create_gpu_instance(); 91 | #endif 92 | while (ros::ok()) { 93 | ros::spinOnce(); 94 | } 95 | #ifdef GPU_SUPPORT 96 | ncnn::destroy_gpu_instance(); 97 | #endif 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /src/hopenet_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_hopenet.h" 16 | #include "ros_ncnn/FaceObject.h" 17 | #include "ros_ncnn/Euler.h" 18 | ncnnHopeNet engine; 19 | ///////////////////////////////// 20 | 21 | HeadPose head; 22 | cv_bridge::CvImagePtr cv_ptr; 23 | ros_ncnn::FaceObject face; 24 | ros::Publisher euler_pub; 25 | 26 | void boundingBoxUpdate(const ros_ncnn::FaceObject& msg) 27 | { 28 | cv::Rect roi; 29 | roi.x = msg.boundingbox.position.x; 30 | roi.y = msg.boundingbox.position.y; 31 | roi.width = msg.boundingbox.size.x; 32 | roi.height = msg.boundingbox.size.y; 33 | 34 | engine.detect(cv_ptr->image, roi, head); 35 | 36 | ros_ncnn::Euler euler; 37 | euler.roll = head.roll; 38 | euler.pitch = head.pitch; 39 | euler.yaw = head.yaw; 40 | 41 | euler_pub.publish(euler); 42 | } 43 | 44 | void imageCallback(const sensor_msgs::ImageConstPtr& msg) 45 | { 46 | try { 47 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 48 | } 49 | catch (cv_bridge::Exception& e) { 50 | ROS_ERROR("CV bridge exception: %s", e.what()); 51 | return; 52 | } 53 | } 54 | 55 | int main(int argc, char** argv) 56 | { 57 | ros::init(argc, argv, "hopenet_node"); 58 | ros::NodeHandle nhLocal("~"); 59 | ros::NodeHandle n; 60 | std::string node_name = ros::this_node::getName(); 61 | int gpu_device; 62 | nhLocal.param("gpu_device", gpu_device, 0); 63 | 64 | #ifndef GPU_SUPPORT 65 | ROS_WARN_STREAM(node_name << " running on CPU"); 66 | #endif 67 | #ifdef GPU_SUPPORT 68 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 69 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 70 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 71 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 72 | engine.neuralnet.opt.use_vulkan_compute = true; 73 | engine.neuralnet.set_vulkan_device(g_vkdev); 74 | #endif 75 | 76 | const std::string package_name = "ros_ncnn"; 77 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 78 | ROS_INFO("Assets path: %s", path.c_str()); 79 | engine.neuralnet.load_param((path+"hopenet.param").c_str()); 80 | engine.neuralnet.load_model((path+"hopenet.bin").c_str()); 81 | 82 | image_transport::ImageTransport it(n); 83 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, imageCallback); 84 | ros::Subscriber facebox_sub = n.subscribe("/retinaface_node/faces", 10, boundingBoxUpdate ); 85 | euler_pub = n.advertise(node_name+"/euler_angles", 10); 86 | engine.initialize(); 87 | 88 | #ifdef GPU_SUPPORT 89 | ncnn::create_gpu_instance(); 90 | #endif 91 | 92 | while (ros::ok()) { 93 | ros::spinOnce(); 94 | } 95 | 96 | #ifdef GPU_SUPPORT 97 | ncnn::destroy_gpu_instance(); 98 | #endif 99 | 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /src/ncnn_fast_rcnn.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_fast_rcnn.h" 3 | 4 | int ncnnFastRcnn::detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads) 5 | { 6 | 7 | const int target_size = 600;// __C.TEST.SCALES 8 | 9 | const int max_per_image = 100; 10 | const float confidence_thresh = 0.05f; 11 | 12 | const float nms_threshold = 0.3f;// __C.TEST.NMS 13 | 14 | // scale to target detect size 15 | int w = bgr.cols; 16 | int h = bgr.rows; 17 | float scale = 1.f; 18 | if (w < h) 19 | { 20 | scale = (float)target_size / w; 21 | w = target_size; 22 | h = h * scale; 23 | } 24 | else 25 | { 26 | scale = (float)target_size / h; 27 | h = target_size; 28 | w = w * scale; 29 | } 30 | 31 | ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR, bgr.cols, bgr.rows, w, h); 32 | 33 | const float mean_vals[3] = { 102.9801f, 115.9465f, 122.7717f }; 34 | in.substract_mean_normalize(mean_vals, 0); 35 | 36 | ncnn::Mat im_info(3); 37 | im_info[0] = h; 38 | im_info[1] = w; 39 | im_info[2] = scale; 40 | 41 | // step1, extract feature and all rois 42 | ncnn::Extractor ex1 = neuralnet.create_extractor(); 43 | ex1.set_num_threads(n_threads); 44 | 45 | ex1.input("data", in); 46 | ex1.input("im_info", im_info); 47 | 48 | ncnn::Mat conv5_relu5;// feature 49 | ncnn::Mat rois;// all rois 50 | ex1.extract("conv5_relu5", conv5_relu5); 51 | ex1.extract("rois", rois); 52 | 53 | // step2, extract bbox and score for each roi 54 | std::vector< std::vector > class_candidates; 55 | for (int i = 0; i < rois.c; i++) 56 | { 57 | ncnn::Extractor ex2 = neuralnet.create_extractor(); 58 | 59 | ncnn::Mat roi = rois.channel(i);// get single roi 60 | ex2.input("conv5_relu5", conv5_relu5); 61 | ex2.input("rois", roi); 62 | 63 | ncnn::Mat bbox_pred; 64 | ncnn::Mat cls_prob; 65 | ex2.extract("bbox_pred", bbox_pred); 66 | ex2.extract("cls_prob", cls_prob); 67 | 68 | int num_class = cls_prob.w; 69 | class_candidates.resize(num_class); 70 | 71 | // find class id with highest score 72 | int label = 0; 73 | float score = 0.f; 74 | for (int i=0; i score) 78 | { 79 | label = i; 80 | score = class_score; 81 | } 82 | } 83 | 84 | // ignore background or low score 85 | if (label == 0 || score <= confidence_thresh) 86 | continue; 87 | 88 | // fprintf(stderr, "%d = %f\n", label, score); 89 | 90 | // unscale to image size 91 | float x1 = roi[0] / scale; 92 | float y1 = roi[1] / scale; 93 | float x2 = roi[2] / scale; 94 | float y2 = roi[3] / scale; 95 | 96 | float pb_w = x2 - x1 + 1; 97 | float pb_h = y2 - y1 + 1; 98 | 99 | // apply bbox regression 100 | float dx = bbox_pred[label * 4]; 101 | float dy = bbox_pred[label * 4 + 1]; 102 | float dw = bbox_pred[label * 4 + 2]; 103 | float dh = bbox_pred[label * 4 + 3]; 104 | 105 | float cx = x1 + pb_w * 0.5f; 106 | float cy = y1 + pb_h * 0.5f; 107 | 108 | float obj_cx = cx + pb_w * dx; 109 | float obj_cy = cy + pb_h * dy; 110 | 111 | float obj_w = pb_w * exp(dw); 112 | float obj_h = pb_h * exp(dh); 113 | 114 | float obj_x1 = obj_cx - obj_w * 0.5f; 115 | float obj_y1 = obj_cy - obj_h * 0.5f; 116 | float obj_x2 = obj_cx + obj_w * 0.5f; 117 | float obj_y2 = obj_cy + obj_h * 0.5f; 118 | 119 | // clip 120 | obj_x1 = std::max(std::min(obj_x1, (float)(bgr.cols - 1)), 0.f); 121 | obj_y1 = std::max(std::min(obj_y1, (float)(bgr.rows - 1)), 0.f); 122 | obj_x2 = std::max(std::min(obj_x2, (float)(bgr.cols - 1)), 0.f); 123 | obj_y2 = std::max(std::min(obj_y2, (float)(bgr.rows - 1)), 0.f); 124 | 125 | // append object 126 | Object obj; 127 | obj.rect = cv::Rect_(obj_x1, obj_y1, obj_x2-obj_x1+1, obj_y2-obj_y1+1); 128 | obj.label = label; 129 | obj.prob = score; 130 | 131 | class_candidates[label].push_back(obj); 132 | } 133 | 134 | // post process 135 | objects.clear(); 136 | for (int i = 0; i < (int)class_candidates.size(); i++) 137 | { 138 | std::vector& candidates = class_candidates[i]; 139 | 140 | qsort_descent_inplace(candidates); 141 | 142 | std::vector picked; 143 | nms_sorted_bboxes(candidates, picked, nms_threshold); 144 | 145 | for (int j = 0; j < (int)picked.size(); j++) 146 | { 147 | int z = picked[j]; 148 | objects.push_back(candidates[z]); 149 | } 150 | } 151 | 152 | qsort_descent_inplace(objects); 153 | 154 | if (max_per_image > 0 && max_per_image < objects.size()) 155 | { 156 | objects.resize(max_per_image); 157 | } 158 | 159 | return 0; 160 | } 161 | 162 | void ncnnFastRcnn::draw(const cv::Mat& bgr, const std::vector& objects, double dT) 163 | { 164 | cv::Mat image = bgr.clone(); 165 | 166 | for (size_t i = 0; i < objects.size(); i++) 167 | { 168 | const Object& obj = objects[i]; 169 | 170 | fprintf(stderr, "%d = %.5f at %.2f %.2f %.2f x %.2f\n", obj.label, obj.prob, 171 | obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 172 | 173 | cv::rectangle(image, obj.rect, cv::Scalar(255, 0, 0)); 174 | 175 | char text[256]; 176 | sprintf(text, "%s %.1f%%", class_names[obj.label], obj.prob * 100); 177 | 178 | int baseLine = 0; 179 | cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); 180 | 181 | int x = obj.rect.x; 182 | int y = obj.rect.y - label_size.height - baseLine; 183 | if (y < 0) 184 | y = 0; 185 | if (x + label_size.width > image.cols) 186 | x = image.cols - label_size.width; 187 | 188 | cv::rectangle(image, cv::Rect(cv::Point(x, y), 189 | cv::Size(label_size.width, label_size.height + baseLine)), 190 | cv::Scalar(255, 255, 255), -1); 191 | 192 | cv::putText(image, text, cv::Point(x, y + label_size.height), 193 | cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 194 | } 195 | 196 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 197 | cv::imshow("Faster R-CNN", image); 198 | cv::waitKey(1); 199 | } 200 | -------------------------------------------------------------------------------- /src/ncnn_hopenet.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_hopenet.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define NEAR_0 1e-10 12 | #define ODIM 66 13 | 14 | void ncnnHopeNet::initialize() 15 | { 16 | for (uint i=1; i<67; i++) idx_tensor[i-1] = i; 17 | }; 18 | 19 | void ncnnHopeNet::softmax(float* z, size_t el) { 20 | double zmax = -INFINITY; 21 | double zsum = 0; 22 | for (size_t i = 0; i < el; i++) if (z[i] > zmax) zmax=z[i]; 23 | for (size_t i=0; i roi.width) 40 | { 41 | roi.x -= diff/2; 42 | roi.width = roi.height; 43 | } 44 | else if (roi.height < roi.width) 45 | { 46 | roi.y -= diff/2; 47 | roi.height = roi.width; 48 | } 49 | 50 | cv::Mat faceImg = bgr(roi); 51 | cv::Mat faceGray; 52 | cv::cvtColor(faceImg, faceGray, CV_BGR2GRAY); 53 | 54 | cv::Size input_geometry = cv::Size(48,48); 55 | cv::resize(faceGray, faceGrayResized, input_geometry); 56 | 57 | ncnn::Mat in = ncnn::Mat::from_pixels(faceGrayResized.data, ncnn::Mat::PIXEL_GRAY, 48, 48); 58 | ncnn::Extractor ex = neuralnet.create_extractor(); 59 | ex.input("data", in); 60 | 61 | ncnn::Mat output; 62 | ex.extract("hybridsequential0_multitask0_dense0_fwd", output); 63 | float* pred_pitch = output.range(0, ODIM); 64 | float* pred_roll = output.range(ODIM, ODIM*2); 65 | float* pred_yaw = output.range(ODIM*2, ODIM*3); 66 | 67 | softmax(pred_pitch, ODIM); 68 | softmax(pred_roll, ODIM); 69 | softmax(pred_yaw, ODIM); 70 | 71 | head_angles.pitch = getAngle(pred_pitch, ODIM); 72 | head_angles.roll = getAngle(pred_roll, ODIM); 73 | head_angles.yaw = getAngle(pred_yaw, ODIM); 74 | return 0; 75 | }; 76 | 77 | void ncnnHopeNet::draw(){ 78 | cv::imshow("HOPENET", faceGrayResized); 79 | cv::waitKey(1); 80 | }; 81 | -------------------------------------------------------------------------------- /src/ncnn_pfld.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_pfld.h" 3 | 4 | void ncnnPFLD::set_threads(uint8_t num){ 5 | num_threads = num; 6 | } 7 | 8 | int ncnnPFLD::detect(const cv::Mat& bgr, cv::Rect roi) 9 | { 10 | cv::Mat face_rect = bgr(roi); 11 | cv::resize(face_rect, face_rect, cv::Size(112, 112)); 12 | 13 | ncnn::Mat out; 14 | ncnn::Mat in = ncnn::Mat::from_pixels(face_rect.data, ncnn::Mat::PIXEL_BGR2RGB, 112, 112); 15 | 16 | in.substract_mean_normalize(0, norm_vals); 17 | ncnn::Extractor ex = neuralnet.create_extractor(); 18 | 19 | ex.set_num_threads(num_threads); 20 | ex.input("input_1", in); 21 | ex.extract("415", out); 22 | 23 | for (int j = 0; j < out.w; j++) { landmarks[j] = out[j]; } 24 | 25 | return 0; 26 | }; 27 | 28 | void ncnnPFLD::draw(const cv::Mat& bgr, cv::Rect roi){ 29 | 30 | cv::Mat image = bgr.clone(); 31 | // int capture_width = display_image.cols; 32 | // int capture_height = display_image.rows; 33 | 34 | for(int i = 0; i < num_landmarks / 2; i++){ 35 | cv::circle(image, cv::Point(landmarks[i * 2] * roi.width + roi.x, landmarks[i * 2 + 1] * roi.height + roi.y), 36 | 2,cv::Scalar(0, 0, 255), -1); 37 | } 38 | 39 | cv::imshow("PFDL", image); 40 | cv::waitKey(1); 41 | }; 42 | -------------------------------------------------------------------------------- /src/ncnn_posenet.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_posenet.h" 3 | 4 | int ncnnPoseNet::detect(const cv::Mat& bgr, std::vector& keypoints, uint8_t n_threads) 5 | { 6 | 7 | int w = bgr.cols; 8 | int h = bgr.rows; 9 | 10 | ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR2RGB, w, h, 192, 256); 11 | 12 | const float mean_vals[3] = {0.485f*255.f, 0.456f*255.f, 0.406f*255.f}; 13 | const float norm_vals[3] = {1/0.229f/255.f, 1/0.224f/255.f, 1/0.225f/255.f}; 14 | in.substract_mean_normalize(mean_vals, norm_vals); 15 | 16 | ncnn::Extractor ex = neuralnet.create_extractor(); 17 | 18 | ex.input("data", in); 19 | ex.set_num_threads(n_threads); 20 | 21 | ncnn::Mat out; 22 | ex.extract("conv3_fwd", out); 23 | 24 | // resolve point from heatmap 25 | keypoints.clear(); 26 | for (int p = 0; p < out.c; p++) 27 | { 28 | const ncnn::Mat m = out.channel(p); 29 | 30 | float max_prob = 0.f; 31 | int max_x = 0; 32 | int max_y = 0; 33 | for (int y = 0; y < out.h; y++) 34 | { 35 | const float* ptr = m.row(y); 36 | for (int x = 0; x < out.w; x++) 37 | { 38 | float prob = ptr[x]; 39 | if (prob > max_prob) 40 | { 41 | max_prob = prob; 42 | max_x = x; 43 | max_y = y; 44 | } 45 | } 46 | } 47 | 48 | KeyPoint keypoint; 49 | keypoint.p = cv::Point2f(max_x * w / (float)out.w, max_y * h / (float)out.h); 50 | keypoint.prob = max_prob; 51 | 52 | keypoints.push_back(keypoint); 53 | } 54 | 55 | return 0; 56 | }; 57 | 58 | 59 | void ncnnPoseNet::draw(const cv::Mat& bgr, const std::vector& keypoints, double dT) 60 | { 61 | cv::Mat image = bgr.clone(); 62 | 63 | // draw bone 64 | static const int joint_pairs[16][2] = { 65 | {0, 1}, {1, 3}, {0, 2}, {2, 4}, 66 | {5, 6}, {5, 7}, {7, 9}, {6, 8}, {8, 10}, 67 | {5, 11}, {6, 12}, {11, 12}, 68 | {11, 13}, {12, 14}, {13, 15}, {14, 16} 69 | }; 70 | 71 | for (int i = 0; i < 16; i++) 72 | { 73 | const KeyPoint& p1 = keypoints[ joint_pairs[i][0] ]; 74 | const KeyPoint& p2 = keypoints[ joint_pairs[i][1] ]; 75 | 76 | if (p1.prob < 0.2f || p2.prob < 0.2f) 77 | continue; 78 | 79 | cv::line(image, p1.p, p2.p, cv::Scalar(255, 0, 0), 2); 80 | } 81 | 82 | // draw joint 83 | for (size_t i = 0; i < keypoints.size(); i++) 84 | { 85 | const KeyPoint& keypoint = keypoints[i]; 86 | 87 | if (keypoint.prob < 0.2f) 88 | continue; 89 | 90 | cv::circle(image, keypoint.p, 3, cv::Scalar(0, 255, 0), -1); 91 | } 92 | 93 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 94 | cv::imshow("PoseNet", image); 95 | cv::waitKey(1); 96 | }; 97 | -------------------------------------------------------------------------------- /src/ncnn_retinaface.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_retinaface.h" 3 | 4 | static ncnn::Mat generate_anchors(int base_size, const ncnn::Mat& ratios, const ncnn::Mat& scales) 5 | { 6 | int num_ratio = ratios.w; 7 | int num_scale = scales.w; 8 | 9 | ncnn::Mat anchors; 10 | anchors.create(4, num_ratio * num_scale); 11 | 12 | const float cx = base_size * 0.5f; 13 | const float cy = base_size * 0.5f; 14 | 15 | for (int i = 0; i < num_ratio; i++) 16 | { 17 | float ar = ratios[i]; 18 | 19 | int r_w = round(base_size / sqrt(ar)); 20 | int r_h = round(r_w * ar); 21 | 22 | for (int j = 0; j < num_scale; j++) 23 | { 24 | float scale = scales[j]; 25 | 26 | float rs_w = r_w * scale; 27 | float rs_h = r_h * scale; 28 | 29 | float* anchor = anchors.row(i * num_scale + j); 30 | 31 | anchor[0] = cx - rs_w * 0.5f; 32 | anchor[1] = cy - rs_h * 0.5f; 33 | anchor[2] = cx + rs_w * 0.5f; 34 | anchor[3] = cy + rs_h * 0.5f; 35 | } 36 | } 37 | 38 | return anchors; 39 | } 40 | 41 | static void generate_proposals(const ncnn::Mat& anchors, int feat_stride, const ncnn::Mat& score_blob, const ncnn::Mat& bbox_blob, const ncnn::Mat& landmark_blob, float prob_threshold, std::vector& faceobjects) 42 | { 43 | int w = score_blob.w; 44 | int h = score_blob.h; 45 | 46 | // generate face proposal from bbox deltas and shifted anchors 47 | const int num_anchors = anchors.h; 48 | 49 | for (int q=0; q= prob_threshold) 74 | { 75 | // apply center size 76 | float dx = bbox.channel(0)[index]; 77 | float dy = bbox.channel(1)[index]; 78 | float dw = bbox.channel(2)[index]; 79 | float dh = bbox.channel(3)[index]; 80 | 81 | float cx = anchor_x + anchor_w * 0.5f; 82 | float cy = anchor_y + anchor_h * 0.5f; 83 | 84 | float pb_cx = cx + anchor_w * dx; 85 | float pb_cy = cy + anchor_h * dy; 86 | 87 | float pb_w = anchor_w * exp(dw); 88 | float pb_h = anchor_h * exp(dh); 89 | 90 | float x0 = pb_cx - pb_w * 0.5f; 91 | float y0 = pb_cy - pb_h * 0.5f; 92 | float x1 = pb_cx + pb_w * 0.5f; 93 | float y1 = pb_cy + pb_h * 0.5f; 94 | 95 | FaceObject obj; 96 | obj.rect.x = x0; 97 | obj.rect.y = y0; 98 | obj.rect.width = x1 - x0 + 1; 99 | obj.rect.height = y1 - y0 + 1; 100 | obj.landmark[0].x = cx + (anchor_w + 1) * landmark.channel(0)[index]; 101 | obj.landmark[0].y = cy + (anchor_h + 1) * landmark.channel(1)[index]; 102 | obj.landmark[1].x = cx + (anchor_w + 1) * landmark.channel(2)[index]; 103 | obj.landmark[1].y = cy + (anchor_h + 1) * landmark.channel(3)[index]; 104 | obj.landmark[2].x = cx + (anchor_w + 1) * landmark.channel(4)[index]; 105 | obj.landmark[2].y = cy + (anchor_h + 1) * landmark.channel(5)[index]; 106 | obj.landmark[3].x = cx + (anchor_w + 1) * landmark.channel(6)[index]; 107 | obj.landmark[3].y = cy + (anchor_h + 1) * landmark.channel(7)[index]; 108 | obj.landmark[4].x = cx + (anchor_w + 1) * landmark.channel(8)[index]; 109 | obj.landmark[4].y = cy + (anchor_h + 1) * landmark.channel(9)[index]; 110 | obj.prob = prob; 111 | 112 | faceobjects.push_back(obj); 113 | } 114 | 115 | anchor_x += feat_stride; 116 | } 117 | 118 | anchor_y += feat_stride; 119 | } 120 | } 121 | 122 | } 123 | 124 | ///////////////// 125 | 126 | int ncnnRetinaface::detect(const cv::Mat& bgr, std::vector& faceobjects, uint8_t n_threads) 127 | { 128 | 129 | const float prob_threshold = 0.8f; // TODO: param / dynamic reconf. 130 | const float nms_threshold = 0.4f; 131 | 132 | int img_w = bgr.cols; 133 | int img_h = bgr.rows; 134 | 135 | ncnn::Mat in = ncnn::Mat::from_pixels(bgr.data, ncnn::Mat::PIXEL_BGR2RGB, img_w, img_h); 136 | 137 | ncnn::Extractor ex = neuralnet.create_extractor(); 138 | ex.set_num_threads(n_threads); 139 | ex.input("data", in); 140 | 141 | std::vector faceproposals; 142 | 143 | // stride 32 144 | { 145 | ncnn::Mat score_blob, bbox_blob, landmark_blob; 146 | ex.extract("face_rpn_cls_prob_reshape_stride32", score_blob); 147 | ex.extract("face_rpn_bbox_pred_stride32", bbox_blob); 148 | ex.extract("face_rpn_landmark_pred_stride32", landmark_blob); 149 | 150 | const int base_size = 16; 151 | const int feat_stride = 32; 152 | ncnn::Mat ratios(1); 153 | ratios[0] = 1.f; 154 | ncnn::Mat scales(2); 155 | scales[0] = 32.f; 156 | scales[1] = 16.f; 157 | ncnn::Mat anchors = generate_anchors(base_size, ratios, scales); 158 | 159 | std::vector faceobjects32; 160 | generate_proposals(anchors, feat_stride, score_blob, bbox_blob, landmark_blob, prob_threshold, faceobjects32); 161 | 162 | faceproposals.insert(faceproposals.end(), faceobjects32.begin(), faceobjects32.end()); 163 | } 164 | 165 | // stride 16 166 | { 167 | ncnn::Mat score_blob, bbox_blob, landmark_blob; 168 | ex.extract("face_rpn_cls_prob_reshape_stride16", score_blob); 169 | ex.extract("face_rpn_bbox_pred_stride16", bbox_blob); 170 | ex.extract("face_rpn_landmark_pred_stride16", landmark_blob); 171 | 172 | const int base_size = 16; 173 | const int feat_stride = 16; 174 | ncnn::Mat ratios(1); 175 | ratios[0] = 1.f; 176 | ncnn::Mat scales(2); 177 | scales[0] = 8.f; 178 | scales[1] = 4.f; 179 | ncnn::Mat anchors = generate_anchors(base_size, ratios, scales); 180 | 181 | std::vector faceobjects16; 182 | generate_proposals(anchors, feat_stride, score_blob, bbox_blob, landmark_blob, prob_threshold, faceobjects16); 183 | 184 | faceproposals.insert(faceproposals.end(), faceobjects16.begin(), faceobjects16.end()); 185 | } 186 | 187 | // stride 8 188 | { 189 | ncnn::Mat score_blob, bbox_blob, landmark_blob; 190 | ex.extract("face_rpn_cls_prob_reshape_stride8", score_blob); 191 | ex.extract("face_rpn_bbox_pred_stride8", bbox_blob); 192 | ex.extract("face_rpn_landmark_pred_stride8", landmark_blob); 193 | 194 | const int base_size = 16; 195 | const int feat_stride = 8; 196 | ncnn::Mat ratios(1); 197 | ratios[0] = 1.f; 198 | ncnn::Mat scales(2); 199 | scales[0] = 2.f; 200 | scales[1] = 1.f; 201 | ncnn::Mat anchors = generate_anchors(base_size, ratios, scales); 202 | 203 | std::vector faceobjects8; 204 | generate_proposals(anchors, feat_stride, score_blob, bbox_blob, landmark_blob, prob_threshold, faceobjects8); 205 | 206 | faceproposals.insert(faceproposals.end(), faceobjects8.begin(), faceobjects8.end()); 207 | } 208 | 209 | // sort all proposals by score from highest to lowest 210 | qsort_descent_inplace(faceproposals); 211 | 212 | // apply nms with nms_threshold 213 | std::vector picked; 214 | nms_sorted_bboxes(faceproposals, picked, nms_threshold); 215 | 216 | int face_count = picked.size(); 217 | 218 | faceobjects.resize(face_count); 219 | for (int i = 0; i < face_count; i++) 220 | { 221 | faceobjects[i] = faceproposals[ picked[i] ]; 222 | 223 | // clip to image size 224 | float x0 = faceobjects[i].rect.x; 225 | float y0 = faceobjects[i].rect.y; 226 | float x1 = x0 + faceobjects[i].rect.width; 227 | float y1 = y0 + faceobjects[i].rect.height; 228 | 229 | x0 = std::max(std::min(x0, (float)img_w - 1), 0.f); 230 | y0 = std::max(std::min(y0, (float)img_h - 1), 0.f); 231 | x1 = std::max(std::min(x1, (float)img_w - 1), 0.f); 232 | y1 = std::max(std::min(y1, (float)img_h - 1), 0.f); 233 | 234 | faceobjects[i].rect.x = x0; 235 | faceobjects[i].rect.y = y0; 236 | faceobjects[i].rect.width = x1 - x0; 237 | faceobjects[i].rect.height = y1 - y0; 238 | } 239 | 240 | return 0; 241 | } 242 | 243 | void ncnnRetinaface::draw(const cv::Mat& bgr, const std::vector& faceobjects, double dT) 244 | { 245 | cv::Mat image = bgr.clone(); 246 | 247 | for (size_t i = 0; i < faceobjects.size(); i++) 248 | { 249 | const FaceObject& obj = faceobjects[i]; 250 | 251 | cv::rectangle(image, obj.rect, cv::Scalar(0, 255, 0)); 252 | 253 | cv::circle(image, obj.landmark[0], 2, cv::Scalar(0, 255, 255), -1); 254 | cv::circle(image, obj.landmark[1], 2, cv::Scalar(0, 255, 255), -1); 255 | cv::circle(image, obj.landmark[2], 2, cv::Scalar(0, 255, 255), -1); 256 | cv::circle(image, obj.landmark[3], 2, cv::Scalar(0, 255, 255), -1); 257 | cv::circle(image, obj.landmark[4], 2, cv::Scalar(0, 255, 255), -1); 258 | 259 | char text[256]; 260 | sprintf(text, "%.1f%%", obj.prob * 100); 261 | 262 | int baseLine = 0; 263 | cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); 264 | 265 | int x = obj.rect.x; 266 | int y = obj.rect.y - label_size.height - baseLine; 267 | if (y < 0) 268 | y = 0; 269 | if (x + label_size.width > image.cols) 270 | x = image.cols - label_size.width; 271 | 272 | cv::rectangle(image, cv::Rect(cv::Point(x, y), 273 | cv::Size(label_size.width, label_size.height + baseLine)), 274 | cv::Scalar(255, 255, 255), -1); 275 | 276 | cv::putText(image, text, cv::Point(x, y + label_size.height), 277 | cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 278 | } 279 | 280 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 281 | cv::imshow("RETINAFACE", image); 282 | cv::waitKey(1); 283 | } 284 | -------------------------------------------------------------------------------- /src/ncnn_ultraface.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_ultraface.h" 3 | 4 | #include 5 | 6 | #define clip(x, y) (x < 0 ? 0 : (x > y ? y : x)) 7 | 8 | void ncnnUltraFace::init(int input_width, int input_length, float score_threshold_, float iou_threshold_) { 9 | 10 | score_threshold = score_threshold_; 11 | iou_threshold = iou_threshold_; 12 | in_w = input_width; 13 | in_h = input_length; 14 | w_h_list = {in_w, in_h}; 15 | 16 | for (auto size : w_h_list) { 17 | std::vector fm_item; 18 | for (float stride : strides) { 19 | fm_item.push_back(ceil(size / stride)); 20 | } 21 | featuremap_size.push_back(fm_item); 22 | } 23 | 24 | for (auto size : w_h_list) { 25 | shrinkage_size.push_back(strides); 26 | } 27 | 28 | /* generate prior anchors */ 29 | for (int index = 0; index < num_featuremap; index++) { 30 | float scale_w = in_w / shrinkage_size[0][index]; 31 | float scale_h = in_h / shrinkage_size[1][index]; 32 | for (int j = 0; j < featuremap_size[1][index]; j++) { 33 | for (int i = 0; i < featuremap_size[0][index]; i++) { 34 | float x_center = (i + 0.5) / scale_w; 35 | float y_center = (j + 0.5) / scale_h; 36 | 37 | for (float k : min_boxes[index]) { 38 | float w = k / in_w; 39 | float h = k / in_h; 40 | priors.push_back({clip(x_center, 1), clip(y_center, 1), clip(w, 1), clip(h, 1)}); 41 | } 42 | } 43 | } 44 | } 45 | num_anchors = priors.size(); 46 | } 47 | 48 | int ncnnUltraFace::detect(const cv::Mat& bgr, std::vector &face_list, uint8_t n_threads) { 49 | 50 | image_w = bgr.cols; 51 | image_h = bgr.rows; 52 | ncnn::Mat in = ncnn::Mat::from_pixels(bgr.data, ncnn::Mat::PIXEL_BGR2RGB, image_w, image_h); 53 | ncnn::Mat ncnn_img; 54 | 55 | ncnn::resize_bilinear(in, ncnn_img, in_w, in_h); 56 | ncnn_img.substract_mean_normalize(mean_vals, norm_vals); 57 | 58 | std::vector bbox_collection; 59 | 60 | ncnn::Extractor ex = neuralnet.create_extractor(); 61 | ex.set_num_threads(n_threads); 62 | ex.input("input", ncnn_img); 63 | 64 | ncnn::Mat scores; 65 | ncnn::Mat boxes; 66 | ex.extract("scores", scores); 67 | ex.extract("boxes", boxes); 68 | generateBBox(bbox_collection, scores, boxes, score_threshold, num_anchors); 69 | nms(bbox_collection, face_list); 70 | 71 | return 0; 72 | } 73 | 74 | void ncnnUltraFace::generateBBox(std::vector &bbox_collection, ncnn::Mat scores, ncnn::Mat boxes, float score_threshold, int num_anchors) { 75 | for (int i = 0; i < num_anchors; i++) { 76 | if (scores.channel(0)[i * 2 + 1] > score_threshold) { 77 | 78 | FaceInfo rects; 79 | float x_center = boxes.channel(0)[i * 4] * center_variance * priors[i][2] + priors[i][0]; 80 | float y_center = boxes.channel(0)[i * 4 + 1] * center_variance * priors[i][3] + priors[i][1]; 81 | float w = exp(boxes.channel(0)[i * 4 + 2] * size_variance) * priors[i][2]; 82 | float h = exp(boxes.channel(0)[i * 4 + 3] * size_variance) * priors[i][3]; 83 | 84 | rects.x1 = clip(x_center - w / 2.0, 1) * image_w; 85 | rects.y1 = clip(y_center - h / 2.0, 1) * image_h; 86 | rects.x2 = clip(x_center + w / 2.0, 1) * image_w; 87 | rects.y2 = clip(y_center + h / 2.0, 1) * image_h; 88 | rects.score = clip(scores.channel(0)[i * 2 + 1], 1); 89 | 90 | bbox_collection.push_back(rects); 91 | } 92 | } 93 | } 94 | 95 | void ncnnUltraFace::draw(const cv::Mat& bgr, const std::vector& face_info, double dT){ 96 | cv::Mat image = bgr.clone(); 97 | 98 | for (long unsigned int i = 0; i < face_info.size(); i++) { 99 | auto face = face_info[i]; 100 | 101 | cv::Point pt1(face.x1, face.y1); 102 | cv::Point pt2(face.x2, face.y2); 103 | cv::rectangle(image, pt1, pt2, cv::Scalar(0, 0, 255), 2); 104 | } 105 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 106 | cv::imshow("ULTRAFACE", image); 107 | cv::waitKey(1); 108 | } 109 | 110 | void ncnnUltraFace::nms(std::vector &input, std::vector &output, int type) { 111 | std::sort(input.begin(), input.end(), [](const FaceInfo &a, const FaceInfo &b) { return a.score > b.score; }); 112 | 113 | int box_num = input.size(); 114 | 115 | std::vector merged(box_num, 0); 116 | 117 | for (int i = 0; i < box_num; i++) { 118 | if (merged[i]) 119 | continue; 120 | std::vector buf; 121 | 122 | buf.push_back(input[i]); 123 | merged[i] = 1; 124 | 125 | float h0 = input[i].y2 - input[i].y1 + 1; 126 | float w0 = input[i].x2 - input[i].x1 + 1; 127 | 128 | float area0 = h0 * w0; 129 | 130 | for (int j = i + 1; j < box_num; j++) { 131 | if (merged[j]) 132 | continue; 133 | 134 | float inner_x0 = input[i].x1 > input[j].x1 ? input[i].x1 : input[j].x1; 135 | float inner_y0 = input[i].y1 > input[j].y1 ? input[i].y1 : input[j].y1; 136 | 137 | float inner_x1 = input[i].x2 < input[j].x2 ? input[i].x2 : input[j].x2; 138 | float inner_y1 = input[i].y2 < input[j].y2 ? input[i].y2 : input[j].y2; 139 | 140 | float inner_h = inner_y1 - inner_y0 + 1; 141 | float inner_w = inner_x1 - inner_x0 + 1; 142 | 143 | if (inner_h <= 0 || inner_w <= 0) 144 | continue; 145 | 146 | float inner_area = inner_h * inner_w; 147 | 148 | float h1 = input[j].y2 - input[j].y1 + 1; 149 | float w1 = input[j].x2 - input[j].x1 + 1; 150 | 151 | float area1 = h1 * w1; 152 | 153 | float score; 154 | 155 | score = inner_area / (area0 + area1 - inner_area); 156 | 157 | if (score > iou_threshold) { 158 | merged[j] = 1; 159 | buf.push_back(input[j]); 160 | } 161 | } 162 | switch (type) { 163 | case hard_nms: { 164 | output.push_back(buf[0]); 165 | break; 166 | } 167 | case blending_nms: { 168 | float total = 0; 169 | for (int i = 0; i < buf.size(); i++) { 170 | total += exp(buf[i].score); 171 | } 172 | FaceInfo rects; 173 | memset(&rects, 0, sizeof(rects)); 174 | for (int i = 0; i < buf.size(); i++) { 175 | float rate = exp(buf[i].score) / total; 176 | rects.x1 += buf[i].x1 * rate; 177 | rects.y1 += buf[i].y1 * rate; 178 | rects.x2 += buf[i].x2 * rate; 179 | rects.y2 += buf[i].y2 * rate; 180 | rects.score += buf[i].score * rate; 181 | } 182 | output.push_back(rects); 183 | break; 184 | } 185 | default: { 186 | printf("wrong type of nms."); 187 | exit(-1); 188 | } 189 | } 190 | } 191 | } 192 | -------------------------------------------------------------------------------- /src/ncnn_yolact.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_yolact.h" 3 | 4 | int ncnnYolact::detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads) 5 | { 6 | 7 | const int target_size = 550; 8 | 9 | int img_w = bgr.cols; 10 | int img_h = bgr.rows; 11 | 12 | ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR2RGB, img_w, img_h, target_size, target_size); 13 | 14 | const float mean_vals[3] = {123.68f, 116.78f, 103.94f}; 15 | const float norm_vals[3] = {1.0/58.40f, 1.0/57.12f, 1.0/57.38f}; 16 | in.substract_mean_normalize(mean_vals, norm_vals); 17 | 18 | ncnn::Extractor ex = neuralnet.create_extractor(); 19 | ex.set_num_threads(n_threads); 20 | 21 | ex.input("input.1", in); 22 | 23 | ncnn::Mat maskmaps; 24 | ncnn::Mat location; 25 | ncnn::Mat mask; 26 | ncnn::Mat confidence; 27 | 28 | ex.extract("619", maskmaps);// 138x138 x 32 29 | 30 | ex.extract("816", location);// 4 x 19248 31 | ex.extract("818", mask);// maskdim 32 x 19248 32 | ex.extract("820", confidence);// 81 x 19248 33 | 34 | int num_class = confidence.w; 35 | int num_priors = confidence.h; 36 | 37 | // make priorbox 38 | ncnn::Mat priorbox(4, num_priors); 39 | { 40 | const int conv_ws[5] = {69, 35, 18, 9, 5}; 41 | const int conv_hs[5] = {69, 35, 18, 9, 5}; 42 | 43 | const float aspect_ratios[3] = {1.f, 0.5f, 2.f}; 44 | const float scales[5] = {24.f, 48.f, 96.f, 192.f, 384.f}; 45 | 46 | float* pb = priorbox; 47 | 48 | for (int p = 0; p < 5; p++) 49 | { 50 | int conv_w = conv_ws[p]; 51 | int conv_h = conv_hs[p]; 52 | 53 | float scale = scales[p]; 54 | 55 | for (int i = 0; i < conv_h; i++) 56 | { 57 | for (int j = 0; j < conv_w; j++) 58 | { 59 | // +0.5 because priors are in center-size notation 60 | float cx = (j + 0.5f) / conv_w; 61 | float cy = (i + 0.5f) / conv_h; 62 | 63 | for (int k = 0; k < 3; k++) 64 | { 65 | float ar = aspect_ratios[k]; 66 | 67 | ar = sqrt(ar); 68 | 69 | float w = scale * ar / 550; 70 | float h = scale / ar / 550; 71 | 72 | // This is for backward compatability with a bug where I made everything square by accident 73 | // cfg.backbone.use_square_anchors: 74 | h = w; 75 | 76 | pb[0] = cx; 77 | pb[1] = cy; 78 | pb[2] = w; 79 | pb[3] = h; 80 | 81 | pb += 4; 82 | } 83 | } 84 | } 85 | } 86 | } 87 | 88 | const float confidence_thresh = 0.05f; 89 | const float nms_threshold = 0.5f; 90 | const int keep_top_k = 200; 91 | 92 | std::vector< std::vector > class_candidates; 93 | class_candidates.resize(num_class); 94 | 95 | for (int i = 0; i < num_priors; i++) 96 | { 97 | const float* conf = confidence.row(i); 98 | const float* loc = location.row(i); 99 | const float* pb = priorbox.row(i); 100 | const float* maskdata = mask.row(i); 101 | 102 | // find class id with highest score 103 | // start from 1 to skip background 104 | int label = 0; 105 | float score = 0.f; 106 | for (int j=1; j score) 110 | { 111 | label = j; 112 | score = class_score; 113 | } 114 | } 115 | 116 | // ignore background or low score 117 | if (label == 0 || score <= confidence_thresh) 118 | continue; 119 | 120 | // CENTER_SIZE 121 | float var[4] = {0.1f, 0.1f, 0.2f, 0.2f}; 122 | 123 | float pb_cx = pb[0]; 124 | float pb_cy = pb[1]; 125 | float pb_w = pb[2]; 126 | float pb_h = pb[3]; 127 | 128 | float bbox_cx = var[0] * loc[0] * pb_w + pb_cx; 129 | float bbox_cy = var[1] * loc[1] * pb_h + pb_cy; 130 | float bbox_w = (float)(exp(var[2] * loc[2]) * pb_w); 131 | float bbox_h = (float)(exp(var[3] * loc[3]) * pb_h); 132 | 133 | float obj_x1 = bbox_cx - bbox_w * 0.5f; 134 | float obj_y1 = bbox_cy - bbox_h * 0.5f; 135 | float obj_x2 = bbox_cx + bbox_w * 0.5f; 136 | float obj_y2 = bbox_cy + bbox_h * 0.5f; 137 | 138 | // clip 139 | obj_x1 = std::max(std::min(obj_x1 * bgr.cols, (float)(bgr.cols - 1)), 0.f); 140 | obj_y1 = std::max(std::min(obj_y1 * bgr.rows, (float)(bgr.rows - 1)), 0.f); 141 | obj_x2 = std::max(std::min(obj_x2 * bgr.cols, (float)(bgr.cols - 1)), 0.f); 142 | obj_y2 = std::max(std::min(obj_y2 * bgr.rows, (float)(bgr.rows - 1)), 0.f); 143 | 144 | // append object 145 | Object obj; 146 | obj.rect = cv::Rect_(obj_x1, obj_y1, obj_x2-obj_x1+1, obj_y2-obj_y1+1); 147 | obj.label = label; 148 | obj.prob = score; 149 | obj.maskdata = std::vector(maskdata, maskdata + mask.w); 150 | 151 | class_candidates[label].push_back(obj); 152 | } 153 | 154 | objects.clear(); 155 | for (int i = 0; i < (int)class_candidates.size(); i++) 156 | { 157 | std::vector& candidates = class_candidates[i]; 158 | 159 | qsort_descent_inplace(candidates); 160 | 161 | std::vector picked; 162 | nms_sorted_bboxes(candidates, picked, nms_threshold); 163 | 164 | for (int j = 0; j < (int)picked.size(); j++) 165 | { 166 | int z = picked[j]; 167 | objects.push_back(candidates[z]); 168 | } 169 | } 170 | 171 | qsort_descent_inplace(objects); 172 | 173 | // keep_top_k 174 | if (keep_top_k < (int)objects.size()) 175 | { 176 | objects.resize(keep_top_k); 177 | } 178 | 179 | // generate mask 180 | for (int i=0; i obj.rect.y + obj.rect.height) 213 | continue; 214 | 215 | const float* mp2 = mask2.ptr(y); 216 | uchar* bmp = obj.mask.ptr(y); 217 | 218 | for (int x=0; x obj.rect.x + obj.rect.width) 221 | continue; 222 | 223 | bmp[x] = mp2[x] > 0.5f ? 255 : 0; 224 | } 225 | } 226 | } 227 | } 228 | 229 | return 0; 230 | } 231 | 232 | void ncnnYolact::draw(const cv::Mat& bgr, const std::vector& objects, double dT) 233 | { 234 | 235 | static const unsigned char colors[19][3] = { 236 | {244, 67, 54}, 237 | {233, 30, 99}, 238 | {156, 39, 176}, 239 | {103, 58, 183}, 240 | { 63, 81, 181}, 241 | { 33, 150, 243}, 242 | { 3, 169, 244}, 243 | { 0, 188, 212}, 244 | { 0, 150, 136}, 245 | { 76, 175, 80}, 246 | {139, 195, 74}, 247 | {205, 220, 57}, 248 | {255, 235, 59}, 249 | {255, 193, 7}, 250 | {255, 152, 0}, 251 | {255, 87, 34}, 252 | {121, 85, 72}, 253 | {158, 158, 158}, 254 | { 96, 125, 139} 255 | }; 256 | 257 | cv::Mat image = bgr.clone(); 258 | 259 | int color_index = 0; 260 | 261 | for (size_t i = 0; i < objects.size(); i++) 262 | { 263 | const Object& obj = objects[i]; 264 | 265 | if (obj.prob < 0.15) 266 | continue; 267 | 268 | const unsigned char* color = colors[color_index++]; 269 | 270 | cv::rectangle(image, obj.rect, cv::Scalar(color[0], color[1], color[2])); 271 | 272 | char text[256]; 273 | sprintf(text, "%s %.1f%%", class_names[obj.label], obj.prob * 100); 274 | 275 | int baseLine = 0; 276 | cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); 277 | 278 | int x = obj.rect.x; 279 | int y = obj.rect.y - label_size.height - baseLine; 280 | if (y < 0) 281 | y = 0; 282 | if (x + label_size.width > image.cols) 283 | x = image.cols - label_size.width; 284 | 285 | cv::rectangle(image, cv::Rect(cv::Point(x, y), 286 | cv::Size(label_size.width, label_size.height + baseLine)), 287 | cv::Scalar(255, 255, 255), -1); 288 | 289 | cv::putText(image, text, cv::Point(x, y + label_size.height), 290 | cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 291 | 292 | // draw mask 293 | for (int y=0; y(p[0] * 0.5 + color[0] * 0.5); 302 | p[1] = cv::saturate_cast(p[1] * 0.5 + color[1] * 0.5); 303 | p[2] = cv::saturate_cast(p[2] * 0.5 + color[2] * 0.5); 304 | } 305 | p += 3; 306 | } 307 | } 308 | } 309 | 310 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 311 | cv::imshow("YOLACT", image); 312 | cv::waitKey(1); 313 | } 314 | -------------------------------------------------------------------------------- /src/ncnn_yolo.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_yolo.h" 3 | 4 | int ncnnYolo::detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads) 5 | { 6 | const int target_size = 352; 7 | 8 | int img_w = bgr.cols; 9 | int img_h = bgr.rows; 10 | 11 | ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR, bgr.cols, bgr.rows, target_size, target_size); 12 | 13 | const float mean_vals[3] = {127.5f, 127.5f, 127.5f}; 14 | const float norm_vals[3] = {0.007843f, 0.007843f, 0.007843f}; 15 | in.substract_mean_normalize(mean_vals, norm_vals); 16 | 17 | ncnn::Extractor ex = neuralnet.create_extractor(); 18 | ex.set_num_threads(n_threads); 19 | 20 | ex.input("data", in); 21 | 22 | ncnn::Mat out; 23 | ex.extract("detection_out", out); 24 | 25 | objects.clear(); 26 | for (int i=0; i& objects, double dT) 45 | { 46 | cv::Mat image = bgr.clone(); 47 | 48 | for (size_t i = 0; i < objects.size(); i++) 49 | { 50 | const Object& obj = objects[i]; 51 | 52 | cv::rectangle(image, obj.rect, cv::Scalar(255, 0, 0)); 53 | 54 | char text[256]; 55 | sprintf(text, "%s %.1f%%", class_names[obj.label], obj.prob * 100); 56 | 57 | int baseLine = 0; 58 | cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); 59 | 60 | int x = obj.rect.x; 61 | int y = obj.rect.y - label_size.height - baseLine; 62 | if (y < 0) 63 | y = 0; 64 | if (x + label_size.width > image.cols) 65 | x = image.cols - label_size.width; 66 | 67 | cv::rectangle(image, cv::Rect(cv::Point(x, y), 68 | cv::Size(label_size.width, label_size.height + baseLine)), 69 | cv::Scalar(255, 255, 255), -1); 70 | 71 | cv::putText(image, text, cv::Point(x, y + label_size.height), 72 | cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 73 | } 74 | 75 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 76 | cv::imshow("YOLO", image); 77 | cv::waitKey(1); 78 | } 79 | -------------------------------------------------------------------------------- /src/ncnn_yolov5.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_ncnn/ncnn_utils.h" 2 | #include "ros_ncnn/ncnn_yolov5.h" 3 | 4 | inline float fast_exp(float x) 5 | { 6 | union {uint32_t i;float f;} v{}; 7 | v.i=(1<<23)*(1.4426950409*x+126.93490512f); 8 | return v.f; 9 | } 10 | 11 | inline float sigmoid(float x){ 12 | return 1.0f / (1.0f + fast_exp(-x)); 13 | } 14 | 15 | std::vector 16 | ncnnYoloV5::decode_infer(ncnn::Mat &data, int stride, const cv::Size &frame_size, int net_size, int num_classes,const std::vector& anchors, float threshold) { 17 | std::vector result; 18 | int grid_size = int(sqrt(data.h)); 19 | float *mat_data[data.c]; 20 | for(int i=0;ithreshold){ 33 | cx = (sigmoid(record[1]) * 2.f - 0.5f + (float)shift_x) * (float) stride; 34 | cy = (sigmoid(record[2]) * 2.f - 0.5f + (float)shift_y) * (float) stride; 35 | w = pow(sigmoid(record[2]) * 2.f,2)*anchors[i].width; 36 | h = pow(sigmoid(record[3]) * 2.f,2)*anchors[i].height; 37 | Object object; 38 | object.rect.width = w; 39 | object.rect.height = h; 40 | object.rect.x = std::max(0,std::min(frame_size.width,int((cx / 2.f) * (float)frame_size.width / (float)net_size))) - w/2; 41 | object.rect.y = std::max(0,std::min(frame_size.height,int((cy / 2.f) * (float)frame_size.height / (float)net_size))) -h/2; 42 | object.prob = score; 43 | object.label = cls; 44 | result.push_back(object); 45 | } 46 | } 47 | } 48 | for(auto& ptr:mat_data){ 49 | ptr+=(num_classes + 5); 50 | } 51 | } 52 | } 53 | return result; 54 | } 55 | 56 | void ncnnYoloV5::nms(std::vector &input_boxes, float NMS_THRESH) { 57 | std::sort(input_boxes.begin(), input_boxes.end(), [](Object a, Object b){return a.prob > b.prob;}); 58 | std::vectorvArea(input_boxes.size()); 59 | for (int i = 0; i < int(input_boxes.size()); ++i) 60 | { 61 | vArea[i] = input_boxes.at(i).rect.width * input_boxes.at(i).rect.height; 62 | } 63 | for (int i = 0; i < int(input_boxes.size()); ++i) 64 | { 65 | for (int j = i + 1; j < int(input_boxes.size());) 66 | { 67 | float xx1 = std::max(input_boxes[i].rect.x, input_boxes[j].rect.x); 68 | float yy1 = std::max(input_boxes[i].rect.y, input_boxes[j].rect.y); 69 | float xx2 = std::min(input_boxes[i].rect.x+input_boxes[i].rect.width, input_boxes[j].rect.x+input_boxes[j].rect.width); 70 | float yy2 = std::min(input_boxes[i].rect.y+input_boxes[i].rect.height, input_boxes[j].rect.y+input_boxes[j].rect.height); 71 | float w = std::max(float(0), xx2 - xx1 + 1); 72 | float h = std::max(float(0), yy2 - yy1 + 1); 73 | float inter = w * h; 74 | float ovr = inter / (vArea[i] + vArea[j] - inter); 75 | if (ovr >= NMS_THRESH) 76 | { 77 | input_boxes.erase(input_boxes.begin() + j); 78 | vArea.erase(vArea.begin() + j); 79 | } 80 | else 81 | { 82 | j++; 83 | } 84 | } 85 | } 86 | } 87 | 88 | int ncnnYoloV5::detect(const cv::Mat& bgr, std::vector& objects, uint8_t n_threads) 89 | { 90 | 91 | double threshold = 0.5; 92 | double nms_threshold = 0.05; 93 | const int target_size = 320; 94 | 95 | ncnn::Mat in = ncnn::Mat::from_pixels_resize(bgr.data, ncnn::Mat::PIXEL_BGR, bgr.cols, bgr.rows, target_size, target_size); 96 | float norm[3] = {1/255.f,1/255.f,1/255.f}; 97 | float mean[3] = {0,0,0}; 98 | in.substract_mean_normalize(mean,norm); 99 | 100 | ncnn::Extractor ex = neuralnet.create_extractor(); 101 | ex.set_num_threads(n_threads); 102 | ex.input(0, in); 103 | 104 | objects.clear(); 105 | for(const auto& layer: layers){ 106 | ncnn::Mat blob; 107 | ex.extract(layer.name.c_str(),blob); 108 | auto boxes = decode_infer(blob,layer.stride,{bgr.cols,bgr.rows},target_size,num_class,layer.anchors,threshold); 109 | objects.insert(objects.begin(),boxes.begin(),boxes.end()); 110 | } 111 | nms(objects,nms_threshold); 112 | return 0; 113 | }; 114 | 115 | void ncnnYoloV5::draw(const cv::Mat& bgr, const std::vector& objects, double dT) 116 | { 117 | cv::Mat image = bgr.clone(); 118 | 119 | for (size_t i = 0; i < objects.size(); i++) 120 | { 121 | const Object& obj = objects[i]; 122 | 123 | cv::rectangle(image, obj.rect, cv::Scalar(255, 0, 0)); 124 | 125 | char text[256]; 126 | sprintf(text, "%s %.1f%%", class_names[obj.label], obj.prob * 100); 127 | 128 | int baseLine = 0; 129 | cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine); 130 | 131 | int x = obj.rect.x; 132 | int y = obj.rect.y - label_size.height - baseLine; 133 | if (y < 0) 134 | y = 0; 135 | if (x + label_size.width > image.cols) 136 | x = image.cols - label_size.width; 137 | 138 | cv::rectangle(image, cv::Rect(cv::Point(x, y), 139 | cv::Size(label_size.width, label_size.height + baseLine)), 140 | cv::Scalar(255, 255, 255), -1); 141 | 142 | cv::putText(image, text, cv::Point(x, y + label_size.height), 143 | cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0)); 144 | } 145 | 146 | cv::putText(image, std::to_string(1/dT)+" Hz", cv::Point(20, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255)); 147 | cv::imshow("YOLO v5", image); 148 | cv::waitKey(1); 149 | } 150 | -------------------------------------------------------------------------------- /src/pfld_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_pfld.h" 16 | #include "ros_ncnn/FaceObject.h" 17 | ncnnPFLD engine; 18 | ///////////////////////////////// 19 | 20 | cv_bridge::CvImagePtr cv_ptr; 21 | 22 | void boundingBoxUpdate(const ros_ncnn::FaceObject& msg) 23 | { 24 | cv::Rect roi; 25 | roi.x = msg.boundingbox.position.x; 26 | roi.y = msg.boundingbox.position.y; 27 | roi.width = msg.boundingbox.size.x; 28 | roi.height = msg.boundingbox.size.y; 29 | 30 | engine.detect(cv_ptr->image, roi); 31 | engine.draw(cv_ptr->image, roi); 32 | } 33 | 34 | void imageCallback(const sensor_msgs::ImageConstPtr& msg) 35 | { 36 | try { 37 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 38 | } 39 | catch (cv_bridge::Exception& e) { 40 | ROS_ERROR("CV bridge exception: %s", e.what()); 41 | return; 42 | } 43 | } 44 | 45 | int main(int argc, char** argv) 46 | { 47 | ros::init(argc, argv, "hopenet_node"); 48 | ros::NodeHandle nhLocal("~"); 49 | ros::NodeHandle n; 50 | std::string node_name = ros::this_node::getName(); 51 | int gpu_device; 52 | nhLocal.param("gpu_device", gpu_device, 0); 53 | int num_threads; 54 | nhLocal.param("num_threads", num_threads, 0); 55 | engine.set_threads(num_threads); 56 | 57 | #ifndef GPU_SUPPORT 58 | ROS_WARN_STREAM(node_name << " running on CPU"); 59 | #endif 60 | #ifdef GPU_SUPPORT 61 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 62 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 63 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 64 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 65 | engine.neuralnet.opt.use_vulkan_compute = true; 66 | engine.neuralnet.set_vulkan_device(g_vkdev); 67 | #endif 68 | 69 | const std::string package_name = "ros_ncnn"; 70 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 71 | ROS_INFO("Assets path: %s", path.c_str()); 72 | engine.neuralnet.load_param((path+"pfld-sim.param").c_str()); 73 | engine.neuralnet.load_model((path+"pfld-sim.bin").c_str()); 74 | 75 | image_transport::ImageTransport it(n); 76 | image_transport::Subscriber video = it.subscribe("/head_camera/image_raw", 1, imageCallback); 77 | ros::Subscriber facebox_sub = n.subscribe("/ultraface_node/faces", 10, boundingBoxUpdate ); 78 | 79 | #ifdef GPU_SUPPORT 80 | ncnn::create_gpu_instance(); 81 | #endif 82 | 83 | while (ros::ok()) { 84 | ros::spinOnce(); 85 | } 86 | 87 | #ifdef GPU_SUPPORT 88 | ncnn::destroy_gpu_instance(); 89 | #endif 90 | 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /src/posenet_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_posenet.h" 16 | ncnnPoseNet engine; 17 | ///////////////////////////////// 18 | 19 | std::vector objects; /* o.O */ 20 | cv_bridge::CvImagePtr cv_ptr; 21 | ros::Time last_time; 22 | bool display_output; 23 | 24 | void print_objects(const std::vector& objects){ 25 | for (size_t i = 0; i < objects.size(); i++) 26 | { 27 | const KeyPoint& obj = objects[i]; 28 | if (obj.prob > 0.2) ROS_INFO("%.5f at %.2f %.2f", obj.prob, obj.p.x, obj.p.y); 29 | } 30 | } 31 | 32 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 33 | { 34 | try { 35 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 36 | engine.detect(cv_ptr->image, objects, n_threads); 37 | print_objects(objects); 38 | 39 | if (display_output) { 40 | ros::Time current_time = ros::Time::now(); 41 | engine.draw(cv_ptr->image, objects, (current_time-last_time).toSec()); 42 | last_time = current_time; 43 | } 44 | } 45 | catch (cv_bridge::Exception& e) { 46 | ROS_ERROR("CV bridge exception: %s", e.what()); 47 | return; 48 | } 49 | } 50 | 51 | int main(int argc, char** argv) 52 | { 53 | ros::init(argc, argv, "posenet_node"); /**/ 54 | ros::NodeHandle nhLocal("~"); 55 | ros::NodeHandle n; 56 | std::string node_name = ros::this_node::getName(); 57 | int gpu_device; 58 | nhLocal.param("gpu_device", gpu_device, 0); 59 | 60 | #ifndef GPU_SUPPORT 61 | ROS_WARN_STREAM(node_name << " running on CPU"); 62 | #endif 63 | #ifdef GPU_SUPPORT 64 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 65 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 66 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 67 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 68 | engine.neuralnet.opt.use_vulkan_compute = true; 69 | engine.neuralnet.set_vulkan_device(g_vkdev); 70 | #endif 71 | 72 | const std::string package_name = "ros_ncnn"; 73 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 74 | ROS_INFO("Assets path: %s", path.c_str()); 75 | engine.neuralnet.load_param((path+("pose.param")).c_str()); /**/ 76 | engine.neuralnet.load_model((path+("pose.bin")).c_str()); /**/ 77 | 78 | nhLocal.param("display_output", display_output, true); 79 | 80 | int num_threads; 81 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 82 | image_transport::ImageTransport it(n); 83 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 84 | 85 | #ifdef GPU_SUPPORT 86 | ncnn::create_gpu_instance(); 87 | #endif 88 | while (ros::ok()) { 89 | ros::spinOnce(); 90 | } 91 | #ifdef GPU_SUPPORT 92 | ncnn::destroy_gpu_instance(); 93 | #endif 94 | 95 | return 0; 96 | } 97 | -------------------------------------------------------------------------------- /src/retinaface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////////// 15 | #include "ros_ncnn/ncnn_retinaface.h" 16 | #include "ros_ncnn/FaceObject.h" 17 | ncnnRetinaface engine; 18 | ///////////////////////////////////// 19 | 20 | ros_ncnn::FaceObject faceMsg; 21 | ros::Publisher face_pub; 22 | cv_bridge::CvImagePtr cv_ptr; 23 | std::vector faceobjects; 24 | ros::Time last_time; 25 | double prob_threshold; 26 | bool display_output; 27 | bool enable_gpu; 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 30 | { 31 | try { 32 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 33 | engine.detect(cv_ptr->image, faceobjects, n_threads); 34 | ros::Time current_time = ros::Time::now(); 35 | for (size_t i = 0; i < faceobjects.size(); i++) 36 | { 37 | const FaceObject& obj = faceobjects[i]; 38 | if (obj.prob > prob_threshold) 39 | { 40 | // ROS_INFO("%.5f at %.2f %.2f %.2f x %.2f", 41 | // obj.prob, obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 42 | faceMsg.header.seq++; 43 | faceMsg.header.stamp = current_time; 44 | faceMsg.probability = obj.prob; 45 | faceMsg.boundingbox.position.x = obj.rect.x; 46 | faceMsg.boundingbox.position.y = obj.rect.y; 47 | faceMsg.boundingbox.size.x = obj.rect.width; 48 | faceMsg.boundingbox.size.y = obj.rect.height; 49 | for (uint8_t i=0; i<5; i++){ 50 | faceMsg.landmark[i].x = obj.landmark[i].x; 51 | faceMsg.landmark[i].y = obj.landmark[i].y;} 52 | face_pub.publish(faceMsg); 53 | } 54 | } 55 | if (display_output) { 56 | engine.draw(cv_ptr->image, faceobjects, (current_time-last_time).toSec()); 57 | } 58 | last_time = current_time; 59 | } 60 | catch (cv_bridge::Exception& e) { 61 | ROS_ERROR("CV bridge exception: %s", e.what()); 62 | return; 63 | } 64 | } 65 | 66 | int main(int argc, char** argv) 67 | { 68 | ros::init(argc, argv, "retinaface_node"); 69 | ros::NodeHandle nhLocal("~"); 70 | ros::NodeHandle n; 71 | std::string node_name = ros::this_node::getName(); 72 | int gpu_device; 73 | nhLocal.param("gpu_device", gpu_device, 0); 74 | nhLocal.param("enable_gpu", enable_gpu, true); // for benchmarking reasons 75 | 76 | #ifndef GPU_SUPPORT 77 | ROS_WARN_STREAM(node_name << " running on CPU"); 78 | #endif 79 | #ifdef GPU_SUPPORT 80 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 81 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 82 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 83 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 84 | engine.neuralnet.opt.use_vulkan_compute = enable_gpu; 85 | engine.neuralnet.set_vulkan_device(g_vkdev); 86 | #endif 87 | 88 | const std::string package_name = "ros_ncnn"; 89 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 90 | ROS_INFO("Assets path: %s", path.c_str()); 91 | engine.neuralnet.load_param((path+("mnet.25-opt.param")).c_str()); 92 | engine.neuralnet.load_model((path+("mnet.25-opt.bin")).c_str()); 93 | 94 | int num_threads; 95 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 96 | nhLocal.param("probability_threshold", prob_threshold, 0.5); 97 | nhLocal.param("display_output", display_output, true); 98 | 99 | face_pub = n.advertise(node_name+"/faces", 10); 100 | 101 | image_transport::ImageTransport it(n); 102 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 103 | 104 | #ifdef GPU_SUPPORT 105 | ncnn::create_gpu_instance(); 106 | #endif 107 | while (ros::ok()) { 108 | ros::spinOnce(); 109 | } 110 | #ifdef GPU_SUPPORT 111 | ncnn::destroy_gpu_instance(); 112 | #endif 113 | 114 | return 0; 115 | } 116 | -------------------------------------------------------------------------------- /src/ultraface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////////// 15 | #include "ros_ncnn/ncnn_ultraface.h" 16 | #include "ros_ncnn/FaceObject.h" 17 | ncnnUltraFace engine; 18 | ///////////////////////////////////// 19 | 20 | std::vector face_info; 21 | ros_ncnn::FaceObject faceMsg; 22 | ros::Publisher face_pub; 23 | cv_bridge::CvImagePtr cv_ptr; 24 | ros::Time last_time; 25 | double prob_threshold; 26 | bool display_output; 27 | bool enable_gpu; 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 30 | { 31 | try { 32 | ros::Time current_time = ros::Time::now(); 33 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 34 | 35 | face_info.clear(); 36 | engine.detect(cv_ptr->image, face_info, n_threads); 37 | 38 | for (size_t i=0; i prob_threshold){ 41 | faceMsg.header.seq++; 42 | faceMsg.header.stamp = current_time; 43 | faceMsg.probability = info.score; 44 | faceMsg.boundingbox.position.x = info.x1; 45 | faceMsg.boundingbox.position.y = info.y1; 46 | faceMsg.boundingbox.size.x = info.x2-info.x1; 47 | faceMsg.boundingbox.size.y = info.y2-info.y1; 48 | face_pub.publish(faceMsg); 49 | } 50 | } 51 | 52 | if (display_output) { 53 | engine.draw(cv_ptr->image, face_info, (current_time-last_time).toSec()); 54 | } 55 | 56 | last_time = current_time; 57 | } 58 | catch (cv_bridge::Exception& e) { 59 | ROS_ERROR("CV bridge exception: %s", e.what()); 60 | return; 61 | } 62 | } 63 | 64 | int main(int argc, char** argv) 65 | { 66 | ros::init(argc, argv, "ultraface_node"); 67 | ros::NodeHandle nhLocal("~"); 68 | ros::NodeHandle n; 69 | std::string node_name = ros::this_node::getName(); 70 | int gpu_device; 71 | nhLocal.param("gpu_device", gpu_device, 0); 72 | nhLocal.param("enable_gpu", enable_gpu, true); // for benchmarking reasons 73 | 74 | #ifndef GPU_SUPPORT 75 | ROS_WARN_STREAM(node_name << " running on CPU"); 76 | #endif 77 | #ifdef GPU_SUPPORT 78 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 79 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 80 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 81 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 82 | engine.neuralnet.opt.use_vulkan_compute = enable_gpu; 83 | engine.neuralnet.set_vulkan_device(g_vkdev); 84 | #endif 85 | 86 | const std::string package_name = "ros_ncnn"; 87 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 88 | ROS_INFO("Assets path: %s", path.c_str()); 89 | engine.neuralnet.load_param((path+("RFB-320.param")).c_str()); 90 | engine.neuralnet.load_model((path+("RFB-320.bin")).c_str()); 91 | 92 | double iou_threshold; 93 | int num_threads; 94 | int input_width; 95 | int input_length; 96 | 97 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 98 | nhLocal.param("probability_threshold", prob_threshold, 0.5); 99 | nhLocal.param("display_output", display_output, true); 100 | nhLocal.param("input_width", input_width, 320); 101 | nhLocal.param("input_length", input_length, 240); 102 | nhLocal.param("iou_threshold", iou_threshold, 0.3); 103 | 104 | engine.init(input_width, input_length, prob_threshold, iou_threshold); 105 | 106 | face_pub = n.advertise(node_name+"/faces", 10); 107 | 108 | image_transport::ImageTransport it(n); 109 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 110 | 111 | #ifdef GPU_SUPPORT 112 | ncnn::create_gpu_instance(); 113 | #endif 114 | while (ros::ok()) { 115 | ros::spinOnce(); 116 | } 117 | #ifdef GPU_SUPPORT 118 | ncnn::destroy_gpu_instance(); 119 | #endif 120 | 121 | return 0; 122 | } 123 | -------------------------------------------------------------------------------- /src/yolact_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_yolact.h" 16 | #include "ros_ncnn/Object.h" 17 | ncnnYolact engine; 18 | ///////////////////////////////// 19 | 20 | ros_ncnn::Object objMsg; 21 | ros::Publisher obj_pub; 22 | std::vector objects; 23 | cv_bridge::CvImagePtr cv_ptr; 24 | ros::Time last_time; 25 | bool display_output; 26 | double prob_threshold; 27 | bool enable_gpu; 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 30 | { 31 | try { 32 | ros::Time current_time = ros::Time::now(); 33 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 34 | engine.detect(cv_ptr->image, objects, n_threads); 35 | for (size_t i = 0; i < objects.size(); i++) 36 | { 37 | const Object& obj = objects[i]; 38 | if (obj.prob > prob_threshold) 39 | { 40 | ROS_INFO("%d = %.5f at %.2f %.2f %.2f x %.2f", obj.label, obj.prob, 41 | obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 42 | objMsg.header.seq++; 43 | objMsg.header.stamp = current_time; 44 | objMsg.probability = obj.prob; 45 | objMsg.label = class_names[obj.label]; 46 | objMsg.boundingbox.position.x = obj.rect.x; 47 | objMsg.boundingbox.position.y = obj.rect.y; 48 | objMsg.boundingbox.size.x = obj.rect.width; 49 | objMsg.boundingbox.size.y = obj.rect.height; 50 | obj_pub.publish(objMsg); 51 | } 52 | } 53 | if (display_output) { 54 | engine.draw(cv_ptr->image, objects, (current_time-last_time).toSec()); 55 | } 56 | last_time = current_time; 57 | } 58 | catch (cv_bridge::Exception& e) { 59 | ROS_ERROR("CV bridge exception: %s", e.what()); 60 | return; 61 | } 62 | } 63 | 64 | int main(int argc, char** argv) 65 | { 66 | ros::init(argc, argv, "yolact_node"); /**/ 67 | ros::NodeHandle nhLocal("~"); 68 | ros::NodeHandle n; 69 | std::string node_name = ros::this_node::getName(); 70 | int gpu_device; 71 | nhLocal.param("gpu_device", gpu_device, 0); 72 | nhLocal.param("enable_gpu", enable_gpu, true); 73 | 74 | #ifndef GPU_SUPPORT 75 | ROS_WARN_STREAM(node_name << " running on CPU"); 76 | #endif 77 | #ifdef GPU_SUPPORT 78 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 79 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 80 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 81 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 82 | engine.neuralnet.opt.use_vulkan_compute = enable_gpu; 83 | engine.neuralnet.set_vulkan_device(g_vkdev); 84 | #endif 85 | 86 | const std::string package_name = "ros_ncnn"; 87 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 88 | ROS_INFO("Assets path: %s", path.c_str()); 89 | engine.neuralnet.load_param((path+("yolact.param")).c_str()); /**/ 90 | engine.neuralnet.load_model((path+("yolact.bin")).c_str()); /**/ 91 | 92 | int num_threads; 93 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 94 | nhLocal.param("display_output", display_output, true); 95 | 96 | obj_pub = n.advertise(node_name+"/objects", 50); 97 | 98 | image_transport::ImageTransport it(n); 99 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 100 | 101 | #ifdef GPU_SUPPORT 102 | ncnn::create_gpu_instance(); 103 | #endif 104 | while (ros::ok()) { 105 | ros::spinOnce(); 106 | } 107 | #ifdef GPU_SUPPORT 108 | ncnn::destroy_gpu_instance(); 109 | #endif 110 | 111 | return 0; 112 | } 113 | -------------------------------------------------------------------------------- /src/yolo_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_yolo.h" 16 | #include "ros_ncnn/Object.h" 17 | ncnnYolo engine; 18 | ///////////////////////////////// 19 | 20 | ros_ncnn::Object objMsg; 21 | ros::Publisher obj_pub; 22 | std::vector objects; 23 | cv_bridge::CvImagePtr cv_ptr; 24 | ros::Time last_time; 25 | bool display_output; 26 | double prob_threshold; 27 | bool enable_gpu; 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 30 | { 31 | try { 32 | ros::Time current_time = ros::Time::now(); 33 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 34 | engine.detect(cv_ptr->image, objects, n_threads); 35 | for (size_t i = 0; i < objects.size(); i++) 36 | { 37 | const Object& obj = objects[i]; 38 | if (obj.prob > prob_threshold) 39 | { 40 | ROS_INFO("%d = %.5f at %.2f %.2f %.2f x %.2f", obj.label, obj.prob, 41 | obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 42 | objMsg.header.seq++; 43 | objMsg.header.stamp = current_time; 44 | objMsg.probability = obj.prob; 45 | objMsg.label = class_names[obj.label]; 46 | objMsg.boundingbox.position.x = obj.rect.x; 47 | objMsg.boundingbox.position.y = obj.rect.y; 48 | objMsg.boundingbox.size.x = obj.rect.width; 49 | objMsg.boundingbox.size.y = obj.rect.height; 50 | obj_pub.publish(objMsg); 51 | } 52 | } 53 | if (display_output) { 54 | engine.draw(cv_ptr->image, objects, (current_time-last_time).toSec()); 55 | } 56 | last_time = current_time; 57 | } 58 | catch (cv_bridge::Exception& e) { 59 | ROS_ERROR("CV bridge exception: %s", e.what()); 60 | return; 61 | } 62 | } 63 | 64 | int main(int argc, char** argv) 65 | { 66 | ros::init(argc, argv, "yolo_node"); /**/ 67 | ros::NodeHandle nhLocal("~"); 68 | ros::NodeHandle n; 69 | std::string node_name = ros::this_node::getName(); 70 | int gpu_device; 71 | nhLocal.param("gpu_device", gpu_device, 0); 72 | nhLocal.param("enable_gpu", enable_gpu, true); 73 | 74 | #ifndef GPU_SUPPORT 75 | ROS_WARN_STREAM(node_name << " running on CPU"); 76 | #endif 77 | #ifdef GPU_SUPPORT 78 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 79 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 80 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 81 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 82 | engine.neuralnet.opt.use_vulkan_compute = enable_gpu; 83 | engine.neuralnet.set_vulkan_device(g_vkdev); 84 | #endif 85 | 86 | const std::string package_name = "ros_ncnn"; 87 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 88 | ROS_INFO("Assets path: %s", path.c_str()); 89 | 90 | std::string model_file, param_file; 91 | nhLocal.param("model_file", model_file, std::string("mobilenetv2_yolov3.bin")); 92 | nhLocal.param("param_file", param_file, std::string("mobilenetv2_yolov3.param")); 93 | engine.neuralnet.load_param((path+param_file).c_str()); 94 | engine.neuralnet.load_model((path+model_file).c_str()); 95 | ROS_INFO("Loaded: %s", model_file.c_str()); 96 | 97 | int num_threads; 98 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 99 | nhLocal.param("display_output", display_output, true); 100 | 101 | obj_pub = n.advertise(node_name+"/objects", 50); 102 | 103 | image_transport::ImageTransport it(n); 104 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 105 | 106 | #ifdef GPU_SUPPORT 107 | ncnn::create_gpu_instance(); 108 | #endif 109 | while (ros::ok()) { 110 | ros::spinOnce(); 111 | } 112 | #ifdef GPU_SUPPORT 113 | ncnn::destroy_gpu_instance(); 114 | #endif 115 | 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /src/yolov5_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ros_ncnn/ncnn_config.h" 9 | #ifdef GPU_SUPPORT 10 | #include "gpu.h" 11 | #include "ros_ncnn/gpu_support.h" 12 | #endif 13 | 14 | ///////////////////////////////// 15 | #include "ros_ncnn/ncnn_yolov5.h" 16 | #include "ros_ncnn/Object.h" 17 | ncnnYoloV5 engine; 18 | ///////////////////////////////// 19 | 20 | ros_ncnn::Object objMsg; 21 | ros::Publisher obj_pub; 22 | std::vector objects; 23 | cv_bridge::CvImagePtr cv_ptr; 24 | ros::Time last_time; 25 | bool display_output; 26 | double prob_threshold; 27 | bool enable_gpu; 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, int n_threads) 30 | { 31 | try { 32 | ros::Time current_time = ros::Time::now(); 33 | cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 34 | engine.detect(cv_ptr->image, objects, n_threads); 35 | 36 | for (size_t i = 0; i < objects.size(); i++) 37 | { 38 | const Object& obj = objects[i]; 39 | if (obj.prob > prob_threshold) 40 | { 41 | ROS_INFO("%s \t = %.5f at %.2f %.2f %.2f x %.2f", class_names[obj.label], obj.prob, 42 | obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height); 43 | objMsg.header.seq++; 44 | objMsg.header.stamp = current_time; 45 | objMsg.probability = obj.prob; 46 | objMsg.label = class_names[obj.label]; 47 | objMsg.boundingbox.position.x = obj.rect.x; 48 | objMsg.boundingbox.position.y = obj.rect.y; 49 | objMsg.boundingbox.size.x = obj.rect.width; 50 | objMsg.boundingbox.size.y = obj.rect.height; 51 | obj_pub.publish(objMsg); 52 | } 53 | } 54 | 55 | if (display_output) { 56 | engine.draw(cv_ptr->image, objects, (current_time-last_time).toSec()); 57 | } 58 | last_time = current_time; 59 | } 60 | catch (cv_bridge::Exception& e) { 61 | ROS_ERROR("CV bridge exception: %s", e.what()); 62 | return; 63 | } 64 | } 65 | 66 | int main(int argc, char** argv) 67 | { 68 | ros::init(argc, argv, "yolov5_node"); /**/ 69 | ros::NodeHandle nhLocal("~"); 70 | ros::NodeHandle n; 71 | std::string node_name = ros::this_node::getName(); 72 | int gpu_device; 73 | nhLocal.param("gpu_device", gpu_device, 0); 74 | nhLocal.param("enable_gpu", enable_gpu, true); 75 | 76 | #ifndef GPU_SUPPORT 77 | ROS_WARN_STREAM(node_name << " running on CPU"); 78 | #endif 79 | #ifdef GPU_SUPPORT 80 | ROS_INFO_STREAM(node_name << " with GPU_SUPPORT, selected gpu_device: " << gpu_device); 81 | g_vkdev = ncnn::get_gpu_device(selectGPU(gpu_device)); 82 | g_blob_vkallocator = new ncnn::VkBlobAllocator(g_vkdev); 83 | g_staging_vkallocator = new ncnn::VkStagingAllocator(g_vkdev); 84 | engine.neuralnet.opt.use_vulkan_compute = enable_gpu; 85 | engine.neuralnet.set_vulkan_device(g_vkdev); 86 | #endif 87 | 88 | const std::string package_name = "ros_ncnn"; 89 | std::string path = ros::package::getPath(package_name)+("/assets/models/"); 90 | ROS_INFO("Assets path: %s", path.c_str()); 91 | 92 | std::string model_file, param_file; 93 | nhLocal.param("model_file", model_file, std::string("yolov5.bin")); 94 | nhLocal.param("param_file", param_file, std::string("yolov5.param")); 95 | engine.neuralnet.load_param((path+param_file).c_str()); 96 | engine.neuralnet.load_model((path+model_file).c_str()); 97 | ROS_INFO("Loaded: %s", model_file.c_str()); 98 | 99 | int num_threads; 100 | nhLocal.param("num_threads", num_threads, ncnn::get_cpu_count()); 101 | nhLocal.param("display_output", display_output, true); 102 | 103 | obj_pub = n.advertise(node_name+"/objects", 50); 104 | 105 | image_transport::ImageTransport it(n); 106 | image_transport::Subscriber video = it.subscribe("/camera/image_raw", 1, boost::bind(&imageCallback, _1, num_threads)); 107 | 108 | #ifdef GPU_SUPPORT 109 | ncnn::create_gpu_instance(); 110 | #endif 111 | while (ros::ok()) { 112 | ros::spinOnce(); 113 | } 114 | #ifdef GPU_SUPPORT 115 | ncnn::destroy_gpu_instance(); 116 | #endif 117 | 118 | return 0; 119 | } 120 | --------------------------------------------------------------------------------