Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion yolox_ros_cpp/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

## Requirements
- ROS2 Humble
- ros-humble-generate-parameter-library
- OpenCV 4.x
- OpenVINO 2021.*
- TensorRT 8.x *
Expand Down Expand Up @@ -60,7 +61,7 @@ cd ~/ros2_ws
- Download model using the following script.
- https://github.com/PINTO0309/PINTO_model_zoo/blob/main/132_YOLOX/download_nano.sh
- `curl -s https://raw.githubusercontent.com/PINTO0309/PINTO_model_zoo/main/132_YOLOX/download_nano.sh | bash`

- ONNX model copy to weight dir
- `cp resouces_new/saved_model_yolox_tiny_480x640/yolox_tiny_480x640.onnx ./src/YOLOX-ROS/weights/onnx/`

Expand Down
12 changes: 7 additions & 5 deletions yolox_ros_cpp/yolox_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,16 @@ endif()

if(YOLOX_USE_TENSORRT)
find_package(CUDA REQUIRED)
find_library(NVINFER NAMES nvinfer REQUIRED)
find_library(NVINFERPLUGIN NAMES nvinfer_plugin REQUIRED)
find_library(NVPARSERS NAMES nvparsers REQUIRED)
find_library(NVONNXPARSER NAMES nvonnxparser REQUIRED)
if (NOT CUDA_TOOLKIT_ROOT_DIR)
set(CUDA_TOOLKIT_ROOT_DIR /usr/local/cuda)
endif()
find_path(TENSORRT_INCLUDE_DIR NvInfer.h
HINTS ${TENSORRT_ROOT} ${CUDA_TOOLKIT_ROOT_DIR}
PATH_SUFFIXES include)

set(ENABLE_TENSORRT ON)
set(TARGET_SRC src/yolox_tensorrt.cpp)
set(TARGET_LIBS nvinfer nvinfer_plugin nvparsers nvonnxparser)
set(TARGET_LIBS nvinfer nvinfer_plugin)
set(TARGET_DPENDENCIES CUDA)
endif()

Expand Down
2 changes: 1 addition & 1 deletion yolox_ros_cpp/yolox_cpp/include/yolox_cpp/coco_names.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ namespace yolox_cpp{
{0.857, 0.857, 0.857},
{0.000, 0.447, 0.741},
{0.314, 0.717, 0.741},
{0.50, 0.5, 0}
{0.500, 0.500, 0.000}
};
}
#endif
68 changes: 35 additions & 33 deletions yolox_ros_cpp/yolox_cpp/include/yolox_cpp/core.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,7 @@ namespace yolox_cpp
/**
* @brief Define names based depends on Unicode path support
*/
#define tcout std::cout
#define file_name_t std::string
#define imread_t cv::imread

struct Object
{
Expand All @@ -24,17 +22,21 @@ namespace yolox_cpp
int grid0;
int grid1;
int stride;
GridAndStride(const int grid0_, const int grid1_, const int stride_)
: grid0(grid0_), grid1(grid1_), stride(stride_)
{
}
};

