From ec3c07d29faf9415ba99beb9e52c248b956e54ca Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 12 Dec 2022 17:10:45 +0100 Subject: [PATCH] First ROS2 (Galactic) version --- CHANGELOG.rst | 4 + CMakeLists.txt | 162 +++--- ...{detectionObject.h => detectionObject.hpp} | 15 +- .../object_detection_openvino/nodeletHelper.h | 29 - ...tDetectionVPU.h => objectDetectionVPU.hpp} | 181 +++--- .../{openvino.h => openvino.hpp} | 119 ++-- .../{yoloParams.h => yoloParams.hpp} | 34 +- launch/default.launch.py | 68 +++ launch/mobilenet_cpu.launch | 25 - launch/mobilenet_cpu_astrastereo.launch | 16 - launch/mobilenet_cpu_realsense.launch | 39 -- launch/mobilenet_myriad.launch | 25 - launch/mobilenet_myriad_nodelet.launch | 28 - launch/mobilenet_myriad_realsense.launch | 39 -- .../mobilenet_myriad_realsense_nodelet.launch | 58 -- launch/yolo_cpu.launch | 25 - launch/yolo_myriad.launch | 25 - nodelet_plugins.xml | 5 - package.xml | 17 +- params/default_params.yaml | 20 + src/objectDetectionVPU.cpp | 536 +++++++++--------- src/objectDetectionVPU_node.cpp | 23 +- src/objectDetectionVPU_nodelet.cpp | 20 - src/openvino.cpp | 310 +++++----- 24 files changed, 812 insertions(+), 1011 deletions(-) rename include/object_detection_openvino/{detectionObject.h => detectionObject.hpp} (90%) delete mode 100644 include/object_detection_openvino/nodeletHelper.h rename include/object_detection_openvino/{objectDetectionVPU.h => objectDetectionVPU.hpp} (53%) rename include/object_detection_openvino/{openvino.h => openvino.hpp} (52%) rename include/object_detection_openvino/{yoloParams.h => yoloParams.hpp} (72%) create mode 100644 launch/default.launch.py delete mode 100644 launch/mobilenet_cpu.launch delete mode 100644 launch/mobilenet_cpu_astrastereo.launch delete mode 100644 launch/mobilenet_cpu_realsense.launch delete mode 100644 launch/mobilenet_myriad.launch delete mode 100644 launch/mobilenet_myriad_nodelet.launch delete mode 100644 launch/mobilenet_myriad_realsense.launch delete mode 100644 launch/mobilenet_myriad_realsense_nodelet.launch delete mode 100644 launch/yolo_cpu.launch delete mode 100644 launch/yolo_myriad.launch delete mode 100644 nodelet_plugins.xml create mode 100644 params/default_params.yaml delete mode 100644 src/objectDetectionVPU_nodelet.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3fea689..0ea276e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package object_detection_openvino ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.0.0 (12-12-2022) +------------------ +* First ROS2 (Galactic) version. + 3.2.2 (26-05-2022) ------------------ * Improve documentation. diff --git a/CMakeLists.txt b/CMakeLists.txt index d72bf7a..64af17b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,78 +1,57 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(object_detection_openvino) -# Boost -find_package(Boost REQUIRED) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + #add_compile_options(-Wall -Wextra -Wpedantic) +endif() -# OpenCV -find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc imgcodecs highgui QUIET) +################################################ +## Find dependencies ## +################################################ +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(message_filters REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(Boost REQUIRED) # Ngraph and InferenceEngine find_package(ngraph REQUIRED) -find_package(InferenceEngine 2.0 REQUIRED) +find_package(InferenceEngine 2021.4.1 REQUIRED) if(NOT(InferenceEngine_FOUND)) message(WARNING "InferenceEngine is disabled or not found.") return() else() include_directories(${InferenceEngine_INCLUDE_DIRS}/../samples/cpp/common/) - #include_directories(${InferenceEngine_INCLUDE_DIRS}/../samples/cpp/common/utils/include/) + include_directories(${InferenceEngine_INCLUDE_DIRS}/../samples/cpp/common/utils/include/) endif() if(TARGET IE::ie_cpu_extension) add_definitions(-DWITH_EXTENSIONS) endif() -# Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - nodelet - std_msgs - sensor_msgs - geometry_msgs - cv_bridge - image_transport - message_filters - vision_msgs - pcl_ros - pcl_conversions -) - - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -################################### -## catkin specific configuration ## -################################### -# The catkin_package macro generates cmake config files for your package -# Declare things to be passed to dependent projects -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - nodelet - std_msgs - sensor_msgs - geometry_msgs - cv_bridge - image_transport - message_filters - vision_msgs - pcl_ros - pcl_conversions - DEPENDS Boost -) - ########### ## Build ## ########### - -# Specify additional locations of header files +## Specify additional locations of header files +## Your package locations should be listed before other locations include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} + include + ${Boost_INCLUDE_DIRS} + ${InferenceEngine_INCLUDE_DIRS} ) # Set SOURCES @@ -81,29 +60,30 @@ set(SOURCES src/openvino.cpp ) -# Declare a C++ executable -add_executable(${PROJECT_NAME}_node ${SOURCES} src/objectDetectionVPU_node.cpp) - -# Declare a C++ library -add_library(${PROJECT_NAME}_nodelet ${SOURCES} src/objectDetectionVPU_nodelet.cpp) - -# Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - PRIVATE - ${InferenceEngine_LIBRARIES} - ${OpenCV_LIBRARIES} - ${NGRAPH_LIBRARIES} - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} +# Executable node +add_executable (${PROJECT_NAME} + ${SOURCES} + src/objectDetectionVPU_node.cpp +) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + tf2 + std_msgs + sensor_msgs + vision_msgs + image_transport + message_filters + visualization_msgs + cv_bridge + pcl_ros + pcl_conversions ) -target_link_libraries(${PROJECT_NAME}_nodelet - PRIVATE +# Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} ${InferenceEngine_LIBRARIES} - ${OpenCV_LIBRARIES} ${NGRAPH_LIBRARIES} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} ) if(TARGET IE::ie_cpu_extension) @@ -114,26 +94,32 @@ endif() ############# ## Install ## ############# - -# Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_nodelet - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" + # PATTERN ".svn" EXCLUDE ) -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) -## Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - FILES_MATCHING PATTERN "*.launch" +install(DIRECTORY params + DESTINATION share/${PROJECT_NAME}/ ) + +############# +## Testing ## +############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/include/object_detection_openvino/detectionObject.h b/include/object_detection_openvino/detectionObject.hpp similarity index 90% rename from include/object_detection_openvino/detectionObject.h rename to include/object_detection_openvino/detectionObject.hpp index a86ea1b..a3c2121 100644 --- a/include/object_detection_openvino/detectionObject.h +++ b/include/object_detection_openvino/detectionObject.hpp @@ -1,7 +1,7 @@ /* * DETECTION OBJECT STRUCT * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán + * Copyright (c) 2020-2022 Alberto José Tudela Roldán * * This file is part of object_detection_openvino project. * @@ -9,8 +9,8 @@ * */ -#ifndef DETECTION_OBJECT_H -#define DETECTION_OBJECT_H +#ifndef OBJECT_DETECTION_OPENVINO__DETECTION_OBJECT_HPP_ +#define OBJECT_DETECTION_OPENVINO__DETECTION_OBJECT_HPP_ #include @@ -118,12 +118,7 @@ struct DetectionObject{ } /** - * @brief Relational operator of the confidence of the detected object. - * - * @param s2 Object to compare to. - * @return True if the left object has lower confidence than the right one. False, otherwise. - */ - bool operator<(const DetectionObject &s2) const { + * @brief Relational operator of OBJECT_DETECTION_OPENVINO__YOLO_PARAMS_HPP_bject &s2) const { return this->confidence < s2.confidence; } @@ -138,4 +133,4 @@ struct DetectionObject{ } }; -#endif +#endif // OBJECT_DETECTION_OPENVINO__DETECTION_OBJECT_HPP_ diff --git a/include/object_detection_openvino/nodeletHelper.h b/include/object_detection_openvino/nodeletHelper.h deleted file mode 100644 index f5a6590..0000000 --- a/include/object_detection_openvino/nodeletHelper.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * NODELET HELPER - * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán - * - * This file is part of object_detection_openvino project. - * - * All rights reserved. - * - */ - -#include - -namespace nodelet_helper{ - template - class TNodelet: public nodelet::Nodelet{ - public: - TNodelet(){}; - - void onInit(){ - NODELET_DEBUG("Initializing nodelet"); - - nodelet_ = std::unique_ptr(new T(getNodeHandle(), getPrivateNodeHandle())); - } - - private: - std::unique_ptr nodelet_; - }; -} // End nodelet_helper namespace diff --git a/include/object_detection_openvino/objectDetectionVPU.h b/include/object_detection_openvino/objectDetectionVPU.hpp similarity index 53% rename from include/object_detection_openvino/objectDetectionVPU.h rename to include/object_detection_openvino/objectDetectionVPU.hpp index 34ed9a4..4fb16de 100644 --- a/include/object_detection_openvino/objectDetectionVPU.h +++ b/include/object_detection_openvino/objectDetectionVPU.hpp @@ -9,8 +9,8 @@ * */ -#ifndef OBJECT_DETECTION_VPU_H -#define OBJECT_DETECTION_VPU_H +#ifndef OBJECT_DETECTION_OPENVINO__OBJECT_DETECTION_VPU_HPP_ +#define OBJECT_DETECTION_OPENVINO__OBJECT_DETECTION_VPU_HPP_ // C++ #include @@ -22,29 +22,28 @@ #include // ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/header.hpp" +#include "tf2_ros/buffer.h" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "visualization_msgs/msg/marker_array.hpp" +#include "vision_msgs/msg/vision_info.hpp" +#include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection3_d_array.hpp" + +// Object detection openvino +#include "object_detection_openvino/detectionObject.hpp" +#include "object_detection_openvino/openvino.hpp" // OpenCV #include - /// The number of the labels in the COCO database. #define COCO_CLASSES 80 @@ -58,55 +57,51 @@ typedef pcl::PointCloud pcloud; * @brief Implmentation of the interface between different neural networks frameworks and ROS. * */ -class ObjectDetectionVPU{ +class ObjectDetectionVPU : public rclcpp::Node{ public: - /// Constructor which initialize the subscribers, the publishers and the inference engine . - ObjectDetectionVPU(ros::NodeHandle& node, ros::NodeHandle& node_private); + /// Constructor which initialize the subscribers, the publishers and the inference engine. + ObjectDetectionVPU(std::string node_name); /// Delete all parameteres of the node. ~ObjectDetectionVPU(); private: - /// Public ROS node handler. - ros::NodeHandle node_; - - /// Private ROS node handler. - ros::NodeHandle nodePrivate_; - - /// The received image. - image_transport::ImageTransport imageTransport_; - /// Subscriber to a color image. - image_transport::SubscriberFilter colorSub_; + image_transport::SubscriberFilter color_sub_; /// Publisher of the image with the bounding boxes of the detected objects. - image_transport::Publisher detectionColorPub_; + image_transport::Publisher detection_color_pub_; /// Subscriber to a pointcloud. - message_filters::Subscriber pointsSub_; + message_filters::Subscriber points_sub_; /// Publisher of the meta-information about the vision pipeline. - ros::Publisher detectionInfoPub_; + rclcpp::Publisher::SharedPtr detection_info_pub_; /// Publisher of the aray with the detected objects in 2 dimensions. - ros::Publisher detections2DPub_; + rclcpp::Publisher::SharedPtr detections_2d_pub_; /// Publisher of the aray with the detected objects in 3 dimensions. - ros::Publisher detections3DPub_; + rclcpp::Publisher::SharedPtr detections_3d_pub_; /// Publisher of markers with the bounding boxes and labels of the detected objects. - ros::Publisher markersPub_; + rclcpp::Publisher::SharedPtr markers_pub_; - /// Listener of the transformations tree. - tf::TransformListener tfListener_; + /// The buffer of the transformations tree. + std::unique_ptr tf_buffer_; /// Typedef for a ApproximateTime policy between an image message and a pointcloud message. - typedef message_filters::sync_policies::ApproximateTime SyncPolicyImagePCL; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::PointCloud2> ApproximatePolicy; + + /// Typedef for synchronizer of the image message and the pointcloud message. + typedef message_filters::Synchronizer ApproximateSync; /// Synchronizer of the image message and the pointcloud message. - message_filters::Synchronizer syncImagePCL_; + std::shared_ptr approximate_sync_; /// Intel OpenVino framework. Openvino openvino_; @@ -115,58 +110,58 @@ class ObjectDetectionVPU{ float thresh_; /// Value of the intersection over union threshold in a range between 0 and 1. - float iouThresh_; + float iou_thresh_; - /// Flag to enable / disable the number of FPS in the iamge. - bool showFPS_; + /// Flag to enable / disable the number of FPS in the image. + bool show_fps_; /// Flag to enable / disable the use of te pointcloud to perform depth analysis. - bool useDepth_; + bool use_depth_; /// Type of the neural network. - std::string networkType_; + std::string network_type_; /// Name of the device to load the neural network into. - std::string deviceTarget_; + std::string device_target_; /// The filename of the model in XML format. - std::string modelFileName_; + std::string model_filename_; /// The filename of the model in BIN format. - std::string binFileName_; + std::string bin_filename_; /// The filename of the labels used in the model. - std::string labelFileName_; + std::string label_filename_; - /// Frame identifier of the color image. - std::string colorFrameId_; + /// Frame of the color image. + std::string color_frame_; - /// Frame identifier of the camera. - std::string cameraFrameId_; + /// Frame of the camera. + std::string camera_frame_; /// Topic of the color image. - std::string colorTopic_; + std::string color_topic_; /// Topic of the pointcloud. - std::string pointCloudTopic_; + std::string pointcloud_topic_; /// Topic of the image with the bounding boxes of the detected objects. - std::string detectionImageTopic_; + std::string detection_image_topic_; /// Topic of the information about the vision pipeline. - std::string detectionInfoTopic_; + std::string detection_info_topic_; /// Topic of the detected objects in 2 dimensions. - std::string detections2DTopic_; + std::string detections_2d_topic_; /// Topic of the detected objects in 3 dimensions. - std::string detections3DTopic_; + std::string detections_3d_topic_; /// Next image to be processed. - cv::Mat nextFrame_; + cv::Mat next_frame_; /// Curent image to be processed. - cv::Mat currFrame_; + cv::Mat current_frame_; /// Class labels of the neural network. std::vector labels_; @@ -175,7 +170,7 @@ class ObjectDetectionVPU{ * @brief Update the parameters of the node. * */ - void getParams(); + void get_params(); /** * @brief Get color of the class. @@ -185,30 +180,29 @@ class ObjectDetectionVPU{ * @param max Maximum number of classes. * @return The color of the class. */ - int getColor(int c, int x, int max); + int get_color(int c, int x, int max); /** * @brief Publish the meta-information about the vision pipeline when a subscriber connect to it. - * - * @param pub The publisher of the information. */ - void connectInfoCallback(const ros::SingleSubscriberPublisher& pub); + void connect_info_callback(); /** * @brief Receive an image, perform 2d object detection on it and publish the detected objects. * - * @param colorImageMsg A message with an image to be processed. + * @param color_image_msg A message with an image to be processed. */ - void colorImageCallback(const sensor_msgs::Image::ConstPtr& colorImageMsg); + void color_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & color_image_msg); /** * @brief Receive an image and a pointcloud, perform 2d object detection on it and publish the detected objects. * * - * @param colorImageMsg A message with an image to be processed. - * @param pointsMsg A message with a pointcloud to be processed. + * @param color_image_msg A message with an image to be processed. + * @param points_msg A message with a pointcloud to be processed. */ - void colorPointCallback(const sensor_msgs::Image::ConstPtr& colorImageMsg, const sensor_msgs::PointCloud2::ConstPtr& pointsMsg); + void color_point_callback(const sensor_msgs::msg::Image::ConstSharedPtr & color_image_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg); /** * @brief Show the histogram of the image. @@ -216,7 +210,7 @@ class ObjectDetectionVPU{ * @param image The image from the sensor. * @param mean The mean value of the pixels. */ - void showHistogram(cv::Mat image, cv::Scalar mean); + void show_histogram(cv::Mat image, cv::Scalar mean); /** * @brief Create a Detection2D message with the detected object. @@ -226,50 +220,65 @@ class ObjectDetectionVPU{ * @param[out] detection2D The detection2D message with the detected object. * @return True if the message was created correctly, false otherwise. */ - bool createDetection2DMsg(DetectionObject object, std_msgs::Header header, vision_msgs::Detection2D& detection2D); + bool create_detection_2d_msg(DetectionObject object, + std_msgs::msg::Header header, + vision_msgs::msg::Detection2D& detection_2d); /** * @brief Create a detection3D message with the detected object. * * @param[in] object The detected object. * @param[in] header The header of the detected object. - * @param[in] cloudPC2 The pointcloud of the detected object in sensor_msgs::PointCloud2 format. - * @param[in] cloudPCL The pointcloud of the detected object in pcloud format. + * @param[in] cloud_pc2 The pointcloud of the detected object in sensor_msgs::PointCloud2 format. + * @param[in] cloud_pcl The pointcloud of the detected object in pcloud format. * @param[out] detection3D The detection3D message with the detected object. * @return True if the message was created correctly, false otherwise. */ - bool createDetection3DMsg(DetectionObject object, std_msgs::Header header, const sensor_msgs::PointCloud2& cloudPC2, pcloud::ConstPtr cloudPCL, vision_msgs::Detection3D& detection3D); + bool create_detection_3d_msg(DetectionObject object, + std_msgs::msg::Header header, + const sensor_msgs::msg::PointCloud2& cloud_pc2, + pcloud::ConstPtr cloud_pcl, + vision_msgs::msg::Detection3D& detection_3d); /** * @brief Create a 3D marker with the bounding box of the object to be shown in Rviz. * * @param[in] id The numeric identifier of the marker. * @param[in] header The header of the marker. - * @param[in] colorRGB The color of the marker. + * @param[in] color_rgb The color of the marker. * @param[in] bbox The dimensions of the bounding box surrounding the object. * @param[out] marker The 3d marker with the bounding of the detected object. * @return True if the marker was created correctly, false otherwise. */ - bool createBBox3DMarker(int id, std_msgs::Header header, float colorRGB[3], vision_msgs::BoundingBox3D bbox, visualization_msgs::Marker& marker); + bool create_bbox_3d_marker(int id, + std_msgs::msg::Header header, + float color_rgb[3], + vision_msgs::msg::BoundingBox3D bbox, + visualization_msgs::msg::Marker& marker); /** * @brief Create a 3d marker with the label of the object to be shown in Rviz. * * @param[in] id The numeric identifier of the marker. * @param[in] header The header of the marker. - * @param[in] colorRGB The color of the marker. + * @param[in] color_rgb The color of the marker. * @param[in] bbox The dimensions of the bounding box surrounding the object. * @param[in] label The label class of the detected object. * @param[out] marker The 3d marker with the label of the detected object. * @return True if the marker was created correctly, false otherwise. */ - bool createLabel3DMarker(int id, std_msgs::Header header, float colorRGB[3], vision_msgs::BoundingBox3D bbox, std::string label, visualization_msgs::Marker& marker); + bool create_label_3d_marker(int id, + std_msgs::msg::Header header, + float color_rgb[3], + vision_msgs::msg::BoundingBox3D bbox, + std::string label, + visualization_msgs::msg::Marker& marker); /** * @brief Publish the image with the bounding boxes of the detected objects. * * @param image The image with the bounding boxes of the detected objects. */ - void publishImage(cv::Mat image); + void publish_image(cv::Mat image); }; -#endif +#endif // OBJECT_DETECTION_OPENVINO__OBJECT_DETECTION_VPU_HPP_ diff --git a/include/object_detection_openvino/openvino.h b/include/object_detection_openvino/openvino.hpp similarity index 52% rename from include/object_detection_openvino/openvino.h rename to include/object_detection_openvino/openvino.hpp index 5544f34..7d970f4 100644 --- a/include/object_detection_openvino/openvino.h +++ b/include/object_detection_openvino/openvino.hpp @@ -1,7 +1,7 @@ /* * OPENVINO CLASS * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán + * Copyright (c) 2020-2022 Alberto José Tudela Roldán * * This file is part of object_detection_openvino project. * @@ -9,8 +9,8 @@ * */ -#ifndef OPENVINO_H -#define OPENVINO_H +#ifndef OBJECT_DETECTION_OPENVINO__OPENVINO_HPP_ +#define OBJECT_DETECTION_OPENVINO__OPENVINO_HPP_ // C++ #include @@ -26,8 +26,8 @@ #endif // Object detection -#include -#include +#include "object_detection_openvino/detectionObject.hpp" +#include "object_detection_openvino/yoloParams.hpp" /** * @brief Implementation of the Intel OpenVino as a more standard API. @@ -40,7 +40,7 @@ class Openvino{ * @brief Constructor. * */ - Openvino(); + Openvino(std::string node_name); /** * @brief Destructor. @@ -53,62 +53,62 @@ class Openvino{ * * @param device The name of the device. */ - void setTargetDevice(std::string device); + void set_target_device(std::string device); /** * @brief Read the Intermediate Representation (IR) generated by the Model Optimizer to be loaded into the device. * - * @param modelFileName The filename of the model in XML format. - * @param binFileName The filename of the model in BIN format. - * @param labelFileName The filename of the labels used in the model. + * @param model_filename The filename of the model in XML format. + * @param bin_filename The filename of the model in BIN format. + * @param label_filename The filename of the labels used in the model. */ - void setNetworkModel(std::string modelFileName, std::string binFileName, std::string labelFileName); + void set_network_model(std::string model_filename, std::string bin_filename, std::string label_filename); /** * @brief Configure the inputs and outputs of the neural network. * - * @param networkType The name of the neural network. + * @param network_type The name of the neural network. */ - void configureNetwork(std::string networkType); + void configure_network(std::string network_type); /** * @brief Load the neural network model to the selected device. * * @param device The name of the device. */ - void loadModelToDevice(std::string device); + void load_model_to_device(std::string device); /** * @brief Create an asynchronous inference request. * */ - void createAsyncInferRequest(); + void create_async_infer_request(); /** * @brief Start the asynchronous inference request in the next frame. * */ - void startNextAsyncInferRequest(); + void start_next_async_infer_request(); /** * @brief Swap the asynchronous inference request between the current frame and the next one. * */ - void swapAsyncInferRequest(); + void swap_async_infer_request(); /** * @brief Check if the device is ready. * * @return True if the device is ready. False, otherwise. */ - bool isDeviceReady(); + bool is_device_ready(); /** * @brief Get the labels of the neural network model. * * @return The names of the labels. */ - std::vector getLabels(); + std::vector get_labels(); /** * @brief Get the detected objects in the frame. @@ -116,48 +116,57 @@ class Openvino{ * @param height Height in pixels of the bounding box surrounding the object. * @param width Width in pixels of the bounding box surrounding the object. * @param threshold Value below which objects will be discarded. - * @param iouThreshold Value of the intersection over union threshold in a range between 0 and 1. + * @param iou_threshold Value of the intersection over union threshold in a range between 0 and 1. * Above this value, the bounding boxes will be overlapped. * @return The detected objects. */ - std::vector getDetectionObjects(size_t height, size_t width, float threshold, float iouThreshold); + std::vector get_detection_objects(size_t height, + size_t width, + float threshold, + float iou_threshold); /** * @brief Convert the frame to an asynchronous inference request. * * @param frame The frame to be converted. - * @param autoResize Value to change the size of the frame. + * @param auto_resize Value to change the size of the frame. */ - void frameToNextInfer(const cv::Mat &frame, bool autoResize = false); + void frame_to_next_infer(const cv::Mat &frame, bool auto_resize = false); private: + /// Name of the node. + std::string node_name_; + /// Name of the input layer. - std::string inputName_; + std::string input_name_; /// Type of the neural network. - std::string networkType_; + std::string network_type_; /// Name of the device to load the neural network into. - std::string deviceTarget_; + std::string device_target_; /// Implementation of the coordinates of regions for YOLOv3 model. - std::map yoloParams_; + std::map yolo_params_; /// Interface of an executable network. - InferenceEngine::ExecutableNetwork infNetwork_; + InferenceEngine::ExecutableNetwork inf_network_; + + /// Interface of asynchronous current infer request. + InferenceEngine::InferRequest::Ptr async_infer_request_current_; - /// Interface of asynchronous infer request. - InferenceEngine::InferRequest::Ptr asyncInferRequestCurr_, asyncInferRequestNext_; + /// Interface of asynchronous next infer request. + InferenceEngine::InferRequest::Ptr async_infer_request_next_; /// A collection that contains string as key, and OutputInfo smart pointer as value. - InferenceEngine::OutputsDataMap outputInfo_; + InferenceEngine::OutputsDataMap output_info_; /// A collection that contains string as key, and InputInfo smart pointer as value. - InferenceEngine::InputsDataMap inputInfo_; + InferenceEngine::InputsDataMap input_info_; /// Information about the Neural Network and the related binary information. - InferenceEngine::CNNNetwork cnnNetwork_; + InferenceEngine::CNNNetwork cnn_network_; /// Inference Engine Core entity. InferenceEngine::Core core_; @@ -175,7 +184,7 @@ class Openvino{ * @param entry The entry. * @return Index of the entry. */ - static int entryIndex(int side, int lcoords, int lclasses, int location, int entry); + static int entry_index(int side, int lcoords, int lclasses, int location, int entry); /** * @brief Intersection over union of the bounding boxes. @@ -184,17 +193,19 @@ class Openvino{ * @param box_2 Second boudning box. * @return Ratio between the area of the overlap and area of the union. */ - double intersectionOverUnion(const DetectionObject &box_1, const DetectionObject &box_2); + double intersection_over_union(const DetectionObject &box_1, const DetectionObject &box_2); /** * @brief Convert frame to blob. * * @param frame The frame to be converted. - * @param inferRequest The inference request. - * @param inputName The name of the input. - * @param autoResize Value to change the size of the frame. + * @param infer_request The inference request. + * @param input_name The name of the input. + * @param auto_resize Value to change the size of the frame. */ - void frameToBlob(const cv::Mat &frame, InferenceEngine::InferRequest::Ptr &inferRequest, const std::string &inputName, bool autoResize = false); + void frame_to_blob(const cv::Mat &frame, + InferenceEngine::InferRequest::Ptr &infer_request, + const std::string &input_name, bool auto_resize = false); /** * @brief Parse Mobilenet-SSD neural network model output. @@ -205,21 +216,33 @@ class Openvino{ * @param[in] threshold Value below which objects will be discarded. * @param[out] objects The detected objects. */ - void parseSSDOutput(const InferenceEngine::Blob::Ptr &blob, const unsigned long height, const unsigned long width, const float threshold, std::vector &objects); + void parse_ssd_output(const InferenceEngine::Blob::Ptr &blob, + const unsigned long height, + const unsigned long width, + const float threshold, + std::vector &objects); /** * @brief Parse YOLOv3 neural network model output. * - * @param[in] cnnNetwork The - * @param[in] outputName The name of the output layer. + * @param[in] cnn_network The + * @param[in] output_name The name of the output layer. * @param[in] blob The frame with the detected objects. - * @param[in] resizedImgH Height in pixels of the image resized. - * @param[in] resizedImgW Width in pixels of the image resized. - * @param[in] originalImgH Height in pixels of the image. - * @param[in] originalImgW Width in pixels of the image. + * @param[in] resized_img_h Height in pixels of the image resized. + * @param[in] resized_img_w Width in pixels of the image resized. + * @param[in] original_img_h Height in pixels of the image. + * @param[in] original_img_w Width in pixels of the image. * @param[in] threshold Value below which objects will be discarded. * @param[out] objects The detected objects. */ - void parseYOLOV3Output(const InferenceEngine::CNNNetwork &cnnNetwork, const std::string &outputName, const InferenceEngine::Blob::Ptr &blob, const unsigned long resizedImgH, const unsigned long resizedImgW, const unsigned long originalImgH, const unsigned long originalImgW, const float threshold, std::vector &objects); + void parse_yolov3_output(const InferenceEngine::CNNNetwork &cnn_network, + const std::string &output_name, + const InferenceEngine::Blob::Ptr &blob, + const unsigned long resized_img_h, + const unsigned long resized_img_w, + const unsigned long original_img_h, + const unsigned long original_img_w, + const float threshold, + std::vector &objects); }; -#endif +#endif // OBJECT_DETECTION_OPENVINO__OPENVINO_HPP_ diff --git a/include/object_detection_openvino/yoloParams.h b/include/object_detection_openvino/yoloParams.hpp similarity index 72% rename from include/object_detection_openvino/yoloParams.h rename to include/object_detection_openvino/yoloParams.hpp index de8d8ce..db36a49 100644 --- a/include/object_detection_openvino/yoloParams.h +++ b/include/object_detection_openvino/yoloParams.hpp @@ -1,16 +1,16 @@ /* * YOLO PARAMS CLASS * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán + * Copyright (c) 2020-2022 Alberto José Tudela Roldán * * This file is part of object_detection_openvino project. * * All rights reserved. * */ - -#ifndef YOLO_PARAMS_H -#define YOLO_PARAMS_H + +#ifndef OBJECT_DETECTION_OPENVINO__YOLO_PARAMS_HPP_ +#define OBJECT_DETECTION_OPENVINO__YOLO_PARAMS_HPP_ // OpenVINO #include @@ -56,30 +56,6 @@ class YoloParams{ computeAnchors(mask); } - /** - * @brief Overloaded constructor, provided for convenience. - * - * @param layer The layer of the neural network. - */ - YoloParams(InferenceEngine::CNNLayer::Ptr layer){ - if(layer->type != "RegionYolo") - throw std::runtime_error("Invalid output type: " + layer->type + ". RegionYolo expected"); - - num = layer->GetParamAsInt("num"); - coords = layer->GetParamAsInt("coords"); - classes = layer->GetParamAsInt("classes"); - - try{ - anchors = layer->GetParamAsFloats("anchors"); - }catch (...){} - try{ - auto mask = layer->GetParamAsInts("mask"); - num = mask.size(); - - computeAnchors(mask); - }catch (...){} - } - /** * @brief Copy constructor. * @@ -109,4 +85,4 @@ class YoloParams{ } }; -#endif +#endif // OBJECT_DETECTION_OPENVINO__YOLO_PARAMS_HPP_ diff --git a/launch/default.launch.py b/launch/default.launch.py new file mode 100644 index 0000000..9d7d53a --- /dev/null +++ b/launch/default.launch.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +''' + Launches an object detection node with default parameters. +''' +import os + +from ament_index_python import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from nav2_common.launch import RewrittenYaml + +def generate_launch_description(): + # Getting directories and launch-files + object_detection_dir = get_package_share_directory('object_detection_openvino') + default_params_file = os.path.join(object_detection_dir, 'params', 'default_params.yaml') + model_xml_file = os.path.join(object_detection_dir, 'model', 'frozen_inference_graph.xml') + model_bin_file = os.path.join(object_detection_dir, 'model', 'frozen_inference_graph.bin') + labels_file = os.path.join(object_detection_dir, 'model', 'labels.txt') + + # Input parameters declaration + params_file = LaunchConfiguration('params_file') + network_type = LaunchConfiguration('network_type', default='yolo') + + declare_params_file_arg = DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the ROS2 parameters file with detection configuration' + ) + + declare_network_type_arg = DeclareLaunchArgument( + 'network_type', + default_value=network_type, + description='Network type: yolo or ssd' + ) + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'model_xml': model_xml_file, + 'model_bin': model_bin_file, + 'labels': labels_file, + } + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True + ) + + # Prepare the laser segmentation node. + detection_node = Node( + package = 'object_detection_openvino', + namespace = '', + executable = 'object_detection_openvino', + name = 'object_detection', + parameters=[configured_params], + emulate_tty = True + ) + + return LaunchDescription([ + declare_params_file_arg, + declare_network_type_arg, + detection_node + ]) \ No newline at end of file diff --git a/launch/mobilenet_cpu.launch b/launch/mobilenet_cpu.launch deleted file mode 100644 index 096ef56..0000000 --- a/launch/mobilenet_cpu.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/mobilenet_cpu_astrastereo.launch b/launch/mobilenet_cpu_astrastereo.launch deleted file mode 100644 index ded3034..0000000 --- a/launch/mobilenet_cpu_astrastereo.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/launch/mobilenet_cpu_realsense.launch b/launch/mobilenet_cpu_realsense.launch deleted file mode 100644 index 39f6a37..0000000 --- a/launch/mobilenet_cpu_realsense.launch +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - keep_organized: true - filter_field_name: z - filter_limit_min: 0.10 - filter_limit_max: 10.0 - filter_limit_negative: False - - - - - - - - - - - diff --git a/launch/mobilenet_myriad.launch b/launch/mobilenet_myriad.launch deleted file mode 100644 index c294914..0000000 --- a/launch/mobilenet_myriad.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/mobilenet_myriad_nodelet.launch b/launch/mobilenet_myriad_nodelet.launch deleted file mode 100644 index 44ba3cb..0000000 --- a/launch/mobilenet_myriad_nodelet.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/mobilenet_myriad_realsense.launch b/launch/mobilenet_myriad_realsense.launch deleted file mode 100644 index 93dc4e3..0000000 --- a/launch/mobilenet_myriad_realsense.launch +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - keep_organized: true - filter_field_name: z - filter_limit_min: 0.10 - filter_limit_max: 10.0 - filter_limit_negative: False - - - - - - - - - - - diff --git a/launch/mobilenet_myriad_realsense_nodelet.launch b/launch/mobilenet_myriad_realsense_nodelet.launch deleted file mode 100644 index d6620af..0000000 --- a/launch/mobilenet_myriad_realsense_nodelet.launch +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - keep_organized: true - filter_field_name: z - filter_limit_min: 0.10 - filter_limit_max: 10.0 - filter_limit_negative: False - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/yolo_cpu.launch b/launch/yolo_cpu.launch deleted file mode 100644 index bfbc576..0000000 --- a/launch/yolo_cpu.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/yolo_myriad.launch b/launch/yolo_myriad.launch deleted file mode 100644 index 8ec4aef..0000000 --- a/launch/yolo_myriad.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml deleted file mode 100644 index fab03a2..0000000 --- a/nodelet_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Object detection using Intel OpenVino - - diff --git a/package.xml b/package.xml index a415382..6b02715 100644 --- a/package.xml +++ b/package.xml @@ -1,25 +1,28 @@ - + + object_detection_openvino - 3.2.2 + 4.0.0 OpenVINO implementation of Darknet ROS (YoloV3) and Mobilenet-SSD networks to achieve realtime processing on x64 CPUs. Alberto Tudela Alberto Tudela BSD - catkin + ament_cmake boost - roscpp - nodelet + rclcpp + tf2 std_msgs sensor_msgs - geometry_msgs image_transport message_filters + visualization_msgs cv_bridge vision_msgs pcl_ros pcl_conversions + ament_lint_auto + ament_lint_common - + ament_cmake diff --git a/params/default_params.yaml b/params/default_params.yaml new file mode 100644 index 0000000..feeae36 --- /dev/null +++ b/params/default_params.yaml @@ -0,0 +1,20 @@ +object_detection: + ros__parameters: + show_fps: false + # Topics parametes + camera_frame: camera_link + color_topic: camera/color/image_raw + points_topic: camera/depth/color/points + detection_image_topic: image_raw + detection_info_topic: detection_info + detections_2d_topic: detections_2d + detections_3d_topic: detections_3d + # Thresholds parameters + model_thresh: 0.4 + model_iou_thresh: 0. 4 + # Model parameters + model_xml: model.xml + model_bin: model.bin + model_labels: model.labels + model_type: network_type + device_target: CPU \ No newline at end of file diff --git a/src/objectDetectionVPU.cpp b/src/objectDetectionVPU.cpp index 0099a41..be770b4 100644 --- a/src/objectDetectionVPU.cpp +++ b/src/objectDetectionVPU.cpp @@ -9,198 +9,226 @@ * */ +// C++ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// PCL +#include "pcl/filters/crop_box.h" -#include "object_detection_openvino/objectDetectionVPU.h" +// ROS +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" +#include "sensor_msgs/image_encodings.hpp" +#include "tf2_sensor_msgs/tf2_sensor_msgs.h" -ObjectDetectionVPU::ObjectDetectionVPU(ros::NodeHandle& node, ros::NodeHandle& node_private): node_(node), nodePrivate_(node_private), - imageTransport_(nodePrivate_), syncImagePCL_(SyncPolicyImagePCL(5), colorSub_, pointsSub_){ +#include "object_detection_openvino/objectDetectionVPU.hpp" + +ObjectDetectionVPU::ObjectDetectionVPU(std::string node_name) : + Node(node_name) , + openvino_(node_name){ // Initialize ROS parameters - getParams(); + get_params(); // Initialize values for depth analysis - useDepth_ = pointCloudTopic_.empty() ? false : true; + use_depth_ = pointcloud_topic_.empty() ? false : true; - // Initialize subscribers, create sync policy and synchronizer - colorSub_.subscribe(imageTransport_, colorTopic_, 10); - if (!useDepth_){ - colorSub_.registerCallback(boost::bind(&ObjectDetectionVPU::colorImageCallback, this,_1)); - }else{ - pointsSub_.subscribe(node_, pointCloudTopic_, 10); - syncImagePCL_.connectInput(colorSub_, pointsSub_); - syncImagePCL_.registerCallback(boost::bind(&ObjectDetectionVPU::colorPointCallback, this,_1,_2)); + // Initialize publishers + detection_info_pub_ = this->create_publisher( + detection_info_topic_, + rclcpp::QoS(1).transient_local()); + connect_info_callback(); + detection_color_pub_ = image_transport::create_publisher(this, detection_image_topic_); + detections_2d_pub_ = this->create_publisher(detections_2d_topic_, 1); + + if (use_depth_){ + detections_3d_pub_ = this->create_publisher(detections_3d_topic_, 1); + markers_pub_ = this->create_publisher("markers", 1); } - // Initialize publishers - detectionInfoPub_ = nodePrivate_.advertise(detectionInfoTopic_, 1, boost::bind(&ObjectDetectionVPU::connectInfoCallback, this, _1)); - detectionColorPub_ = imageTransport_.advertise(detectionImageTopic_, 1); - detections2DPub_ = nodePrivate_.advertise(detections2DTopic_, 1); + // Initialize subscribers, create sync policy and synchronizer + image_transport::TransportHints hints(this, "raw"); + color_sub_.subscribe(this, color_topic_, hints.getTransport()); - if (useDepth_){ - detections3DPub_ = nodePrivate_.advertise(detections3DTopic_, 1); - markersPub_ = nodePrivate_.advertise("markers", 1); + if (!use_depth_){ + color_sub_.registerCallback(std::bind(&ObjectDetectionVPU::color_image_callback, + this, + std::placeholders::_1)); + }else{ + points_sub_.subscribe(this, pointcloud_topic_); + approximate_sync_ = std::make_shared( + ApproximatePolicy(5), + color_sub_, + points_sub_); + approximate_sync_->registerCallback(std::bind(&ObjectDetectionVPU::color_point_callback, + this, + std::placeholders::_1, + std::placeholders::_2)); } + // Initialize transform buffer + tf_buffer_ = std::make_unique(this->get_clock()); + // Set target device - openvino_.setTargetDevice(deviceTarget_); + openvino_.set_target_device(device_target_); // Set network model - openvino_.setNetworkModel(modelFileName_, binFileName_, labelFileName_); + openvino_.set_network_model(model_filename_, bin_filename_, label_filename_); // Get labels - labels_ = openvino_.getLabels(); + labels_ = openvino_.get_labels(); - // Configuring input and output - openvino_.configureNetwork(networkType_); + // Configuring input and output + openvino_.configure_network(network_type_); // Load model to the device - openvino_.loadModelToDevice(deviceTarget_); + openvino_.load_model_to_device(device_target_); // Create async inference request - openvino_.createAsyncInferRequest(); + openvino_.create_async_infer_request(); } ObjectDetectionVPU::~ObjectDetectionVPU(){ - nodePrivate_.deleteParam("model_thresh"); - nodePrivate_.deleteParam("model_iou_thresh"); - - nodePrivate_.deleteParam("model_xml"); - nodePrivate_.deleteParam("model_bin"); - nodePrivate_.deleteParam("model_labels"); - nodePrivate_.deleteParam("model_type"); - nodePrivate_.deleteParam("device_target"); - - nodePrivate_.deleteParam("camera_frame"); - nodePrivate_.deleteParam("color_topic"); - nodePrivate_.deleteParam("points_topic"); - nodePrivate_.deleteParam("detection_image_topic"); - nodePrivate_.deleteParam("detection_info_topic"); - nodePrivate_.deleteParam("detections2d_topic"); - nodePrivate_.deleteParam("detections3d_topic"); - - nodePrivate_.deleteParam("show_fps"); } -void ObjectDetectionVPU::getParams(){ - ROS_INFO("[Object detection VPU]: Reading ROS parameters"); - - nodePrivate_.param("model_thresh", thresh_, 0.3); - nodePrivate_.param("model_iou_thresh", iouThresh_, 0.4); - - nodePrivate_.param("model_xml", modelFileName_, ""); - nodePrivate_.param("model_bin", binFileName_, ""); - nodePrivate_.param("model_labels", labelFileName_, ""); - nodePrivate_.param("model_type", networkType_, ""); - nodePrivate_.param("device_target", deviceTarget_, "CPU"); - - nodePrivate_.param("camera_frame", cameraFrameId_, "camera_link"); - nodePrivate_.param("color_topic", colorTopic_, "/camera/color/image_raw"); - nodePrivate_.param("points_topic", pointCloudTopic_, ""); - nodePrivate_.param("detection_image_topic", detectionImageTopic_, "image_raw"); - nodePrivate_.param("detection_info_topic", detectionInfoTopic_, "detection_info"); - nodePrivate_.param("detections2d_topic", detections2DTopic_, "detections2d"); - nodePrivate_.param("detections3d_topic", detections3DTopic_, "detections3d"); - - nodePrivate_.param("show_fps", showFPS_, false); +void ObjectDetectionVPU::get_params(){ + // Model parameters + this->declare_parameter("model_thresh", 0.3); + this->get_parameter("model_thresh", thresh_); + this->declare_parameter("model_iou_thresh", 0.4); + this->get_parameter("model_iou_thresh", iou_thresh_); + + // Network parameters + this->declare_parameter("model_xml", ""); + this->get_parameter("model_xml", model_filename_); + this->declare_parameter("model_bin", ""); + this->get_parameter("model_bin", bin_filename_); + this->declare_parameter("model_labels", ""); + this->get_parameter("model_labels", label_filename_); + this->declare_parameter("model_type", ""); + this->get_parameter("model_type", network_type_); + this->declare_parameter("device_target", "CPU"); + this->get_parameter("device_target", device_target_); + + this->declare_parameter("camera_frame", "camera_link"); + this->get_parameter("camera_frame", camera_frame_); + + // Topics + this->declare_parameter("color_topic", "/camera/color/image_raw"); + this->get_parameter("color_topic", color_topic_); + this->declare_parameter("points_topic", ""); + this->get_parameter("points_topic", pointcloud_topic_); + this->declare_parameter("detection_info_topic", "/detection_info"); + this->get_parameter("detection_info_topic", detection_info_topic_); + this->declare_parameter("detection_image_topic", "/detection_image"); + this->get_parameter("detection_image_topic", detection_image_topic_); + this->declare_parameter("detections_2d_topic", "/detections_2d"); + this->get_parameter("detections_2d_topic", detections_2d_topic_); + this->declare_parameter("detections_3d_topic", "/detections_3d"); + this->get_parameter("detections_3d_topic", detections_3d_topic_); + + this->declare_parameter("show_fps", false); + this->get_parameter("show_fps", show_fps_); } -void ObjectDetectionVPU::connectInfoCallback(const ros::SingleSubscriberPublisher& pub){ - ROS_INFO("[Object detection VPU]: Subscribed to vision info topic"); +// TODO(ros2) Implement when SubscriberStatusCallback is available +void ObjectDetectionVPU::connect_info_callback(){ + RCLCPP_INFO(this->get_logger(), "Subscribed to vision info topic"); // Create the key on the param server - std::string classKey = std::string("class_labels"); - if (!nodePrivate_.hasParam(classKey)){ - nodePrivate_.setParam(classKey, labels_); + std::string class_key = std::string("class_labels"); + if (!this->has_parameter(class_key)){ + this->declare_parameter(class_key, labels_); } // Create and publish info - vision_msgs::VisionInfo detectionInfo; - detectionInfo.header.frame_id = cameraFrameId_; - detectionInfo.header.stamp = ros::Time::now(); - detectionInfo.method = networkType_ + " detection"; - detectionInfo.database_version = 0; - detectionInfo.database_location = nodePrivate_.getNamespace() + std::string("/") + classKey; - - detectionInfoPub_.publish(detectionInfo); + vision_msgs::msg::VisionInfo detection_info; + detection_info.header.frame_id = camera_frame_; + detection_info.header.stamp = this->now(); + detection_info.method = network_type_ + " detection"; + detection_info.database_version = 0; + detection_info.database_location = this->get_namespace() + std::string("/") + class_key; + + detection_info_pub_->publish(detection_info); } -void ObjectDetectionVPU::colorImageCallback(const sensor_msgs::Image::ConstPtr& colorImageMsg){ - colorPointCallback(colorImageMsg, nullptr); +void ObjectDetectionVPU::color_image_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & color_image_msg){ + color_point_callback(color_image_msg, nullptr); } -void ObjectDetectionVPU::colorPointCallback(const sensor_msgs::Image::ConstPtr& colorImageMsg, const sensor_msgs::PointCloud2::ConstPtr& pointsMsg){ +void ObjectDetectionVPU::color_point_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & color_image_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg){ // Note: Only infer object if there's any subscriber - if (detectionColorPub_.getNumSubscribers() == 0 && detections2DPub_.getNumSubscribers() == 0 - && detections3DPub_.getNumSubscribers() == 0 && markersPub_.getNumSubscribers() == 0) return; - ROS_INFO_ONCE("[Object detection VPU]: Subscribed to color image topic: %s", colorTopic_.c_str()); + if (detection_color_pub_.getNumSubscribers() == 0 && + detections_2d_pub_->get_subscription_count() == 0 && + detections_3d_pub_->get_subscription_count() == 0 && + markers_pub_->get_subscription_count() == 0){ + return; + } + RCLCPP_INFO_ONCE(this->get_logger(), "Subscribed to color image topic: %s", color_topic_.c_str()); // Read header - colorFrameId_ = colorImageMsg->header.frame_id; + color_frame_ = color_image_msg->header.frame_id; // Create arrays to publish and format headers - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray marker_array; - vision_msgs::Detection2DArray detections2D; - detections2D.header.frame_id = colorFrameId_; - detections2D.header.stamp = colorImageMsg->header.stamp; + vision_msgs::msg::Detection2DArray detections_2d; + detections_2d.header.frame_id = color_frame_; + detections_2d.header.stamp = color_image_msg->header.stamp; - vision_msgs::Detection3DArray detections3D; - detections3D.header.frame_id = cameraFrameId_; - detections3D.header.stamp = colorImageMsg->header.stamp; + vision_msgs::msg::Detection3DArray detections_3d; + detections_3d.header.frame_id = camera_frame_; + detections_3d.header.stamp = color_image_msg->header.stamp; auto wallclock = std::chrono::high_resolution_clock::now(); // Convert from ROS to CV image - cv_bridge::CvImagePtr colorImageCv; + cv_bridge::CvImagePtr color_image_cv; try{ - colorImageCv = cv_bridge::toCvCopy(colorImageMsg, sensor_msgs::image_encodings::BGR8); + color_image_cv = cv_bridge::toCvCopy(color_image_msg, sensor_msgs::image_encodings::BGR8); }catch (cv_bridge::Exception& e){ - ROS_ERROR("[Object detection VPU]: cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - const size_t colorHeight = (size_t) colorImageCv->image.size().height; - const size_t colorWidth = (size_t) colorImageCv->image.size().width; + const size_t color_height = (size_t) color_image_cv->image.size().height; + const size_t color_width = (size_t) color_image_cv->image.size().width; // Copy data from image to input blob - nextFrame_ = colorImageCv->image.clone(); - openvino_.frameToNextInfer(nextFrame_, false); - - // Tranform the pointcloud - sensor_msgs::PointCloud2 localCloudPC2; - pcloud::Ptr localCloudPCLPtr(new pcl::PointCloud); - if (useDepth_){ - ROS_INFO_ONCE("[Object detection VPU]: Subscribed to pointcloud topic: %s", pointCloudTopic_.c_str()); + next_frame_ = color_image_cv->image.clone(); + openvino_.frame_to_next_infer(next_frame_, false); + + // Transform the pointcloud + sensor_msgs::msg::PointCloud2 local_cloud_pc2; + pcloud::Ptr local_cloud(new pcl::PointCloud); + if (use_depth_){ + RCLCPP_INFO_ONCE(this->get_logger(), "Subscribed to pointcloud topic: %s", pointcloud_topic_.c_str()); // Transform to camera frame try{ - pcl_ros::transformPointCloud(cameraFrameId_, *pointsMsg, localCloudPC2, tfListener_); - }catch (tf::TransformException& ex){ - ROS_ERROR_STREAM("[Object detection VPU]: Transform error of sensor data: " << ex.what() << ", quitting callback"); + geometry_msgs::msg::TransformStamped tf_stamped = + tf_buffer_->lookupTransform( + camera_frame_, points_msg->header.frame_id, + tf2_ros::fromMsg(points_msg->header.stamp)); + tf2::doTransform(*points_msg, local_cloud_pc2, tf_stamped); + }catch (const tf2::TransformException & ex){ + RCLCPP_WARN(this->get_logger(), ex.what()); return; } - // Convert to PCL - pcl::fromROSMsg(localCloudPC2, *localCloudPCLPtr); + // Convert the cloud to PCL format + pcl::fromROSMsg(local_cloud_pc2, *local_cloud); } // Load network // In the truly Async mode we start the NEXT infer request, while waiting for the CURRENT to complete auto t0 = std::chrono::high_resolution_clock::now(); - openvino_.startNextAsyncInferRequest(); + openvino_.start_next_async_infer_request(); auto t1 = std::chrono::high_resolution_clock::now(); - if (openvino_.isDeviceReady()){ + if (openvino_.is_device_ready()){ // Show FPS - if (showFPS_){ + if (show_fps_){ t1 = std::chrono::high_resolution_clock::now(); ms detection = std::chrono::duration_cast(t1 - t0); @@ -211,23 +239,23 @@ void ObjectDetectionVPU::colorPointCallback(const sensor_msgs::Image::ConstPtr& t0 = std::chrono::high_resolution_clock::now(); std::ostringstream out; - cv::putText(currFrame_, out.str(), cv::Point2f(0, 25), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + cv::putText(current_frame_, out.str(), cv::Point2f(0, 25), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); out.str(""); out << "Wallclock time "; out << std::fixed << std::setprecision(2) << wall.count() << " ms (" << 1000.f / wall.count() << " fps)"; - cv::putText(currFrame_, out.str(), cv::Point2f(0, 50), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); + cv::putText(current_frame_, out.str(), cv::Point2f(0, 50), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(0, 0, 255), 1, cv::LINE_AA); out.str(""); out << "Detection time : " << std::fixed << std::setprecision(2) << detection.count() << " ms (" << 1000.f / detection.count() << " fps)"; - cv::putText(currFrame_, out.str(), cv::Point2f(0, 75), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(255, 0, 0), 1, cv::LINE_AA); + cv::putText(current_frame_, out.str(), cv::Point2f(0, 75), cv::FONT_HERSHEY_TRIPLEX, 0.6, cv::Scalar(255, 0, 0), 1, cv::LINE_AA); } // Get detection objects - std::vector objects = openvino_.getDetectionObjects(colorHeight, colorWidth, thresh_, iouThresh_); + std::vector objects = openvino_.get_detection_objects(color_height, color_width, thresh_, iou_thresh_); - int detectionId = 0; + int detection_id = 0; /* Process objects */ for (auto &object: objects){ @@ -237,41 +265,41 @@ void ObjectDetectionVPU::colorPointCallback(const sensor_msgs::Image::ConstPtr& auto label = object.classId; float confidence = object.confidence; - ROS_DEBUG("[Object detection VPU]: %s tag (%.2f%%)", labels_[label].c_str(), confidence*100); + RCLCPP_DEBUG(this->get_logger(), "%s tag (%.2f%%)", labels_[label].c_str(), confidence*100); // Improve bounding box object.xmin = object.xmin < 0 ? 0 : object.xmin; object.ymin = object.ymin < 0 ? 0 : object.ymin; - object.xmax = object.xmax > colorWidth ? colorWidth : object.xmax; - object.ymax = object.ymax > colorHeight ? colorHeight : object.ymax; + object.xmax = object.xmax > color_width ? color_width : object.xmax; + object.ymax = object.ymax > color_height ? color_height : object.ymax; // Color of the class int offset = object.classId * 123457 % COCO_CLASSES; - float colorRGB[3]; - colorRGB[0] = getColor(2, offset, COCO_CLASSES); - colorRGB[1] = getColor(1, offset, COCO_CLASSES); - colorRGB[2] = getColor(0, offset, COCO_CLASSES); + float color_rgb[3]; + color_rgb[0] = get_color(2, offset, COCO_CLASSES); + color_rgb[1] = get_color(1, offset, COCO_CLASSES); + color_rgb[2] = get_color(0, offset, COCO_CLASSES); // Create detection2D and push to array - vision_msgs::Detection2D detection2D; - if (createDetection2DMsg(object, detections2D.header, detection2D)){ - detections2D.detections.push_back(detection2D); + vision_msgs::msg::Detection2D detection_2d; + if (create_detection_2d_msg(object, detections_2d.header, detection_2d)){ + detections_2d.detections.push_back(detection_2d); } - if (useDepth_){ + if (use_depth_){ // Create detection3D and push to array - vision_msgs::Detection3D detection3D; + vision_msgs::msg::Detection3D detection_3d; - if (createDetection3DMsg(object, detections3D.header, localCloudPC2, localCloudPCLPtr, detection3D)){ + if (create_detection_3d_msg(object, detections_3d.header, local_cloud_pc2, local_cloud, detection_3d)){ // Create markers - visualization_msgs::Marker vizMarker, labelMarker; + visualization_msgs::msg::Marker marker_viz, marker_label; - createBBox3DMarker(detectionId, detections3D.header, colorRGB, detection3D.bbox, vizMarker); - createLabel3DMarker(detectionId*10, detections3D.header, colorRGB, detection3D.bbox, labels_[label], labelMarker); + create_bbox_3d_marker(detection_id, detections_3d.header, color_rgb, detection_3d.bbox, marker_viz); + create_label_3d_marker(detection_id*10, detections_3d.header, color_rgb, detection_3d.bbox, labels_[label], marker_label); - detections3D.detections.push_back(detection3D); - markerArray.markers.push_back(vizMarker); - markerArray.markers.push_back(labelMarker); + detections_3d.detections.push_back(detection_3d); + marker_array.markers.push_back(marker_viz); + marker_array.markers.push_back(marker_label); } } @@ -279,94 +307,86 @@ void ObjectDetectionVPU::colorPointCallback(const sensor_msgs::Image::ConstPtr& // Text label std::ostringstream conf; conf << ":" << std::fixed << std::setprecision(3) << confidence; - std::string labelText = (label < this->labels_.size() ? this->labels_[label] : std::string("label #") + std::to_string(label)) + conf.str(); + std::string label_text = (label < this->labels_.size() ? this->labels_[label] : std::string("label #") + std::to_string(label)) + conf.str(); // Rectangles for class - cv::rectangle(currFrame_, cv::Point2f(object.xmin-1, object.ymin), cv::Point2f(object.xmin + 180, object.ymin - 22), cv::Scalar(colorRGB[2], colorRGB[1], colorRGB[0]), cv::FILLED, cv::LINE_AA); - cv::putText(currFrame_, labelText, cv::Point2f(object.xmin, object.ymin - 5), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(0, 0, 0), 1.5, cv::LINE_AA); - cv::rectangle(currFrame_, cv::Point2f(object.xmin, object.ymin), cv::Point2f(object.xmax, object.ymax), cv::Scalar(colorRGB[2], colorRGB[1], colorRGB[0]), 4, cv::LINE_AA); + cv::rectangle(current_frame_, cv::Point2f(object.xmin-1, object.ymin), cv::Point2f(object.xmin + 180, object.ymin - 22), cv::Scalar(color_rgb[2], color_rgb[1], color_rgb[0]), cv::FILLED, cv::LINE_AA); + cv::putText(current_frame_, label_text, cv::Point2f(object.xmin, object.ymin - 5), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(0, 0, 0), 1.5, cv::LINE_AA); + cv::rectangle(current_frame_, cv::Point2f(object.xmin, object.ymin), cv::Point2f(object.xmax, object.ymax), cv::Scalar(color_rgb[2], color_rgb[1], color_rgb[0]), 4, cv::LINE_AA); - detectionId++; + detection_id++; } // Publish detections and markers - publishImage(currFrame_); + publish_image(current_frame_); if (!objects.empty()){ - detections2DPub_.publish(detections2D); - if (useDepth_){ - detections3DPub_.publish(detections3D); - markersPub_.publish(markerArray); + detections_2d_pub_->publish(detections_2d); + if (use_depth_){ + detections_3d_pub_->publish(detections_3d); + markers_pub_->publish(marker_array); } } } // In the truly Async mode we swap the NEXT and CURRENT requests for the next iteration - currFrame_ = nextFrame_; - nextFrame_ = cv::Mat(); - openvino_.swapAsyncInferRequest(); + current_frame_ = next_frame_; + next_frame_ = cv::Mat(); + openvino_.swap_async_infer_request(); } -void ObjectDetectionVPU::showHistogram(cv::Mat image, cv::Scalar mean){ - int histSize = 256; - float range[] = { 0, histSize }; //the upper boundary is exclusive - const float* histRange = { range }; +void ObjectDetectionVPU::show_histogram(cv::Mat image, cv::Scalar mean){ + int hist_size = 256; + float range[] = { 0, hist_size }; //the upper boundary is exclusive + const float* hist_range = { range }; bool uniform = true, accumulate = false; - cv::Mat depthHist; - calcHist( &image, 1, 0, cv::Mat(), depthHist, 1, &histSize, &histRange, uniform, accumulate ); + cv::Mat depth_hist; + calcHist( &image, 1, 0, cv::Mat(), depth_hist, 1, &hist_size, &hist_range, uniform, accumulate ); int hist_w = 512, hist_h = 400; - int bin_w = cvRound( (double) hist_w/histSize ); - cv::Mat histImage(hist_h, hist_w, CV_8UC3, cv::Scalar( 0,0,0) ); - normalize(depthHist, depthHist, 0, histImage.rows, cv::NORM_MINMAX, -1, cv::Mat() ); - for(int i = 1; i < histSize; i++){ - cv::line(histImage, cv::Point( bin_w*(i-1), hist_h - cvRound(depthHist.at(i-1)) ), - cv::Point( bin_w*(i), hist_h - cvRound(depthHist.at(i)) ), - cv::Scalar( 0, 0, 255), 2, 8, 0 ); + int bin_w = cvRound( (double) hist_w/hist_size ); + cv::Mat hist_image(hist_h, hist_w, CV_8UC3, cv::Scalar( 0,0,0) ); + normalize(depth_hist, depth_hist, 0, hist_image.rows, cv::NORM_MINMAX, -1, cv::Mat() ); + for (int i = 1; i < hist_size; i++){ + cv::line(hist_image, cv::Point( bin_w*(i-1), hist_h - cvRound(depth_hist.at(i-1)) ), + cv::Point( bin_w*(i), hist_h - cvRound(depth_hist.at(i)) ), + cv::Scalar( 0, 0, 255), 2, 8, 0 ); } - cv::line(histImage, cv::Point(mean[0], 0), cv::Point(mean[0], histImage.rows), cv::Scalar(0,255,0)); - cv::imshow("Histogram", histImage ); + cv::line(hist_image, cv::Point(mean[0], 0), cv::Point(mean[0], hist_image.rows), cv::Scalar(0,255,0)); + cv::imshow("Histogram", hist_image ); cv::waitKey(10); } -bool ObjectDetectionVPU::createDetection2DMsg(DetectionObject object, std_msgs::Header header, vision_msgs::Detection2D& detection2D){ - detection2D.header = header; +bool ObjectDetectionVPU::create_detection_2d_msg(DetectionObject object, + std_msgs::msg::Header header, + vision_msgs::msg::Detection2D& detection_2d){ + detection_2d.header = header; // Class probabilities - vision_msgs::ObjectHypothesisWithPose hypo; - hypo.id = object.classId; - hypo.score = object.confidence; - detection2D.results.push_back(hypo); + vision_msgs::msg::ObjectHypothesisWithPose hypo_with_pose; + hypo_with_pose.hypothesis.class_id = std::to_string(object.classId); + hypo_with_pose.hypothesis.score = object.confidence; + detection_2d.results.push_back(hypo_with_pose); // 2D bounding box surrounding the object - detection2D.bbox.center.x = (object.xmax + object.xmin) / 2; - detection2D.bbox.center.y = (object.ymax + object.ymin) / 2; - detection2D.bbox.size_x = object.xmax - object.xmin; - detection2D.bbox.size_y = object.ymax - object.ymin; - - // The 2D data that generated these results - cv::Mat croppedImage = currFrame_(cv::Rect(object.xmin, object.ymin, object.xmax - object.xmin, object.ymax - object.ymin)); - - detection2D.source_img.header = header; - detection2D.source_img.height = croppedImage.rows; - detection2D.source_img.width = croppedImage.cols; - detection2D.source_img.encoding = "bgr8"; - detection2D.source_img.is_bigendian = false; - detection2D.source_img.step = croppedImage.cols * 3; - size_t size = detection2D.source_img.step * croppedImage.rows; - detection2D.source_img.data.resize(size); - memcpy((char*)(&detection2D.source_img.data[0]), croppedImage.data, size); + detection_2d.bbox.center.x = (object.xmax + object.xmin) / 2; + detection_2d.bbox.center.y = (object.ymax + object.ymin) / 2; + detection_2d.bbox.size_x = object.xmax - object.xmin; + detection_2d.bbox.size_y = object.ymax - object.ymin; return true; } -bool ObjectDetectionVPU::createDetection3DMsg(DetectionObject object, std_msgs::Header header, const sensor_msgs::PointCloud2& cloudPC2, pcloud::ConstPtr cloudPCL, vision_msgs::Detection3D& detection3D){ +bool ObjectDetectionVPU::create_detection_3d_msg(DetectionObject object, + std_msgs::msg::Header header, + const sensor_msgs::msg::PointCloud2& cloud_pc2, + pcloud::ConstPtr cloud_pcl, + vision_msgs::msg::Detection3D& detection_3d){ // Calculate the center in 3D coordinates - int centerX, centerY; - centerX = (object.xmax + object.xmin) / 2; - centerY = (object.ymax + object.ymin) / 2; + int center_x = (object.xmax + object.xmin) / 2; + int center_y = (object.ymax + object.ymin) / 2; - int pclIndex = centerX + (centerY * cloudPC2.width); - pcl::PointXYZRGB centerPoint = cloudPCL->at(pclIndex); + int index_pcl = center_x + (center_y * cloud_pc2.width); + pcl::PointXYZRGB center_point = cloud_pcl->at(index_pcl); - if (std::isnan(centerPoint.x)) return false; + if (std::isnan(center_point.x)) return false; // Calculate the bounding box float maxX, minX, maxY, minY, maxZ, minZ; @@ -375,11 +395,11 @@ bool ObjectDetectionVPU::createDetection3DMsg(DetectionObject object, std_msgs:: for (int i = object.xmin; i < object.xmax; i++){ for (int j = object.ymin; j < object.ymax; j++){ - pclIndex = i + (j * cloudPC2.width); - pcl::PointXYZRGB point = cloudPCL->at(pclIndex); + index_pcl = i + (j * cloud_pc2.width); + pcl::PointXYZRGB point = cloud_pcl->at(index_pcl); if (std::isnan(point.x)) continue; - if (fabs(point.x - centerPoint.x) > thresh_) continue; + if (fabs(point.x - center_point.x) > thresh_) continue; maxX = std::max(point.x, maxX); maxY = std::max(point.y, maxY); @@ -391,93 +411,89 @@ bool ObjectDetectionVPU::createDetection3DMsg(DetectionObject object, std_msgs:: } // Header - detection3D.header = header; + detection_3d.header = header; // 3D bounding box surrounding the object - detection3D.bbox.center.position.x = centerPoint.x; - detection3D.bbox.center.position.y = centerPoint.y; - detection3D.bbox.center.position.z = centerPoint.z; - detection3D.bbox.center.orientation.x = 0.0; - detection3D.bbox.center.orientation.y = 0.0; - detection3D.bbox.center.orientation.z = 0.0; - detection3D.bbox.center.orientation.w = 1.0; - detection3D.bbox.size.x = maxX - minX; - detection3D.bbox.size.y = maxY - minY; - detection3D.bbox.size.z = maxZ - minZ; + detection_3d.bbox.center.position.x = center_point.x; + detection_3d.bbox.center.position.y = center_point.y; + detection_3d.bbox.center.position.z = center_point.z; + detection_3d.bbox.center.orientation.x = 0.0; + detection_3d.bbox.center.orientation.y = 0.0; + detection_3d.bbox.center.orientation.z = 0.0; + detection_3d.bbox.center.orientation.w = 1.0; + detection_3d.bbox.size.x = maxX - minX; + detection_3d.bbox.size.y = maxY - minY; + detection_3d.bbox.size.z = maxZ - minZ; // Class probabilities // We use the pose as the min Z of the bounding box // Because this is not a "real" detection 3D - vision_msgs::ObjectHypothesisWithPose hypo; - hypo.id = object.classId; - hypo.score = object.confidence; - hypo.pose.pose = detection3D.bbox.center; - hypo.pose.pose.position.z -= detection3D.bbox.size.z / 2.0; - detection3D.results.push_back(hypo); - - // The 3D data that generated these results: - // Cropping the cloud - pcl::CropBox boxFilter; - pcloud::Ptr croppedCloudPCLPtr(new pcl::PointCloud); - boxFilter.setMin(Eigen::Vector4f(minX, minY, minZ, 1.0)); - boxFilter.setMax(Eigen::Vector4f(maxX, maxY, maxZ, 1.0)); - boxFilter.setInputCloud(cloudPCL); - boxFilter.filter(*croppedCloudPCLPtr); - // Convert to sensor_msgs - sensor_msgs::PointCloud2 croppedCloudPC2; - pcl::toROSMsg(*croppedCloudPCLPtr, croppedCloudPC2); - detection3D.source_cloud = croppedCloudPC2; + vision_msgs::msg::ObjectHypothesisWithPose hypo_with_pose; + hypo_with_pose.hypothesis.class_id = std::to_string(object.classId); + hypo_with_pose.hypothesis.score = object.confidence; + hypo_with_pose.pose.pose = detection_3d.bbox.center; + hypo_with_pose.pose.pose.position.z -= detection_3d.bbox.size.z / 2.0; + detection_3d.results.push_back(hypo_with_pose); return true; } -bool ObjectDetectionVPU::createBBox3DMarker(int id, std_msgs::Header header, float colorRGB[3], vision_msgs::BoundingBox3D bbox, visualization_msgs::Marker& marker){ +bool ObjectDetectionVPU::create_bbox_3d_marker(int id, + std_msgs::msg::Header header, + float color_rgb[3], + vision_msgs::msg::BoundingBox3D bbox, + visualization_msgs::msg::Marker& marker){ marker.header = header; marker.ns = "boundingBox3d"; marker.id = id; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.lifetime = ros::Duration(0.15); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.03); marker.pose = bbox.center; marker.scale = bbox.size; - marker.color.r = colorRGB[0] / 255.0; - marker.color.g = colorRGB[1] / 255.0; - marker.color.b = colorRGB[2] / 255.0; + marker.color.r = color_rgb[0] / 255.0; + marker.color.g = color_rgb[1] / 255.0; + marker.color.b = color_rgb[2] / 255.0; marker.color.a = 0.2f; return true; } -bool ObjectDetectionVPU::createLabel3DMarker(int id, std_msgs::Header header, float colorRGB[3], vision_msgs::BoundingBox3D bbox, std::string label, visualization_msgs::Marker& marker){ +bool ObjectDetectionVPU::create_label_3d_marker(int id, + std_msgs::msg::Header header, + float color_rgb[3], + vision_msgs::msg::BoundingBox3D bbox, + std::string label, + visualization_msgs::msg::Marker& marker){ marker.header = header; marker.ns = "label3d"; marker.id = id; marker.text = label; - marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::Marker::ADD; - marker.lifetime = ros::Duration(0.15); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.03); marker.pose = bbox.center; marker.pose.position.z += bbox.size.z / 2.0 + 0.05; marker.scale.z = 0.3; - marker.color.r = colorRGB[0] / 255.0; - marker.color.g = colorRGB[1] / 255.0; - marker.color.b = colorRGB[2] / 255.0; + marker.color.r = color_rgb[0] / 255.0; + marker.color.g = color_rgb[1] / 255.0; + marker.color.b = color_rgb[2] / 255.0; marker.color.a = 0.8f; return true; } -void ObjectDetectionVPU::publishImage(cv::Mat image){ - cv_bridge::CvImage outputImageMsg; - outputImageMsg.header.stamp = ros::Time::now(); - outputImageMsg.header.frame_id = colorFrameId_; - outputImageMsg.encoding = sensor_msgs::image_encodings::BGR8; - outputImageMsg.image = image; +void ObjectDetectionVPU::publish_image(cv::Mat image){ + cv_bridge::CvImage output_msg; + output_msg.header.stamp = this->now(); + output_msg.header.frame_id = color_frame_; + output_msg.encoding = sensor_msgs::image_encodings::BGR8; + output_msg.image = image; - detectionColorPub_.publish(outputImageMsg.toImageMsg()); + detection_color_pub_.publish(output_msg.toImageMsg()); } -int ObjectDetectionVPU::getColor(int c, int x, int max){ +int ObjectDetectionVPU::get_color(int c, int x, int max){ float colors[6][3] = { {1,0,1}, {0,0,1},{0,1,1},{0,1,0},{1,1,0},{1,0,0} }; float ratio = ((float)x/max)*5; diff --git a/src/objectDetectionVPU_node.cpp b/src/objectDetectionVPU_node.cpp index c62fe55..a755cf8 100644 --- a/src/objectDetectionVPU_node.cpp +++ b/src/objectDetectionVPU_node.cpp @@ -1,7 +1,7 @@ /* * OBJECT DETECTION VPU ROS NODE * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán + * Copyright (c) 2020-2022 Alberto José Tudela Roldán * * This file is part of object_detection_openvino project. * @@ -9,22 +9,15 @@ * */ -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "object_detection_openvino/objectDetectionVPU.hpp" /* Main */ int main(int argc, char** argv){ - ros::init(argc, argv, "object_detection_vpu"); - ros::NodeHandle node(""); - ros::NodeHandle node_private("~"); + rclcpp::init(argc, argv); - try{ - ROS_INFO("[Object detection VPU]: Initializing node"); - ObjectDetectionVPU detector(node, node_private); - ros::spin(); - }catch(const char* s){ - ROS_FATAL_STREAM("[Object detection VPU]: " << s); - }catch(...){ - ROS_FATAL_STREAM("[Object detection VPU]: Unexpected error"); - } + auto node = std::make_shared("object_detection_vpu"); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } diff --git a/src/objectDetectionVPU_nodelet.cpp b/src/objectDetectionVPU_nodelet.cpp deleted file mode 100644 index 497c267..0000000 --- a/src/objectDetectionVPU_nodelet.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* - * OBJECT DETECTION VPU NODELET - * - * Copyright (c) 2020-2021 Alberto José Tudela Roldán - * - * This file is part of object_detection_openvino project. - * - * All rights reserved. - * - */ - -#include -#include -#include - -namespace object_detection_openvino{ - using ObjectDetectionVPUNodelet = nodelet_helper::TNodelet; -} - -PLUGINLIB_EXPORT_CLASS(object_detection_openvino::ObjectDetectionVPUNodelet, nodelet::Nodelet) diff --git a/src/openvino.cpp b/src/openvino.cpp index ae1aae9..9395454 100644 --- a/src/openvino.cpp +++ b/src/openvino.cpp @@ -18,19 +18,21 @@ #include // ROS -#include +#include "rclcpp/rclcpp.hpp" -#include "object_detection_openvino/openvino.h" +#include "object_detection_openvino/openvino.hpp" -Openvino::Openvino(){ +Openvino::Openvino(std::string node_name) : node_name_(node_name){ } Openvino::~Openvino(){ } -void Openvino::setTargetDevice(std::string device){ - ROS_INFO_STREAM("[Object detection VPU]: Loading Inference Engine" << InferenceEngine::GetInferenceEngineVersion()); - ROS_INFO_STREAM("[Object detection VPU]: Device info: " << core_.GetVersions(device)); +void Openvino::set_target_device(std::string device){ + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name_), + "Loading Inference Engine" << InferenceEngine::GetInferenceEngineVersion()); + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name_), + "Device info: " << core_.GetVersions(device)); // Load extensions for the plugin #ifdef WITH_EXTENSIONS @@ -47,131 +49,154 @@ void Openvino::setTargetDevice(std::string device){ #endif } -void Openvino::setNetworkModel(std::string modelFileName, std::string binFileName, std::string labelFileName){ +void Openvino::set_network_model(std::string model_filename, + std::string bin_filename, + std::string label_filename){ // Read network model - ROS_INFO("[Object detection VPU]: Loading network files"); - if (!boost::filesystem::exists(modelFileName)){ - ROS_FATAL("[Object detection VPU]: Network file doesn't exist."); - ros::shutdown(); + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Loading network files"); + if (!boost::filesystem::exists(model_filename)){ + RCLCPP_FATAL(rclcpp::get_logger(node_name_), "Network file doesn't exist."); + rclcpp::shutdown(); } - cnnNetwork_ = core_.ReadNetwork(modelFileName, binFileName); + cnn_network_ = core_.ReadNetwork(model_filename, bin_filename); // Read labels (if any) - std::ifstream inputFile(labelFileName); - std::copy(std::istream_iterator(inputFile), std::istream_iterator(), std::back_inserter(this->labels_)); + std::ifstream input_file(label_filename); + std::copy(std::istream_iterator(input_file), + std::istream_iterator(), + std::back_inserter(this->labels_)); } -void Openvino::configureNetwork(std::string networkType){ - networkType_ = networkType; +void Openvino::configure_network(std::string network_type){ + network_type_ = network_type; // Prepare input blobs - ROS_INFO("[Object detection VPU]: Checking that the inputs are as expected"); - - inputInfo_ = InferenceEngine::InputsDataMap(cnnNetwork_.getInputsInfo()); - if (networkType_ == "YOLO"){ - if (inputInfo_.size() != 1){ - ROS_FATAL("[Object detection VPU]: Only accepts networks that have only one input"); - ros::shutdown(); + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Checking that the inputs are as expected"); + + input_info_ = InferenceEngine::InputsDataMap(cnn_network_.getInputsInfo()); + if (network_type_ == "YOLO"){ + if (input_info_.size() != 1){ + RCLCPP_FATAL(rclcpp::get_logger(node_name_), + "Only accepts networks that have only one input"); + rclcpp::shutdown(); } - InferenceEngine::InputInfo::Ptr& input = inputInfo_.begin()->second; - inputName_ = inputInfo_.begin()->first; + InferenceEngine::InputInfo::Ptr& input = input_info_.begin()->second; + input_name_ = input_info_.begin()->first; input->setPrecision(InferenceEngine::Precision::U8); input->getInputData()->setLayout(InferenceEngine::Layout::NCHW); - }else if (networkType_ == "SSD"){ - if (inputInfo_.size() != 1 && inputInfo_.size() != 2 ){ - ROS_FATAL("[Object detection VPU]: Only accepts networks with 1 or 2 inputs"); - ros::shutdown(); + }else if (network_type_ == "SSD"){ + if (input_info_.size() != 1 && input_info_.size() != 2 ){ + RCLCPP_FATAL(rclcpp::get_logger(node_name_), + "Only accepts networks with 1 or 2 inputs"); + rclcpp::shutdown(); } - for (auto &input : inputInfo_){ + for (auto &input : input_info_){ // First input contains images if (input.second->getTensorDesc().getDims().size() == 4){ - inputName_ = input.first; + input_name_ = input.first; input.second->setPrecision(InferenceEngine::Precision::U8); input.second->getInputData()->setLayout(InferenceEngine::Layout::NCHW); // Second input contains image info }else if (input.second->getTensorDesc().getDims().size() == 2){ - inputName_ = input.first; + input_name_ = input.first; input.second->setPrecision(InferenceEngine::Precision::FP32); }else{ - throw std::logic_error("[Object detection VPU]: Unsupported " + std::to_string(input.second->getTensorDesc().getDims().size()) + "D " - "input layer '" + input.first + "'. Only 2D and 4D input layers are supported"); - ros::shutdown(); + throw std::logic_error("OpenVino: Unsupported " + + std::to_string(input.second->getTensorDesc().getDims().size()) + + "D input layer '" + input.first + + "'. Only 2D and 4D input layers are supported"); + rclcpp::shutdown(); } } } // Set batch size to 1 - ROS_INFO("[Object detection VPU]: Batch size is forced to 1"); - InferenceEngine::ICNNNetwork::InputShapes inputShapes = cnnNetwork_.getInputShapes(); + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Batch size is forced to 1"); + InferenceEngine::ICNNNetwork::InputShapes inputShapes = cnn_network_.getInputShapes(); InferenceEngine::SizeVector& inSizeVector = inputShapes.begin()->second; inSizeVector[0] = 1; - cnnNetwork_.reshape(inputShapes); + cnn_network_.reshape(inputShapes); // Prepare output blobs - ROS_INFO("[Object detection VPU]: Checking that the outputs are as expected"); - outputInfo_ = InferenceEngine::OutputsDataMap(cnnNetwork_.getOutputsInfo()); - if (networkType_ == "YOLO"){ - if (outputInfo_.size() != 3 && outputInfo_.size() != 2){ - ROS_FATAL("[Object detection VPU]: Only accepts networks with three (YOLO) or two (tiny-YOLO) outputs"); - ros::shutdown(); + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Checking that the outputs are as expected"); + output_info_ = InferenceEngine::OutputsDataMap(cnn_network_.getOutputsInfo()); + if (network_type_ == "YOLO"){ + if (output_info_.size() != 3 && output_info_.size() != 2){ + RCLCPP_FATAL(rclcpp::get_logger(node_name_), + "Only accepts networks with three (YOLO) or two (tiny-YOLO) outputs"); + rclcpp::shutdown(); } - for (auto &output : outputInfo_){ + for (auto &output : output_info_){ output.second->setPrecision(InferenceEngine::Precision::FP32); output.second->setLayout(InferenceEngine::Layout::NCHW); } - }else if (networkType_ == "SSD"){ - if (outputInfo_.size() != 1){ - throw std::logic_error("[Object detection VPU]: Only accepts networks with one output"); + }else if (network_type_ == "SSD"){ + if (output_info_.size() != 1){ + throw std::logic_error("Openvino: Only accepts networks with one output"); } - for (auto &output : outputInfo_){ + for (auto &output : output_info_){ output.second->setPrecision(InferenceEngine::Precision::FP32); output.second->setLayout(InferenceEngine::Layout::NCHW); } } } -void Openvino::loadModelToDevice(std::string device){ - ROS_INFO("[Object detection VPU]: Loading model to the device"); - infNetwork_ = core_.LoadNetwork(cnnNetwork_, device); +void Openvino::load_model_to_device(std::string device){ + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Loading model to the device"); + inf_network_ = core_.LoadNetwork(cnn_network_, device); } -void Openvino::createAsyncInferRequest(){ - ROS_INFO("[Object detection VPU]: Create infer request"); - asyncInferRequestCurr_ = infNetwork_.CreateInferRequestPtr(); - asyncInferRequestNext_ = infNetwork_.CreateInferRequestPtr(); +void Openvino::create_async_infer_request(){ + RCLCPP_INFO(rclcpp::get_logger(node_name_), "Create infer request"); + async_infer_request_current_ = inf_network_.CreateInferRequestPtr(); + async_infer_request_next_ = inf_network_.CreateInferRequestPtr(); } -void Openvino::startNextAsyncInferRequest(){ - asyncInferRequestNext_->StartAsync(); +void Openvino::start_next_async_infer_request(){ + async_infer_request_next_->StartAsync(); } -void Openvino::swapAsyncInferRequest(){ - asyncInferRequestCurr_.swap(asyncInferRequestNext_); +void Openvino::swap_async_infer_request(){ + async_infer_request_current_.swap(async_infer_request_next_); } -bool Openvino::isDeviceReady(){ - return InferenceEngine::OK == asyncInferRequestCurr_->Wait(InferenceEngine::IInferRequest::WaitMode::RESULT_READY); +bool Openvino::is_device_ready(){ + return InferenceEngine::OK == async_infer_request_current_->Wait(InferenceEngine::IInferRequest::WaitMode::RESULT_READY); } -std::vector Openvino::getLabels(){ +std::vector Openvino::get_labels(){ return labels_; } -std::vector Openvino::getDetectionObjects(size_t height, size_t width, float threshold, float iouThreshold){ +std::vector Openvino::get_detection_objects(size_t height, + size_t width, + float threshold, + float iou_threshold){ // Processing output blobs of the CURRENT request - const InferenceEngine::TensorDesc& inputDesc = inputInfo_.begin()->second.get()->getTensorDesc(); - unsigned long resizedImgH = getTensorHeight(inputDesc); - unsigned long resizedImgW = getTensorWidth(inputDesc); + const InferenceEngine::TensorDesc& inputDesc = input_info_.begin()->second.get()->getTensorDesc(); + unsigned long resized_img_h = getTensorHeight(inputDesc); + unsigned long resized_img_w = getTensorWidth(inputDesc); // Parsing outputs std::vector objects; - for (auto &output: outputInfo_){ - auto outputName = output.first; - InferenceEngine::Blob::Ptr blob = asyncInferRequestCurr_->GetBlob(outputName); - - if (networkType_ == "YOLO") parseYOLOV3Output(cnnNetwork_, outputName, blob, resizedImgH, resizedImgW, height, width, threshold, objects); - else if (networkType_ == "SSD") parseSSDOutput(blob, height, width, threshold, objects); + for (auto &output: output_info_){ + auto output_name = output.first; + InferenceEngine::Blob::Ptr blob = async_infer_request_current_->GetBlob(output_name); + + if (network_type_ == "YOLO"){ + parse_yolov3_output(cnn_network_, + output_name, + blob, + resized_img_h, + resized_img_w, + height, + width, + threshold, + objects); + }else if (network_type_ == "SSD"){ + parse_ssd_output(blob, height, width, threshold, objects); + } } // Filtering overlapping boxes @@ -180,7 +205,7 @@ std::vector Openvino::getDetectionObjects(size_t height, size_t if (objects[i].confidence == 0) continue; for (int j = i + 1; j < objects.size(); ++j) { - if (intersectionOverUnion(objects[i], objects[j]) >= iouThreshold) { + if (intersection_over_union(objects[i], objects[j]) >= iou_threshold) { objects[j].confidence = 0; } } @@ -189,17 +214,17 @@ std::vector Openvino::getDetectionObjects(size_t height, size_t return objects; } -void Openvino::frameToNextInfer(const cv::Mat &frame, bool autoResize){ - frameToBlob(frame, asyncInferRequestNext_, inputName_, autoResize); +void Openvino::frame_to_next_infer(const cv::Mat &frame, bool auto_resize){ + frame_to_blob(frame, async_infer_request_next_, input_name_, auto_resize); } -int Openvino::entryIndex(int side, int lcoords, int lclasses, int location, int entry) { +int Openvino::entry_index(int side, int lcoords, int lclasses, int location, int entry) { int n = location / (side * side); int loc = location % (side * side); return n * side * side * (lcoords + lclasses + 1) + entry * side * side + loc; } -double Openvino::intersectionOverUnion(const DetectionObject &box_1, const DetectionObject &box_2) { +double Openvino::intersection_over_union(const DetectionObject &box_1, const DetectionObject &box_2) { double width_of_overlap_area = fmin(box_1.xmax, box_2.xmax) - fmax(box_1.xmin, box_2.xmin); double height_of_overlap_area = fmin(box_1.ymax, box_2.ymax) - fmax(box_1.ymin, box_2.ymin); double area_of_overlap; @@ -217,110 +242,127 @@ double Openvino::intersectionOverUnion(const DetectionObject &box_1, const Detec return area_of_overlap / area_of_union; } -void Openvino::frameToBlob(const cv::Mat &frame, InferenceEngine::InferRequest::Ptr &inferRequest, const std::string &inputName, bool autoResize){ - if (autoResize){ +void Openvino::frame_to_blob(const cv::Mat &frame, + InferenceEngine::InferRequest::Ptr &infer_request, + const std::string &input_name, bool auto_resize){ + if (auto_resize){ // Just set input blob containing read image. Resize and layout conversion will be done automatically - inferRequest->SetBlob(inputName, wrapMat2Blob(frame)); + infer_request->SetBlob(input_name, wrapMat2Blob(frame)); }else{ // Resize and copy data from the image to the input blob - InferenceEngine::Blob::Ptr frameBlob = inferRequest->GetBlob(inputName); - matU8ToBlob(frame, frameBlob); + InferenceEngine::Blob::Ptr frame_blob = infer_request->GetBlob(input_name); + matU8ToBlob(frame, frame_blob); } } -void Openvino::parseSSDOutput(const InferenceEngine::Blob::Ptr &blob, const unsigned long height, const unsigned long width, const float threshold, std::vector &objects){ +void Openvino::parse_ssd_output(const InferenceEngine::Blob::Ptr &blob, + const unsigned long height, + const unsigned long width, + const float threshold, + std::vector &objects){ // Validating output parameters - InferenceEngine::SizeVector outputDims = blob->getTensorDesc().getDims(); - int maxProposalCount = static_cast(blob->getTensorDesc().getDims()[2]); - const int objectSize = static_cast(blob->getTensorDesc().getDims()[3]); + InferenceEngine::SizeVector output_dims = blob->getTensorDesc().getDims(); + int max_proposal_count = static_cast(blob->getTensorDesc().getDims()[2]); + const int object_size = static_cast(blob->getTensorDesc().getDims()[3]); - if (objectSize != 7){ - throw std::logic_error("[Object detection VPU]: Output should have 7 as a last dimension"); + if (object_size != 7){ + throw std::logic_error("Openvino: Output should have 7 as a last dimension"); } - if (outputDims.size() != 4){ - throw std::logic_error("[Object detection VPU]: Incorrect output dimensions for SSD"); + if (output_dims.size() != 4){ + throw std::logic_error("Openvino: Incorrect output dimensions for SSD"); } - const float *outputBlob = blob->buffer().as::value_type *>(); - for (int i = 0; i < maxProposalCount; i++){ - float id = outputBlob[i * objectSize + 0]; + const float *output_blob = blob->buffer().as::value_type *>(); + for (int i = 0; i < max_proposal_count; i++){ + float id = output_blob[i * object_size + 0]; if (id < 0) break; - auto label = static_cast(outputBlob[i * objectSize + 1]); - float prob = outputBlob[i * objectSize + 2]; - float xmin = outputBlob[i * objectSize + 3] * width; - float ymin = outputBlob[i * objectSize + 4] * height; - float xmax = outputBlob[i * objectSize + 5] * width; - float ymax = outputBlob[i * objectSize + 6] * height; + auto label = static_cast(output_blob[i * object_size + 1]); + float prob = output_blob[i * object_size + 2]; + float xmin = output_blob[i * object_size + 3] * width; + float ymin = output_blob[i * object_size + 4] * height; + float xmax = output_blob[i * object_size + 5] * width; + float ymax = output_blob[i * object_size + 6] * height; - double newWidth = xmax - xmin; - double newHeight = ymax - ymin; + double new_height = ymax - ymin; + double new_width = xmax - xmin; if (prob < threshold) continue; - DetectionObject obj(xmin, ymin, newHeight, newWidth, label, this->labels_[label], prob); + DetectionObject obj(xmin, ymin, new_height, new_width, label, this->labels_[label], prob); objects.push_back(obj); } } -void Openvino::parseYOLOV3Output(const InferenceEngine::CNNNetwork &cnnNetwork, const std::string &outputName, const InferenceEngine::Blob::Ptr &blob, const unsigned long resizedImgH, const unsigned long resizedImgW, const unsigned long originalImgH, const unsigned long originalImgW, const float threshold, std::vector &objects) { +void Openvino::parse_yolov3_output(const InferenceEngine::CNNNetwork &cnn_network, + const std::string &output_name, + const InferenceEngine::Blob::Ptr &blob, + const unsigned long resized_img_h, + const unsigned long resized_img_w, + const unsigned long original_img_h, + const unsigned long original_img_w, + const float threshold, + std::vector &objects) { // Validating output parameters - const int outBlobH = static_cast(blob->getTensorDesc().getDims()[2]); - const int outBlobW = static_cast(blob->getTensorDesc().getDims()[3]); - if (outBlobH != outBlobW){ - throw std::runtime_error("[Object detection VPU]: Invalid size of output " + outputName + - " It should be in NCHW layout and H should be equal to W. Current H = " + std::to_string(outBlobH) + - ", current W = " + std::to_string(outBlobW)); + const int out_blob_h = static_cast(blob->getTensorDesc().getDims()[2]); + const int out_blob_w = static_cast(blob->getTensorDesc().getDims()[3]); + if (out_blob_h != out_blob_w){ + throw std::runtime_error("Openvino: Invalid size of output " + output_name + + " It should be in NCHW layout and H should be equal to W. Current H = " + + std::to_string(out_blob_h) + + ", current W = " + std::to_string(out_blob_w)); } // Extracting layer parameters YoloParams params; - if (auto ngraphFunction = cnnNetwork.getFunction()){ - for (const auto op: ngraphFunction->get_ops()){ - if (op->get_friendly_name() == outputName){ - auto regionYolo = std::dynamic_pointer_cast(op); - if (!regionYolo){ - throw std::runtime_error("Invalid output type: " + - std::string(regionYolo->get_type_info().name) + ". RegionYolo expected"); + if (auto ngraph_function = cnn_network.getFunction()){ + for (const auto op: ngraph_function->get_ops()){ + if (op->get_friendly_name() == output_name){ + auto region_yolo = std::dynamic_pointer_cast(op); + if (!region_yolo){ + throw std::runtime_error("Openvino: Invalid output type: " + + std::string(region_yolo->get_type_info().name) + + ". region_yolo expected"); } - params = regionYolo; + params = region_yolo; break; } } }else{ - throw std::runtime_error("Can't get ngraph::Function. Make sure the provided model is in IR version 10 or greater."); + throw std::runtime_error("Openvino: Can't get ngraph::Function." + "Make sure the provided model is in IR version 10 or greater."); } - auto side = outBlobH; - auto sideSquare = side * side; - const float *outputBlob = blob->buffer().as::value_type *>(); + auto side = out_blob_h; + auto side_square = side * side; + const float *output_blob = blob->buffer().as::value_type *>(); // Parsing YOLO Region output - for (int i = 0; i < sideSquare; ++i){ + for (int i = 0; i < side_square; ++i){ int row = i / side; int col = i % side; for (int n = 0; n < params.num; ++n){ - int objIdx = entryIndex(side, params.coords, params.classes, n * side * side + i, params.coords); - int boxIdx = entryIndex(side, params.coords, params.classes, n * side * side + i, 0); - float scale = outputBlob[objIdx]; + int obj_idx = entry_index(side, params.coords, params.classes, n * side * side + i, params.coords); + int box_idx = entry_index(side, params.coords, params.classes, n * side * side + i, 0); + float scale = output_blob[obj_idx]; if (scale < threshold) continue; - double x = (col + outputBlob[boxIdx + 0 * sideSquare]) / side * resizedImgW; - double y = (row + outputBlob[boxIdx + 1 * sideSquare]) / side * resizedImgH; - double height = std::exp(outputBlob[boxIdx + 3 * sideSquare]) * params.anchors[2 * n + 1]; - double width = std::exp(outputBlob[boxIdx + 2 * sideSquare]) * params.anchors[2 * n]; + double x = (col + output_blob[box_idx + 0 * side_square]) / side * resized_img_w; + double y = (row + output_blob[box_idx + 1 * side_square]) / side * resized_img_h; + double height = std::exp(output_blob[box_idx + 3 * side_square]) * params.anchors[2 * n + 1]; + double width = std::exp(output_blob[box_idx + 2 * side_square]) * params.anchors[2 * n]; for (int j = 0; j < params.classes; ++j) { - int classIdx = entryIndex(side, params.coords, params.classes, n * sideSquare + i, params.coords + 1 + j); - float prob = scale * outputBlob[classIdx]; + int class_idx = entry_index(side, params.coords, params.classes, n * side_square + i, params.coords + 1 + j); + float prob = scale * output_blob[class_idx]; if (prob < threshold) continue; DetectionObject obj(x, y, height, width, j, this->labels_[j], prob, - static_cast(originalImgH) / static_cast(resizedImgH), - static_cast(originalImgW) / static_cast(resizedImgW)); + static_cast(original_img_h) / static_cast(resized_img_h), + static_cast(original_img_w) / static_cast(resized_img_w)); objects.push_back(obj); } }