900 lines
36 KiB
C++
900 lines
36 KiB
C++
#include <atomic>
|
|
#include <chrono>
|
|
#include <cstddef>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <memory>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include "face/face_result.h"
|
|
#include "node.h"
|
|
#include "utils/dma_alloc.h"
|
|
#include "utils/logger.h"
|
|
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
#include "im2d.hpp"
|
|
#include "im2d_buffer.h"
|
|
#include "im2d_type.h"
|
|
#endif
|
|
|
|
namespace rk3588 {
|
|
|
|
namespace {
|
|
|
|
constexpr int kObjClassNum = 80;
|
|
|
|
const char* kCocoLabels[kObjClassNum] = {
|
|
"person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat",
|
|
"traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat",
|
|
"dog", "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack",
|
|
"umbrella", "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball",
|
|
"kite", "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket",
|
|
"bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple",
|
|
"sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair",
|
|
"couch", "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse",
|
|
"remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator",
|
|
"book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"
|
|
};
|
|
|
|
struct Color {
|
|
uint8_t r, g, b;
|
|
};
|
|
|
|
const Color kClassColors[] = {
|
|
{255, 0, 0}, {0, 255, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255},
|
|
{0, 255, 255}, {128, 0, 0}, {0, 128, 0}, {0, 0, 128}, {128, 128, 0},
|
|
{128, 0, 128}, {0, 128, 128}, {255, 128, 0}, {255, 0, 128}, {128, 255, 0},
|
|
{0, 255, 128}, {128, 0, 255}, {0, 128, 255}, {255, 128, 128}, {128, 255, 128}
|
|
};
|
|
|
|
inline Color GetClassColor(int cls_id) {
|
|
return kClassColors[cls_id % 20];
|
|
}
|
|
|
|
inline const char* GetClassName(int cls_id) {
|
|
if (cls_id >= 0 && cls_id < kObjClassNum) {
|
|
return kCocoLabels[cls_id];
|
|
}
|
|
return "unknown";
|
|
}
|
|
|
|
inline int Clamp(int val, int min_val, int max_val) {
|
|
return val < min_val ? min_val : (val > max_val ? max_val : val);
|
|
}
|
|
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
inline uint32_t PackColorArgb(const Color& c) {
|
|
return (0xFFu << 24) | (static_cast<uint32_t>(c.r) << 16) |
|
|
(static_cast<uint32_t>(c.g) << 8) | static_cast<uint32_t>(c.b);
|
|
}
|
|
|
|
inline bool WrapFrameAsRgaDst(const Frame& frame, rga_buffer_t& dst, int& rga_stride_bytes) {
|
|
if (frame.dma_fd < 0) return false;
|
|
|
|
int rga_fmt = 0;
|
|
int stride_bytes = 0;
|
|
int wstride = 0;
|
|
int hstride = frame.height;
|
|
|
|
if (frame.format == PixelFormat::RGB) {
|
|
rga_fmt = RK_FORMAT_RGB_888;
|
|
stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width * 3);
|
|
wstride = stride_bytes / 3;
|
|
if (frame.planes[0].size > 0 && stride_bytes > 0) {
|
|
const int hs = frame.planes[0].size / stride_bytes;
|
|
if (hs >= frame.height) hstride = hs;
|
|
}
|
|
} else if (frame.format == PixelFormat::BGR) {
|
|
rga_fmt = RK_FORMAT_BGR_888;
|
|
stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width * 3);
|
|
wstride = stride_bytes / 3;
|
|
if (frame.planes[0].size > 0 && stride_bytes > 0) {
|
|
const int hs = frame.planes[0].size / stride_bytes;
|
|
if (hs >= frame.height) hstride = hs;
|
|
}
|
|
} else if (frame.format == PixelFormat::NV12) {
|
|
rga_fmt = RK_FORMAT_YCbCr_420_SP;
|
|
stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width);
|
|
wstride = stride_bytes;
|
|
if (frame.planes[0].size > 0 && stride_bytes > 0) {
|
|
const int hs = frame.planes[0].size / stride_bytes;
|
|
if (hs >= frame.height) hstride = hs;
|
|
}
|
|
} else if (frame.format == PixelFormat::YUV420) {
|
|
rga_fmt = RK_FORMAT_YCbCr_420_P;
|
|
stride_bytes = frame.planes[0].stride > 0 ? frame.planes[0].stride : (frame.stride > 0 ? frame.stride : frame.width);
|
|
wstride = stride_bytes;
|
|
if (frame.planes[0].size > 0 && stride_bytes > 0) {
|
|
const int hs = frame.planes[0].size / stride_bytes;
|
|
if (hs >= frame.height) hstride = hs;
|
|
}
|
|
} else {
|
|
return false;
|
|
}
|
|
|
|
rga_stride_bytes = stride_bytes;
|
|
dst = wrapbuffer_fd_t(frame.dma_fd, frame.width, frame.height, wstride, hstride, rga_fmt);
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
void DrawHLine(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
|
|
int x1, int x2, int y, int thickness, const Color& color) {
|
|
x1 = Clamp(x1, 0, w - 1);
|
|
x2 = Clamp(x2, 0, w - 1);
|
|
if (x1 > x2) std::swap(x1, x2);
|
|
|
|
for (int t = 0; t < thickness; ++t) {
|
|
int cy = y + t;
|
|
if (cy < 0 || cy >= h) continue;
|
|
|
|
if (fmt == PixelFormat::RGB) {
|
|
for (int x = x1; x <= x2; ++x) {
|
|
int idx = (cy * stride) + x * 3;
|
|
data[idx] = color.r;
|
|
data[idx + 1] = color.g;
|
|
data[idx + 2] = color.b;
|
|
}
|
|
} else if (fmt == PixelFormat::BGR) {
|
|
for (int x = x1; x <= x2; ++x) {
|
|
int idx = (cy * stride) + x * 3;
|
|
data[idx] = color.b;
|
|
data[idx + 1] = color.g;
|
|
data[idx + 2] = color.r;
|
|
}
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
|
|
for (int x = x1; x <= x2; ++x) {
|
|
data[cy * stride + x] = Y;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void DrawVLine(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
|
|
int x, int y1, int y2, int thickness, const Color& color) {
|
|
y1 = Clamp(y1, 0, h - 1);
|
|
y2 = Clamp(y2, 0, h - 1);
|
|
if (y1 > y2) std::swap(y1, y2);
|
|
|
|
for (int t = 0; t < thickness; ++t) {
|
|
int cx = x + t;
|
|
if (cx < 0 || cx >= w) continue;
|
|
|
|
if (fmt == PixelFormat::RGB) {
|
|
for (int y = y1; y <= y2; ++y) {
|
|
int idx = (y * stride) + cx * 3;
|
|
data[idx] = color.r;
|
|
data[idx + 1] = color.g;
|
|
data[idx + 2] = color.b;
|
|
}
|
|
} else if (fmt == PixelFormat::BGR) {
|
|
for (int y = y1; y <= y2; ++y) {
|
|
int idx = (y * stride) + cx * 3;
|
|
data[idx] = color.b;
|
|
data[idx + 1] = color.g;
|
|
data[idx + 2] = color.r;
|
|
}
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
|
|
for (int y = y1; y <= y2; ++y) {
|
|
data[y * stride + cx] = Y;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void DrawRect(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
|
|
int x1, int y1, int x2, int y2, int thickness, const Color& color) {
|
|
DrawHLine(data, w, h, stride, fmt, x1, x2, y1, thickness, color);
|
|
DrawHLine(data, w, h, stride, fmt, x1, x2, y2 - thickness + 1, thickness, color);
|
|
DrawVLine(data, w, h, stride, fmt, x1, y1, y2, thickness, color);
|
|
DrawVLine(data, w, h, stride, fmt, x2 - thickness + 1, y1, y2, thickness, color);
|
|
}
|
|
|
|
const uint8_t kFont5x7[96][7] = {
|
|
{0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // ' '
|
|
{0x04,0x04,0x04,0x04,0x00,0x00,0x04}, // '!'
|
|
{0x0A,0x0A,0x0A,0x00,0x00,0x00,0x00}, // '"'
|
|
{0x0A,0x0A,0x1F,0x0A,0x1F,0x0A,0x0A}, // '#'
|
|
{0x04,0x0F,0x14,0x0E,0x05,0x1E,0x04}, // '$'
|
|
{0x18,0x19,0x02,0x04,0x08,0x13,0x03}, // '%'
|
|
{0x0C,0x12,0x14,0x08,0x15,0x12,0x0D}, // '&'
|
|
{0x0C,0x04,0x08,0x00,0x00,0x00,0x00}, // '''
|
|
{0x02,0x04,0x08,0x08,0x08,0x04,0x02}, // '('
|
|
{0x08,0x04,0x02,0x02,0x02,0x04,0x08}, // ')'
|
|
{0x00,0x04,0x15,0x0E,0x15,0x04,0x00}, // '*'
|
|
{0x00,0x04,0x04,0x1F,0x04,0x04,0x00}, // '+'
|
|
{0x00,0x00,0x00,0x00,0x0C,0x04,0x08}, // ','
|
|
{0x00,0x00,0x00,0x1F,0x00,0x00,0x00}, // '-'
|
|
{0x00,0x00,0x00,0x00,0x00,0x0C,0x0C}, // '.'
|
|
{0x00,0x01,0x02,0x04,0x08,0x10,0x00}, // '/'
|
|
{0x0E,0x11,0x13,0x15,0x19,0x11,0x0E}, // '0'
|
|
{0x04,0x0C,0x04,0x04,0x04,0x04,0x0E}, // '1'
|
|
{0x0E,0x11,0x01,0x02,0x04,0x08,0x1F}, // '2'
|
|
{0x1F,0x02,0x04,0x02,0x01,0x11,0x0E}, // '3'
|
|
{0x02,0x06,0x0A,0x12,0x1F,0x02,0x02}, // '4'
|
|
{0x1F,0x10,0x1E,0x01,0x01,0x11,0x0E}, // '5'
|
|
{0x06,0x08,0x10,0x1E,0x11,0x11,0x0E}, // '6'
|
|
{0x1F,0x01,0x02,0x04,0x08,0x08,0x08}, // '7'
|
|
{0x0E,0x11,0x11,0x0E,0x11,0x11,0x0E}, // '8'
|
|
{0x0E,0x11,0x11,0x0F,0x01,0x02,0x0C}, // '9'
|
|
{0x00,0x0C,0x0C,0x00,0x0C,0x0C,0x00}, // ':'
|
|
{0x00,0x0C,0x0C,0x00,0x0C,0x04,0x08}, // ';'
|
|
{0x02,0x04,0x08,0x10,0x08,0x04,0x02}, // '<'
|
|
{0x00,0x00,0x1F,0x00,0x1F,0x00,0x00}, // '='
|
|
{0x08,0x04,0x02,0x01,0x02,0x04,0x08}, // '>'
|
|
{0x0E,0x11,0x01,0x02,0x04,0x00,0x04}, // '?'
|
|
{0x0E,0x11,0x17,0x15,0x17,0x10,0x0E}, // '@'
|
|
{0x0E,0x11,0x11,0x1F,0x11,0x11,0x11}, // 'A'
|
|
{0x1E,0x11,0x11,0x1E,0x11,0x11,0x1E}, // 'B'
|
|
{0x0E,0x11,0x10,0x10,0x10,0x11,0x0E}, // 'C'
|
|
{0x1C,0x12,0x11,0x11,0x11,0x12,0x1C}, // 'D'
|
|
{0x1F,0x10,0x10,0x1E,0x10,0x10,0x1F}, // 'E'
|
|
{0x1F,0x10,0x10,0x1E,0x10,0x10,0x10}, // 'F'
|
|
{0x0E,0x11,0x10,0x17,0x11,0x11,0x0F}, // 'G'
|
|
{0x11,0x11,0x11,0x1F,0x11,0x11,0x11}, // 'H'
|
|
{0x0E,0x04,0x04,0x04,0x04,0x04,0x0E}, // 'I'
|
|
{0x07,0x02,0x02,0x02,0x02,0x12,0x0C}, // 'J'
|
|
{0x11,0x12,0x14,0x18,0x14,0x12,0x11}, // 'K'
|
|
{0x10,0x10,0x10,0x10,0x10,0x10,0x1F}, // 'L'
|
|
{0x11,0x1B,0x15,0x15,0x11,0x11,0x11}, // 'M'
|
|
{0x11,0x11,0x19,0x15,0x13,0x11,0x11}, // 'N'
|
|
{0x0E,0x11,0x11,0x11,0x11,0x11,0x0E}, // 'O'
|
|
{0x1E,0x11,0x11,0x1E,0x10,0x10,0x10}, // 'P'
|
|
{0x0E,0x11,0x11,0x11,0x15,0x12,0x0D}, // 'Q'
|
|
{0x1E,0x11,0x11,0x1E,0x14,0x12,0x11}, // 'R'
|
|
{0x0F,0x10,0x10,0x0E,0x01,0x01,0x1E}, // 'S'
|
|
{0x1F,0x04,0x04,0x04,0x04,0x04,0x04}, // 'T'
|
|
{0x11,0x11,0x11,0x11,0x11,0x11,0x0E}, // 'U'
|
|
{0x11,0x11,0x11,0x11,0x11,0x0A,0x04}, // 'V'
|
|
{0x11,0x11,0x11,0x15,0x15,0x15,0x0A}, // 'W'
|
|
{0x11,0x11,0x0A,0x04,0x0A,0x11,0x11}, // 'X'
|
|
{0x11,0x11,0x11,0x0A,0x04,0x04,0x04}, // 'Y'
|
|
{0x1F,0x01,0x02,0x04,0x08,0x10,0x1F}, // 'Z'
|
|
{0x0E,0x08,0x08,0x08,0x08,0x08,0x0E}, // '['
|
|
{0x00,0x10,0x08,0x04,0x02,0x01,0x00}, // '\'
|
|
{0x0E,0x02,0x02,0x02,0x02,0x02,0x0E}, // ']'
|
|
{0x04,0x0A,0x11,0x00,0x00,0x00,0x00}, // '^'
|
|
{0x00,0x00,0x00,0x00,0x00,0x00,0x1F}, // '_'
|
|
{0x08,0x04,0x02,0x00,0x00,0x00,0x00}, // '`'
|
|
{0x00,0x00,0x0E,0x01,0x0F,0x11,0x0F}, // 'a'
|
|
{0x10,0x10,0x16,0x19,0x11,0x11,0x1E}, // 'b'
|
|
{0x00,0x00,0x0E,0x10,0x10,0x11,0x0E}, // 'c'
|
|
{0x01,0x01,0x0D,0x13,0x11,0x11,0x0F}, // 'd'
|
|
{0x00,0x00,0x0E,0x11,0x1F,0x10,0x0E}, // 'e'
|
|
{0x06,0x09,0x08,0x1C,0x08,0x08,0x08}, // 'f'
|
|
{0x00,0x0F,0x11,0x11,0x0F,0x01,0x0E}, // 'g'
|
|
{0x10,0x10,0x16,0x19,0x11,0x11,0x11}, // 'h'
|
|
{0x04,0x00,0x0C,0x04,0x04,0x04,0x0E}, // 'i'
|
|
{0x02,0x00,0x06,0x02,0x02,0x12,0x0C}, // 'j'
|
|
{0x10,0x10,0x12,0x14,0x18,0x14,0x12}, // 'k'
|
|
{0x0C,0x04,0x04,0x04,0x04,0x04,0x0E}, // 'l'
|
|
{0x00,0x00,0x1A,0x15,0x15,0x11,0x11}, // 'm'
|
|
{0x00,0x00,0x16,0x19,0x11,0x11,0x11}, // 'n'
|
|
{0x00,0x00,0x0E,0x11,0x11,0x11,0x0E}, // 'o'
|
|
{0x00,0x00,0x1E,0x11,0x1E,0x10,0x10}, // 'p'
|
|
{0x00,0x00,0x0D,0x13,0x0F,0x01,0x01}, // 'q'
|
|
{0x00,0x00,0x16,0x19,0x10,0x10,0x10}, // 'r'
|
|
{0x00,0x00,0x0E,0x10,0x0E,0x01,0x1E}, // 's'
|
|
{0x08,0x08,0x1C,0x08,0x08,0x09,0x06}, // 't'
|
|
{0x00,0x00,0x11,0x11,0x11,0x13,0x0D}, // 'u'
|
|
{0x00,0x00,0x11,0x11,0x11,0x0A,0x04}, // 'v'
|
|
{0x00,0x00,0x11,0x11,0x15,0x15,0x0A}, // 'w'
|
|
{0x00,0x00,0x11,0x0A,0x04,0x0A,0x11}, // 'x'
|
|
{0x00,0x00,0x11,0x11,0x0F,0x01,0x0E}, // 'y'
|
|
{0x00,0x00,0x1F,0x02,0x04,0x08,0x1F}, // 'z'
|
|
{0x02,0x04,0x04,0x08,0x04,0x04,0x02}, // '{'
|
|
{0x04,0x04,0x04,0x04,0x04,0x04,0x04}, // '|'
|
|
{0x08,0x04,0x04,0x02,0x04,0x04,0x08}, // '}'
|
|
{0x00,0x00,0x08,0x15,0x02,0x00,0x00}, // '~'
|
|
{0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // DEL
|
|
};
|
|
|
|
void DrawChar(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
|
|
int x, int y, char c, int scale, const Color& color) {
|
|
if (c < 32 || c > 127) c = ' ';
|
|
int idx = c - 32;
|
|
const uint8_t* glyph = kFont5x7[idx];
|
|
|
|
for (int row = 0; row < 7; ++row) {
|
|
for (int col = 0; col < 5; ++col) {
|
|
if (glyph[row] & (1 << (4 - col))) {
|
|
for (int sy = 0; sy < scale; ++sy) {
|
|
for (int sx = 0; sx < scale; ++sx) {
|
|
int px = x + col * scale + sx;
|
|
int py = y + row * scale + sy;
|
|
if (px < 0 || px >= w || py < 0 || py >= h) continue;
|
|
|
|
if (fmt == PixelFormat::RGB) {
|
|
int i = py * stride + px * 3;
|
|
data[i] = color.r;
|
|
data[i + 1] = color.g;
|
|
data[i + 2] = color.b;
|
|
} else if (fmt == PixelFormat::BGR) {
|
|
int i = py * stride + px * 3;
|
|
data[i] = color.b;
|
|
data[i + 1] = color.g;
|
|
data[i + 2] = color.r;
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
uint8_t Y = static_cast<uint8_t>(0.299f * color.r + 0.587f * color.g + 0.114f * color.b);
|
|
data[py * stride + px] = Y;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void DrawText(uint8_t* data, int w, int h, int stride, PixelFormat fmt,
|
|
int x, int y, const char* text, int scale, const Color& color) {
|
|
int cx = x;
|
|
while (*text) {
|
|
DrawChar(data, w, h, stride, fmt, cx, y, *text, scale, color);
|
|
cx += 6 * scale;
|
|
++text;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
class OsdNode : public INode {
|
|
public:
|
|
std::string Id() const override { return id_; }
|
|
std::string Type() const override { return "osd"; }
|
|
|
|
bool Init(const SimpleJson& config, const NodeContext& ctx) override {
|
|
id_ = config.ValueOr<std::string>("id", "osd");
|
|
draw_bbox_ = config.ValueOr<bool>("draw_bbox", true);
|
|
draw_text_ = config.ValueOr<bool>("draw_text", true);
|
|
draw_face_det_ = config.ValueOr<bool>("draw_face_det", draw_face_det_);
|
|
line_width_ = config.ValueOr<int>("line_width", 2);
|
|
font_scale_ = config.ValueOr<int>("font_scale", 1);
|
|
use_rga_bbox_ = config.ValueOr<bool>("use_rga_bbox", use_rga_bbox_);
|
|
|
|
if (const SimpleJson* dbg = config.Find("debug"); dbg && dbg->IsObject()) {
|
|
stats_log_ = dbg->ValueOr<bool>("stats", stats_log_);
|
|
stats_interval_ = std::max<uint64_t>(
|
|
1, static_cast<uint64_t>(dbg->ValueOr<int>("stats_interval", static_cast<int>(stats_interval_))));
|
|
}
|
|
|
|
// Load custom labels from config
|
|
if (const SimpleJson* labels = config.Find("labels")) {
|
|
for (const auto& item : labels->AsArray()) {
|
|
labels_.push_back(item.AsString(""));
|
|
}
|
|
LogInfo("[osd] loaded " + std::to_string(labels_.size()) + " custom labels");
|
|
}
|
|
|
|
input_queue_ = ctx.input_queue;
|
|
if (!input_queue_) {
|
|
LogError("[osd] no input queue for node " + id_);
|
|
return false;
|
|
}
|
|
if (ctx.output_queues.empty()) {
|
|
LogError("[osd] no output queue for node " + id_);
|
|
return false;
|
|
}
|
|
output_queues_ = ctx.output_queues;
|
|
return true;
|
|
}
|
|
|
|
bool Start() override {
|
|
LogInfo(std::string("[osd] start id=") + id_ + " draw_bbox=" + (draw_bbox_ ? "true" : "false") +
|
|
" draw_text=" + (draw_text_ ? "true" : "false") +
|
|
" draw_face_det=" + (draw_face_det_ ? "true" : "false") +
|
|
" use_rga_bbox=" + (use_rga_bbox_ ? "true" : "false"));
|
|
#if !defined(RK3588_ENABLE_RGA)
|
|
if (use_rga_bbox_ && draw_bbox_) {
|
|
LogError("[osd] use_rga_bbox=true but RK3588_ENABLE_RGA is disabled at build; bbox will NOT be drawn id=" + id_);
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
void Stop() override {
|
|
LogInfo("[osd] stop id=" + id_);
|
|
}
|
|
|
|
NodeStatus Process(FramePtr frame) override {
|
|
if (!frame) return NodeStatus::DROP;
|
|
|
|
FramePtr out = frame;
|
|
if (out->data && (out->det || out->face_det || out->face_recog)) {
|
|
if (out.use_count() > 1) {
|
|
out = CloneFrameForWrite(out);
|
|
}
|
|
DrawDetections(out);
|
|
if (draw_face_det_) DrawFaceDet(out);
|
|
DrawFaceRecog(out);
|
|
}
|
|
|
|
PushToDownstream(out);
|
|
++processed_;
|
|
|
|
if (stats_log_ && stats_interval_ > 0 && (processed_ % stats_interval_) == 0) {
|
|
LogInfo("[osd] processed=" + std::to_string(processed_) + " id=" + id_);
|
|
}
|
|
return NodeStatus::OK;
|
|
}
|
|
|
|
private:
|
|
static FramePtr CloneFrameForWrite(const FramePtr& src) {
|
|
if (!src || !src->data || src->data_size == 0) return src;
|
|
|
|
// Prefer DMA clone when src already lives in DMA-BUF.
|
|
if (src->dma_fd >= 0) {
|
|
if (auto dma = DmaAlloc(src->data_size); dma && dma->valid()) {
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
// Fast path: RGA copy.
|
|
rga_buffer_t rga_src{};
|
|
rga_buffer_t rga_dst{};
|
|
int dummy_stride = 0;
|
|
Frame dst_frame = *src;
|
|
dst_frame.dma_fd = dma->fd;
|
|
dst_frame.data = dma->data();
|
|
dst_frame.data_size = dma->size;
|
|
dst_frame.data_owner = dma;
|
|
if (WrapFrameAsRgaDst(*src, rga_src, dummy_stride) &&
|
|
WrapFrameAsRgaDst(dst_frame, rga_dst, dummy_stride)) {
|
|
if (imcopy(rga_src, rga_dst, 1, nullptr) == IM_STATUS_SUCCESS) {
|
|
auto out = std::make_shared<Frame>(*src);
|
|
out->dma_fd = dma->fd;
|
|
out->data = dma->data();
|
|
out->data_size = dma->size;
|
|
out->data_owner = dma;
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
FramePlane p = src->planes[i];
|
|
int offset = p.offset;
|
|
if (p.data && src->data) {
|
|
const ptrdiff_t d = p.data - src->data;
|
|
if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
|
|
offset = static_cast<int>(d);
|
|
}
|
|
}
|
|
p.offset = offset;
|
|
if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
|
|
p.data = out->data + offset;
|
|
} else {
|
|
p.data = nullptr;
|
|
}
|
|
out->planes[i] = p;
|
|
}
|
|
return out;
|
|
}
|
|
}
|
|
#endif
|
|
// Fallback: CPU memcpy (requires cache sync on linux).
|
|
DmaSyncStartFd(src->dma_fd);
|
|
DmaSyncStart(dma.get());
|
|
memcpy(dma->data(), src->data, src->data_size);
|
|
DmaSyncEnd(dma.get());
|
|
DmaSyncEndFd(src->dma_fd);
|
|
|
|
auto out = std::make_shared<Frame>(*src);
|
|
out->dma_fd = dma->fd;
|
|
out->data = dma->data();
|
|
out->data_size = dma->size;
|
|
out->data_owner = dma;
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
FramePlane p = src->planes[i];
|
|
int offset = p.offset;
|
|
if (p.data && src->data) {
|
|
const ptrdiff_t d = p.data - src->data;
|
|
if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
|
|
offset = static_cast<int>(d);
|
|
}
|
|
}
|
|
p.offset = offset;
|
|
if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
|
|
p.data = out->data + offset;
|
|
} else {
|
|
p.data = nullptr;
|
|
}
|
|
out->planes[i] = p;
|
|
}
|
|
return out;
|
|
}
|
|
}
|
|
|
|
auto buf = std::make_shared<std::vector<uint8_t>>(src->data_size);
|
|
memcpy(buf->data(), src->data, src->data_size);
|
|
|
|
auto out = std::make_shared<Frame>(*src);
|
|
out->dma_fd = -1;
|
|
out->data = buf->data();
|
|
out->data_size = buf->size();
|
|
out->data_owner = buf;
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
FramePlane p = src->planes[i];
|
|
int offset = p.offset;
|
|
if (p.data && src->data) {
|
|
const ptrdiff_t d = p.data - src->data;
|
|
if (d >= 0 && static_cast<size_t>(d) < src->data_size) {
|
|
offset = static_cast<int>(d);
|
|
}
|
|
}
|
|
p.offset = offset;
|
|
if (offset >= 0 && static_cast<size_t>(offset) < out->data_size) {
|
|
p.data = out->data + offset;
|
|
} else {
|
|
p.data = nullptr;
|
|
}
|
|
out->planes[i] = p;
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
void PushToDownstream(FramePtr frame) {
|
|
for (auto& q : output_queues_) {
|
|
q->Push(frame);
|
|
}
|
|
}
|
|
|
|
const char* GetLabel(int cls_id) const {
|
|
if (!labels_.empty()) {
|
|
if (cls_id >= 0 && cls_id < static_cast<int>(labels_.size())) {
|
|
return labels_[cls_id].c_str();
|
|
}
|
|
return "unknown";
|
|
}
|
|
return GetClassName(cls_id);
|
|
}
|
|
|
|
void DrawDetections(FramePtr frame) {
|
|
if (!frame->det || frame->det->items.empty()) return;
|
|
|
|
int w = frame->width;
|
|
int h = frame->height;
|
|
uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
|
|
PixelFormat fmt = frame->format;
|
|
|
|
const bool want_rga_bbox = use_rga_bbox_ && draw_bbox_;
|
|
bool rga_bbox_ok = false;
|
|
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
if (want_rga_bbox) {
|
|
if (frame->dma_fd < 0) {
|
|
LogRgaBboxErrorThrottled("frame has no dma_fd");
|
|
} else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
|
|
fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
|
|
LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
|
|
} else {
|
|
rga_buffer_t dst{};
|
|
int rga_stride_bytes = 0;
|
|
if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
|
|
LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
|
|
} else {
|
|
(void)rga_stride_bytes;
|
|
im_job_handle_t job = imbeginJob();
|
|
if (!job) {
|
|
LogRgaBboxErrorThrottled("imbeginJob failed");
|
|
} else {
|
|
bool ok = true;
|
|
for (const auto& det : frame->det->items) {
|
|
int x = Clamp(static_cast<int>(det.bbox.x), 0, w - 1);
|
|
int y = Clamp(static_cast<int>(det.bbox.y), 0, h - 1);
|
|
int rw = static_cast<int>(det.bbox.w);
|
|
int rh = static_cast<int>(det.bbox.h);
|
|
rw = Clamp(rw, 1, w - x);
|
|
rh = Clamp(rh, 1, h - y);
|
|
im_rect rect{x, y, rw, rh};
|
|
const uint32_t c = PackColorArgb(GetClassColor(det.cls_id));
|
|
if (imrectangleTask(job, dst, rect, c, line_width_) <= 0) {
|
|
ok = false;
|
|
break;
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
|
|
} else {
|
|
(void)imendJob(job, IM_SYNC, 0, nullptr);
|
|
}
|
|
rga_bbox_ok = ok;
|
|
if (!rga_bbox_ok) {
|
|
LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
if (want_rga_bbox) {
|
|
LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
|
|
}
|
|
#endif
|
|
|
|
int stride;
|
|
if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
|
|
} else {
|
|
return; // Unsupported format
|
|
}
|
|
|
|
const bool do_cpu_bbox = draw_bbox_ && !use_rga_bbox_;
|
|
|
|
const bool do_cpu_text = draw_text_;
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncStartFd(frame->dma_fd);
|
|
}
|
|
|
|
for (const auto& det : frame->det->items) {
|
|
int x1 = static_cast<int>(det.bbox.x);
|
|
int y1 = static_cast<int>(det.bbox.y);
|
|
int x2 = static_cast<int>(det.bbox.x + det.bbox.w);
|
|
int y2 = static_cast<int>(det.bbox.y + det.bbox.h);
|
|
|
|
Color color = GetClassColor(det.cls_id);
|
|
|
|
if (do_cpu_bbox) {
|
|
DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
|
|
}
|
|
|
|
if (do_cpu_text) {
|
|
char label[96];
|
|
if (det.track_id >= 0) {
|
|
snprintf(label, sizeof(label), "%s %.0f%% id=%d", GetLabel(det.cls_id), det.score * 100, det.track_id);
|
|
} else {
|
|
snprintf(label, sizeof(label), "%s %.0f%%", GetLabel(det.cls_id), det.score * 100);
|
|
}
|
|
int text_y = y1 - 8 * font_scale_;
|
|
if (text_y < 0) text_y = y1 + 2;
|
|
DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
|
|
}
|
|
}
|
|
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncEndFd(frame->dma_fd);
|
|
}
|
|
}
|
|
|
|
void DrawFaceRecog(FramePtr frame) {
|
|
if (!frame->face_recog || frame->face_recog->items.empty()) return;
|
|
|
|
int w = frame->width;
|
|
int h = frame->height;
|
|
uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
|
|
PixelFormat fmt = frame->format;
|
|
|
|
const bool want_rga_bbox = use_rga_bbox_ && draw_bbox_;
|
|
bool rga_bbox_ok = false;
|
|
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
if (want_rga_bbox) {
|
|
if (frame->dma_fd < 0) {
|
|
LogRgaBboxErrorThrottled("frame has no dma_fd");
|
|
} else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
|
|
fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
|
|
LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
|
|
} else {
|
|
rga_buffer_t dst{};
|
|
int rga_stride_bytes = 0;
|
|
if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
|
|
LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
|
|
} else {
|
|
(void)rga_stride_bytes;
|
|
im_job_handle_t job = imbeginJob();
|
|
if (!job) {
|
|
LogRgaBboxErrorThrottled("imbeginJob failed");
|
|
} else {
|
|
bool ok = true;
|
|
for (const auto& it : frame->face_recog->items) {
|
|
int x = Clamp(static_cast<int>(it.bbox.x), 0, w - 1);
|
|
int y = Clamp(static_cast<int>(it.bbox.y), 0, h - 1);
|
|
int rw = static_cast<int>(it.bbox.w);
|
|
int rh = static_cast<int>(it.bbox.h);
|
|
rw = Clamp(rw, 1, w - x);
|
|
rh = Clamp(rh, 1, h - y);
|
|
im_rect rect{x, y, rw, rh};
|
|
const Color color = it.unknown ? Color{255, 0, 0} : Color{0, 255, 0};
|
|
if (imrectangleTask(job, dst, rect, PackColorArgb(color), line_width_) <= 0) {
|
|
ok = false;
|
|
break;
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
|
|
} else {
|
|
(void)imendJob(job, IM_SYNC, 0, nullptr);
|
|
}
|
|
rga_bbox_ok = ok;
|
|
if (!rga_bbox_ok) {
|
|
LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
if (want_rga_bbox) {
|
|
LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
|
|
}
|
|
#endif
|
|
|
|
int stride;
|
|
if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
const bool do_cpu_bbox = draw_bbox_ && !use_rga_bbox_;
|
|
const bool do_cpu_text = draw_text_;
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncStartFd(frame->dma_fd);
|
|
}
|
|
|
|
for (const auto& it : frame->face_recog->items) {
|
|
int x1 = static_cast<int>(it.bbox.x);
|
|
int y1 = static_cast<int>(it.bbox.y);
|
|
int x2 = static_cast<int>(it.bbox.x + it.bbox.w);
|
|
int y2 = static_cast<int>(it.bbox.y + it.bbox.h);
|
|
|
|
const Color color = it.unknown ? Color{255, 0, 0} : Color{0, 255, 0};
|
|
|
|
if (do_cpu_bbox) {
|
|
DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
|
|
}
|
|
|
|
if (do_cpu_text) {
|
|
char label[96];
|
|
const char* name = it.best_name.empty() ? (it.unknown ? "unknown" : "") : it.best_name.c_str();
|
|
snprintf(label, sizeof(label), "%s %.2f", name, it.best_sim);
|
|
int text_y = y1 - 8 * font_scale_;
|
|
if (text_y < 0) text_y = y1 + 2;
|
|
DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
|
|
}
|
|
}
|
|
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncEndFd(frame->dma_fd);
|
|
}
|
|
}
|
|
|
|
void DrawFaceDet(FramePtr frame) {
|
|
if (!frame->face_det || frame->face_det->faces.empty()) return;
|
|
|
|
int w = frame->width;
|
|
int h = frame->height;
|
|
uint8_t* data = frame->planes[0].data ? frame->planes[0].data : frame->data;
|
|
PixelFormat fmt = frame->format;
|
|
|
|
const bool want_rga_bbox = use_rga_bbox_ && draw_bbox_;
|
|
bool rga_bbox_ok = false;
|
|
|
|
#if defined(RK3588_ENABLE_RGA)
|
|
if (want_rga_bbox) {
|
|
if (frame->dma_fd < 0) {
|
|
LogRgaBboxErrorThrottled("frame has no dma_fd");
|
|
} else if (!(fmt == PixelFormat::RGB || fmt == PixelFormat::BGR ||
|
|
fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420)) {
|
|
LogRgaBboxErrorThrottled("unsupported format=" + std::to_string(static_cast<int>(fmt)));
|
|
} else {
|
|
rga_buffer_t dst{};
|
|
int rga_stride_bytes = 0;
|
|
if (!WrapFrameAsRgaDst(*frame, dst, rga_stride_bytes)) {
|
|
LogRgaBboxErrorThrottled("WrapFrameAsRgaDst failed");
|
|
} else {
|
|
(void)rga_stride_bytes;
|
|
im_job_handle_t job = imbeginJob();
|
|
if (!job) {
|
|
LogRgaBboxErrorThrottled("imbeginJob failed");
|
|
} else {
|
|
bool ok = true;
|
|
const Color color{0, 255, 255};
|
|
for (const auto& it : frame->face_det->faces) {
|
|
int x = Clamp(static_cast<int>(it.bbox.x), 0, w - 1);
|
|
int y = Clamp(static_cast<int>(it.bbox.y), 0, h - 1);
|
|
int rw = static_cast<int>(it.bbox.w);
|
|
int rh = static_cast<int>(it.bbox.h);
|
|
rw = Clamp(rw, 1, w - x);
|
|
rh = Clamp(rh, 1, h - y);
|
|
im_rect rect{x, y, rw, rh};
|
|
if (imrectangleTask(job, dst, rect, PackColorArgb(color), line_width_) <= 0) {
|
|
ok = false;
|
|
break;
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = (imendJob(job, IM_SYNC, 0, nullptr) > 0);
|
|
} else {
|
|
(void)imendJob(job, IM_SYNC, 0, nullptr);
|
|
}
|
|
rga_bbox_ok = ok;
|
|
if (!rga_bbox_ok) {
|
|
LogRgaBboxErrorThrottled("imrectangle/imendJob failed");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
if (want_rga_bbox) {
|
|
LogRgaBboxErrorThrottled("RK3588_ENABLE_RGA is disabled at build");
|
|
}
|
|
#endif
|
|
|
|
int stride;
|
|
if (fmt == PixelFormat::RGB || fmt == PixelFormat::BGR) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w * 3);
|
|
} else if (fmt == PixelFormat::NV12 || fmt == PixelFormat::YUV420) {
|
|
stride = frame->planes[0].stride > 0 ? frame->planes[0].stride : (frame->stride > 0 ? frame->stride : w);
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
const bool do_cpu_bbox = draw_bbox_ && !use_rga_bbox_;
|
|
const bool do_cpu_text = draw_text_;
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncStartFd(frame->dma_fd);
|
|
}
|
|
|
|
const Color color{0, 255, 255};
|
|
for (const auto& it : frame->face_det->faces) {
|
|
int x1 = static_cast<int>(it.bbox.x);
|
|
int y1 = static_cast<int>(it.bbox.y);
|
|
int x2 = static_cast<int>(it.bbox.x + it.bbox.w);
|
|
int y2 = static_cast<int>(it.bbox.y + it.bbox.h);
|
|
|
|
if (do_cpu_bbox) {
|
|
DrawRect(data, w, h, stride, fmt, x1, y1, x2, y2, line_width_, color);
|
|
}
|
|
if (do_cpu_text) {
|
|
char label[64];
|
|
snprintf(label, sizeof(label), "face %.0f%%", it.score * 100);
|
|
int text_y = y1 - 8 * font_scale_;
|
|
if (text_y < 0) text_y = y1 + 2;
|
|
DrawText(data, w, h, stride, fmt, x1, text_y, label, font_scale_, color);
|
|
}
|
|
}
|
|
|
|
if ((do_cpu_bbox || do_cpu_text) && frame->dma_fd >= 0) {
|
|
DmaSyncEndFd(frame->dma_fd);
|
|
}
|
|
}
|
|
|
|
std::string id_;
|
|
bool draw_bbox_ = true;
|
|
bool draw_text_ = true;
|
|
bool draw_face_det_ = true;
|
|
bool use_rga_bbox_ = true;
|
|
int line_width_ = 2;
|
|
int font_scale_ = 1;
|
|
std::vector<std::string> labels_;
|
|
|
|
void LogRgaBboxErrorThrottled(const std::string& reason) {
|
|
const auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::steady_clock::now().time_since_epoch())
|
|
.count();
|
|
if (last_rga_bbox_err_ms_ > 0 &&
|
|
now_ms - static_cast<int64_t>(last_rga_bbox_err_ms_) < 1000) {
|
|
return;
|
|
}
|
|
last_rga_bbox_err_ms_ = static_cast<uint64_t>(now_ms);
|
|
LogError("[osd] RGA bbox failed: " + reason + " id=" + id_);
|
|
}
|
|
|
|
uint64_t last_rga_bbox_err_ms_ = 0;
|
|
|
|
std::shared_ptr<SpscQueue<FramePtr>> input_queue_;
|
|
std::vector<std::shared_ptr<SpscQueue<FramePtr>>> output_queues_;
|
|
uint64_t processed_ = 0;
|
|
|
|
bool stats_log_ = false;
|
|
uint64_t stats_interval_ = 100;
|
|
};
|
|
|
|
REGISTER_NODE(OsdNode, "osd");
|
|
|
|
} // namespace rk3588
|