class AbcYoloX
{
public:
AbcYoloX() {}
AbcYoloX(float nms_th = 0.45, float conf_th = 0.3,
std::string model_version = "0.1.1rc0",
int num_classes = 80, bool p6 = false)
AbcYoloX(const float nms_th = 0.45, const float conf_th = 0.3,
const std::string &model_version = "0.1.1rc0",
const int num_classes = 80, const bool p6 = false)
: nms_thresh_(nms_th), bbox_conf_thresh_(conf_th),
model_version_(model_version), num_classes_(num_classes), p6_(p6)
num_classes_(num_classes), p6_(p6), model_version_(model_version)
{
}
virtual std::vector<Object> inference(const cv::Mat &frame) = 0;
Expand All @@ -55,10 +57,10 @@ namespace yolox_cpp

cv::Mat static_resize(const cv::Mat &img)
{
float r = std::min(input_w_ / (img.cols * 1.0), input_h_ / (img.rows * 1.0));
const 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;
int unpad_h = r * img.rows;
const int unpad_w = r * img.cols;
const int unpad_h = r * img.rows;
cv::Mat re(unpad_h, unpad_w, CV_8UC3);
cv::resize(img, re, re.size());
cv::Mat out(input_h_, input_w_, CV_8UC3, cv::Scalar(114, 114, 114));
Expand All @@ -69,9 +71,9 @@ namespace yolox_cpp
// for NCHW
void blobFromImage(const cv::Mat &img, float *blob_data)
{
size_t channels = 3;
size_t img_h = img.rows;
size_t img_w = img.cols;
const size_t channels = 3;
const size_t img_h = img.rows;
const size_t img_w = img.cols;
if (this->model_version_ == "0.1.0")
{
for (size_t c = 0; c < channels; ++c)
Expand Down Expand Up @@ -104,9 +106,9 @@ namespace yolox_cpp
// for NHWC
void blobFromImage_nhwc(const cv::Mat &img, float *blob_data)
{
size_t channels = 3;
size_t img_h = img.rows;
size_t img_w = img.cols;
const size_t channels = 3;
const size_t img_h = img.rows;
const size_t img_w = img.cols;
if (this->model_version_ == "0.1.0")
{
for (size_t i = 0; i < img_h * img_w; ++i)
Expand Down Expand Up @@ -134,19 +136,19 @@ namespace yolox_cpp
{
for (auto stride : strides)
{
int num_grid_w = target_w / stride;
int num_grid_h = target_h / stride;
const int num_grid_w = target_w / stride;
const int num_grid_h = target_h / stride;
for (int g1 = 0; g1 < num_grid_h; ++g1)
{
for (int g0 = 0; g0 < num_grid_w; ++g0)
{
grid_strides.push_back((GridAndStride){g0, g1, stride});
grid_strides.emplace_back(g0, g1, stride);
}
}
}
}

void generate_yolox_proposals(const std::vector<GridAndStride> grid_strides, const float *feat_ptr, const float prob_threshold, std::vector<Object> &objects)
void generate_yolox_proposals(const std::vector<GridAndStride> &grid_strides, const float *feat_ptr, const float prob_threshold, std::vector<Object> &objects)
{
const int num_anchors = grid_strides.size();

Expand All @@ -158,13 +160,13 @@ namespace yolox_cpp

const int basic_pos = anchor_idx * (num_classes_ + 5);

float box_objectness = feat_ptr[basic_pos + 4];
const 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)
{
float box_cls_score = feat_ptr[basic_pos + 5 + class_idx];
float box_prob = box_objectness * box_cls_score;
const float box_cls_score = feat_ptr[basic_pos + 5 + class_idx];
const float box_prob = box_objectness * box_cls_score;
if (box_prob > max_class_score)
{
class_id = class_idx;
Expand All @@ -176,12 +178,12 @@ namespace yolox_cpp
// yolox/models/yolo_head.py decode logic
// outputs[..., :2] = (outputs[..., :2] + grids) * strides
// outputs[..., 2:4] = torch.exp(outputs[..., 2:4]) * strides
float x_center = (feat_ptr[basic_pos + 0] + grid0) * stride;
float y_center = (feat_ptr[basic_pos + 1] + grid1) * stride;
float w = exp(feat_ptr[basic_pos + 2]) * stride;
float h = exp(feat_ptr[basic_pos + 3]) * stride;
float x0 = x_center - w * 0.5f;
float y0 = y_center - h * 0.5f;
const float x_center = (feat_ptr[basic_pos + 0] + grid0) * stride;
const float y_center = (feat_ptr[basic_pos + 1] + grid1) * stride;
const float w = exp(feat_ptr[basic_pos + 2]) * stride;
const float h = exp(feat_ptr[basic_pos + 3]) * stride;
const float x0 = x_center - w * 0.5f;
const float y0 = y_center - h * 0.5f;

Object obj;
obj.rect.x = x0;
Expand All @@ -197,7 +199,7 @@ namespace yolox_cpp

float intersection_area(const Object &a, const Object &b)
{
cv::Rect_<float> inter = a.rect & b.rect;
const cv::Rect_<float> inter = a.rect & b.rect;
return inter.area();
}

Expand Down Expand Up @@ -260,8 +262,8 @@ namespace yolox_cpp
const Object &b = faceobjects[picked[j]];

// intersection over union
float inter_area = intersection_area(a, b);
float union_area = areas[i] + areas[picked[j]] - inter_area;
const float inter_area = intersection_area(a, b);
const float union_area = areas[i] + areas[picked[j]] - inter_area;
// float IoU = inter_area / union_area
if (inter_area / union_area > nms_threshold)
keep = 0;
Expand All @@ -272,7 +274,7 @@ namespace yolox_cpp
}
}

void decode_outputs(const float *prob, const std::vector<GridAndStride> grid_strides,
void decode_outputs(const float *prob, const std::vector<GridAndStride> &grid_strides,
std::vector<Object> &objects, const float bbox_conf_thresh,
const float scale, const int img_w, const int img_h)
{
Expand Down Expand Up @@ -312,4 +314,4 @@ namespace yolox_cpp
}
};
}
#endif
#endif
31 changes: 18 additions & 13 deletions yolox_ros_cpp/yolox_cpp/include/yolox_cpp/utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,39 +8,44 @@
#include "core.hpp"
#include "coco_names.hpp"

namespace yolox_cpp{
namespace utils{
namespace yolox_cpp
{
namespace utils
{

static std::vector<std::string> read_class_labels_file(file_name_t file_name)
static std::vector<std::string> read_class_labels_file(const file_name_t &file_name)
{
std::vector<std::string> class_names;
std::ifstream ifs(file_name);
std::string buff;
if(ifs.fail()){
if (ifs.fail())
{
return class_names;
}
while (getline(ifs, buff)) {
while (getline(ifs, buff))
{
if (buff == "")
continue;
class_names.push_back(buff);
}
return class_names;
}

static void draw_objects(cv::Mat bgr, const std::vector<Object>& objects, const std::vector<std::string>& class_names=COCO_CLASSES)
static void draw_objects(cv::Mat bgr, const std::vector<Object> &objects, const std::vector<std::string> &class_names = COCO_CLASSES)
{

for (size_t i = 0; i < objects.size(); i++)
for (const Object &obj : objects)
{
const Object& obj = objects[i];

int color_index = obj.label % 80;
const int color_index = obj.label % 80;
cv::Scalar color = cv::Scalar(color_list[color_index][0], color_list[color_index][1], color_list[color_index][2]);
float c_mean = cv::mean(color)[0];
cv::Scalar txt_color;
if (c_mean > 0.5){
if (c_mean > 0.5)
{
txt_color = cv::Scalar(0, 0, 0);
}else{
}
else
{
txt_color = cv::Scalar(255, 255, 255);
}

Expand Down Expand Up @@ -68,4 +73,4 @@ namespace yolox_cpp{
}
}
}
#endif
#endif
2 changes: 2 additions & 0 deletions yolox_ros_cpp/yolox_cpp/include/yolox_cpp/yolox_tensorrt.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ namespace yolox_cpp{
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",
int num_classes=80, bool p6=false);
~YoloXTensorRT();
std::vector<Object> inference(const cv::Mat& frame) override;

private:
Expand All @@ -49,6 +50,7 @@ namespace yolox_cpp{
int output_size_;
const int inputIndex_ = 0;
const int outputIndex_ = 1;
void *inference_buffers_[2];

};
} // namespace yolox_cpp
Expand Down
Loading