diff --git a/yolox_ros_cpp/yolox_cpp/CMakeLists.txt b/yolox_ros_cpp/yolox_cpp/CMakeLists.txt index fc9ee11..62ef630 100644 --- a/yolox_ros_cpp/yolox_cpp/CMakeLists.txt +++ b/yolox_ros_cpp/yolox_cpp/CMakeLists.txt @@ -25,11 +25,6 @@ set(ENABLE_TENSORRT OFF) # find dependencies find_package(ament_cmake REQUIRED) find_package(OpenCV REQUIRED) -find_package(OpenMP) -if(OpenMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -endif() if(YOLOX_USE_OPENVINO) find_package(InferenceEngine) @@ -139,7 +134,7 @@ install( DIRECTORY include/ DESTINATION include ) - + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/core.hpp b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/core.hpp index 739dcd6..04d3294 100644 --- a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/core.hpp +++ b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/core.hpp @@ -33,7 +33,7 @@ namespace yolox_cpp{ :nms_thresh_(nms_th), bbox_conf_thresh_(conf_th), model_version_(model_version) {} - virtual std::vector inference(cv::Mat frame) = 0; + virtual std::vector inference(const cv::Mat& frame) = 0; protected: int input_w_; int input_h_; @@ -46,7 +46,8 @@ namespace yolox_cpp{ const std::vector strides_ = {8, 16, 32}; std::vector grid_strides_; - cv::Mat static_resize(cv::Mat& img) { + cv::Mat static_resize(const cv::Mat& img) + { float r = std::min(input_w_ / (img.cols*1.0), input_h_ / (img.rows*1.0)); // r = std::min(r, 1.0f); int unpad_w = r * img.cols; @@ -58,31 +59,31 @@ namespace yolox_cpp{ return out; } - void blobFromImage(cv::Mat& img, float *blob_data){ + void blobFromImage(const cv::Mat& img, float *blob_data) + { int channels = 3; int img_h = img.rows; int img_w = img.cols; if(this->model_version_=="0.1.0"){ - for (size_t c = 0; c < channels; c++) + for (size_t c = 0; c < channels; ++c) { - for (size_t h = 0; h < img_h; h++) + for (size_t h = 0; h < img_h; ++h) { - for (size_t w = 0; w < img_w; w++) + for (size_t w = 0; w < img_w; ++w) { blob_data[c * img_w * img_h + h * img_w + w] = - ((float)img.at(h, w)[c] / 255.0 - this->mean_[c]) / this->std_[c]; + ((float)img.ptr(h)[w][c]/ 255.0 - this->mean_[c]) / this->std_[c]; } } } }else{ - for (size_t c = 0; c < channels; c++) + for (size_t c = 0; c < channels; ++c) { - for (size_t h = 0; h < img_h; h++) + for (size_t h = 0; h < img_h; ++h) { - for (size_t w = 0; w < img_w; w++) + for (size_t w = 0; w < img_w; ++w) { - blob_data[c * img_w * img_h + h * img_w + w] = - (float)img.at(h, w)[c]; // 0.1.1rc0 or later + blob_data[c * img_w * img_h + h * img_w + w] = (float)img.ptr(h)[w][c]; // 0.1.1rc0 or later } } } @@ -95,9 +96,9 @@ namespace yolox_cpp{ { int num_grid_w = target_w / stride; int num_grid_h = target_h / stride; - for (int g1 = 0; g1 < num_grid_h; g1++) + for (int g1 = 0; g1 < num_grid_h; ++g1) { - for (int g0 = 0; g0 < num_grid_w; g0++) + for (int g0 = 0; g0 < num_grid_w; ++g0) { grid_strides.push_back((GridAndStride){g0, g1, stride}); } @@ -109,7 +110,7 @@ namespace yolox_cpp{ { const int num_anchors = grid_strides.size(); - for (int anchor_idx = 0; anchor_idx < num_anchors; anchor_idx++) + for (int anchor_idx = 0; anchor_idx < num_anchors; ++anchor_idx) { const int grid0 = grid_strides[anchor_idx].grid0; const int grid1 = grid_strides[anchor_idx].grid1; @@ -120,7 +121,7 @@ namespace yolox_cpp{ float box_objectness = feat_ptr[basic_pos + 4]; int class_id = 0; float max_class_score = 0.0; - for (int class_idx = 0; class_idx < num_classes_; class_idx++) + for (int class_idx = 0; class_idx < num_classes_; ++class_idx) { float box_cls_score = feat_ptr[basic_pos + 5 + class_idx]; float box_prob = box_objectness * box_cls_score; @@ -168,32 +169,21 @@ namespace yolox_cpp{ while (i <= j) { while (faceobjects[i].prob > p) - i++; + ++i; while (faceobjects[j].prob < p) - j--; + --j; if (i <= j) { - // swap std::swap(faceobjects[i], faceobjects[j]); - i++; - j--; - } - } - - #pragma omp parallel sections - { - #pragma omp section - { - if (left < j) qsort_descent_inplace(faceobjects, left, j); - } - #pragma omp section - { - if (i < right) qsort_descent_inplace(faceobjects, i, right); + ++i; + --j; } } + if (left < j) qsort_descent_inplace(faceobjects, left, j); + if (i < right) qsort_descent_inplace(faceobjects, i, right); } void qsort_descent_inplace(std::vector& objects) @@ -211,17 +201,18 @@ namespace yolox_cpp{ const int n = faceobjects.size(); std::vector areas(n); - for (int i = 0; i < n; i++) + for (int i = 0; i < n; ++i) { areas[i] = faceobjects[i].rect.area(); } - for (int i = 0; i < n; i++) + for (int i = 0; i < n; ++i) { const Object& a = faceobjects[i]; + const int picked_size = picked.size(); int keep = 1; - for (int j = 0; j < (int)picked.size(); j++) + for (int j = 0; j < picked_size; ++j) { const Object& b = faceobjects[picked[j]]; @@ -238,19 +229,23 @@ namespace yolox_cpp{ } } - void decode_outputs(const float* prob, const std::vector grid_strides, std::vector& objects, const float bbox_conf_thresh, const float scale, const int img_w, const int img_h) { - std::vector proposals; + void decode_outputs(const float* prob, const std::vector grid_strides, + std::vector& objects, const float bbox_conf_thresh, + const float scale, const int img_w, const int img_h) + { + std::vector proposals; generate_yolox_proposals(grid_strides, prob, bbox_conf_thresh, proposals); qsort_descent_inplace(proposals); std::vector picked; nms_sorted_bboxes(proposals, picked, nms_thresh_); + int count = picked.size(); objects.resize(count); - for (int i = 0; i < count; i++) + for (int i = 0; i < count; ++i) { objects[i] = proposals[picked[i]]; diff --git a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_openvino.hpp b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_openvino.hpp index 885912d..26a65d8 100644 --- a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_openvino.hpp +++ b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_openvino.hpp @@ -17,7 +17,7 @@ namespace yolox_cpp{ public: YoloXOpenVINO(file_name_t path_to_model, std::string device_name, float nms_th=0.45, float conf_th=0.3, std::string model_version="0.1.1rc0"); - std::vector inference(cv::Mat frame) override; + std::vector inference(const cv::Mat& frame) override; private: std::string device_name_; diff --git a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_tensorrt.hpp b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_tensorrt.hpp index b09a7a4..769a8de 100644 --- a/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_tensorrt.hpp +++ b/yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_tensorrt.hpp @@ -35,7 +35,7 @@ namespace yolox_cpp{ public: YoloXTensorRT(file_name_t path_to_engine, int device=0, float nms_th=0.45, float conf_th=0.3, std::string model_version="0.1.1rc0"); - std::vector inference(cv::Mat frame) override; + std::vector inference(const cv::Mat& frame) override; private: void doInference(float* input, float* output); diff --git a/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp b/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp index 115f3a9..79d27a0 100644 --- a/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp +++ b/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp @@ -62,7 +62,8 @@ namespace yolox_cpp{ generate_grids_and_stride(this->input_w_, this->input_h_, this->strides_, this->grid_strides_); } - std::vector YoloXOpenVINO::inference(cv::Mat frame){ + std::vector YoloXOpenVINO::inference(const cv::Mat& frame) + { // preprocess cv::Mat pr_img = static_resize(frame); InferenceEngine::Blob::Ptr imgBlob = infer_request_.GetBlob(input_name_); @@ -79,12 +80,7 @@ namespace yolox_cpp{ // do inference /* Running the request synchronously */ - try{ - infer_request_.Infer(); - }catch (const std::exception& ex) { - std::cerr << ex.what() << std::endl; - return {}; - } + infer_request_.Infer(); // Process output const InferenceEngine::Blob::Ptr output_blob = infer_request_.GetBlob(output_name_); @@ -100,7 +96,6 @@ namespace yolox_cpp{ const float* net_pred = moutputHolder.as::value_type*>(); float scale = std::min(input_w_ / (frame.cols*1.0), input_h_ / (frame.rows*1.0)); - std::vector objects; decode_outputs(net_pred, this->grid_strides_, objects, this->bbox_conf_thresh_, scale, frame.cols, frame.rows); return objects; diff --git a/yolox_ros_cpp/yolox_cpp/src/yolox_tensorrt.cpp b/yolox_ros_cpp/yolox_cpp/src/yolox_tensorrt.cpp index cddbff6..1c5fb12 100644 --- a/yolox_ros_cpp/yolox_cpp/src/yolox_tensorrt.cpp +++ b/yolox_ros_cpp/yolox_cpp/src/yolox_tensorrt.cpp @@ -42,7 +42,7 @@ namespace yolox_cpp{ auto out_dims = this->engine_->getBindingDimensions(1); this->output_size_ = 1; - for(int j=0;joutput_size_ *= out_dims.d[j]; } @@ -57,7 +57,9 @@ namespace yolox_cpp{ // Prepare GridAndStrides generate_grids_and_stride(this->input_w_, this->input_h_, this->strides_, this->grid_strides_); } - std::vector YoloXTensorRT::inference(cv::Mat frame){ + + std::vector YoloXTensorRT::inference(const cv::Mat& frame) + { // preprocess auto pr_img = static_resize(frame); float* input_blob = new float[pr_img.total()*3]; @@ -77,7 +79,8 @@ namespace yolox_cpp{ return objects; } - void YoloXTensorRT::doInference(float* input, float* output) { + void YoloXTensorRT::doInference(float* input, float* output) + { // Pointers to input and output device buffers to pass to engine. // Engine requires exactly IEngine::getNbBindings() number of buffers. void* buffers[2]; diff --git a/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp b/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp index 7b6e07c..faccc04 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp +++ b/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp @@ -7,8 +7,8 @@ namespace yolox_ros_cpp{ {} YoloXNode::YoloXNode(const std::string &node_name, const rclcpp::NodeOptions& options) - : rclcpp::Node("yolox_ros_cpp", node_name, options){ - + : rclcpp::Node("yolox_ros_cpp", node_name, options) + { RCLCPP_INFO(this->get_logger(), "initialize"); this->initializeParameter(); @@ -52,13 +52,14 @@ namespace yolox_ros_cpp{ } - void YoloXNode::initializeParameter(){ + void YoloXNode::initializeParameter() + { this->declare_parameter("imshow_isshow", true); - this->declare_parameter("model_path", "/root/ros2_ws/src/YOLOX-ROS/weights/tensorrt/YOLOX_outputs/nano/model_trt.engine"); + this->declare_parameter("model_path", "src/YOLOX-ROS/weights/openvino/yolox_tiny.xml"); this->declare_parameter("conf", 0.3f); this->declare_parameter("nms", 0.45f); - this->declare_parameter("device", "0"); - this->declare_parameter("model_type", "tensorrt"); + this->declare_parameter("device", "CPU"); + this->declare_parameter("model_type", "openvino"); this->declare_parameter("model_version", "0.1.1rc0"); this->declare_parameter("src_image_topic_name", "image_raw"); this->declare_parameter("publish_image_topic_name", "yolox/image_raw"); @@ -86,14 +87,20 @@ namespace yolox_ros_cpp{ RCLCPP_INFO(this->get_logger(), "Set parameter publish_image_topic_name: '%s'", this->publish_image_topic_name_.c_str()); } - void YoloXNode::colorImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& ptr){ + void YoloXNode::colorImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& ptr) + { auto img = cv_bridge::toCvCopy(ptr, "bgr8"); cv::Mat frame = img->image; // fps auto now = std::chrono::system_clock::now(); + auto objects = this->yolox_->inference(frame); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - now); + RCLCPP_INFO(this->get_logger(), "Inference: %f FPS", 1000.0f / elapsed.count()); + yolox_cpp::utils::draw_objects(frame, objects); if(this->imshow_){ cv::imshow(this->WINDOW_NAME_, frame); @@ -110,11 +117,9 @@ namespace yolox_ros_cpp{ pub_img = cv_bridge::CvImage(img->header, "bgr8", frame).toImageMsg(); this->pub_image_.publish(pub_img); - auto end = std::chrono::system_clock::now(); - auto elapsed = std::chrono::duration_cast(end - now); - RCLCPP_INFO(this->get_logger(), "fps: %f", 1000.0f / elapsed.count()); } - bboxes_ex_msgs::msg::BoundingBoxes YoloXNode::objects_to_bboxes(cv::Mat frame, std::vector objects, std_msgs::msg::Header header){ + bboxes_ex_msgs::msg::BoundingBoxes YoloXNode::objects_to_bboxes(cv::Mat frame, std::vector objects, std_msgs::msg::Header header) + { bboxes_ex_msgs::msg::BoundingBoxes boxes; boxes.header = header; for(auto obj: objects){