spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

替换 Mild v12 模型 + 同事编码器刹车红绿灯状态机合并

Browse Source
This commit is contained in:
spdis
2026-06-13 13:05:18 +08:00
parent 457d608621
commit b9e32c5a40
66 changed files with 1698 additions and 5625 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/camera.cpp
+171 -7
View File
@@ -22,7 +22,8 @@ PwmController servo(1, 0);
// ── 模型检测参数 ──
#define ZEBRA_CLASS 3
static float g_thresh[4] = {0.80f, 0.80f, 0.80f, 0.75f};
#define CONE_CLASS 0
static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f};
// ── 斑马线去抖 ──
#define ZEBRA_MIN_FRAMES 5
@@ -36,6 +37,23 @@ static ZState g_zstate = Z_NORMAL;
static time_t g_ztime = 0;
static bool g_zebra_ever = false;
// ── 红绿灯状态机 ──
#define TL_RED_CLASS 1
#define TL_GREEN_CLASS 2
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
static TLState g_tl_state = TL_NORMAL;
static int g_tl_red_frames = 0;
static int g_tl_green_frames = 0;
// ── 锥桶检测 ──
static int g_cone_frames = 0;
static int g_cone_last_row = -1;
static int g_cone_last_col = -1;
static bool g_cone_confirmed = false;
static int g_cone_hold_ctr = 0;
static int g_cone_hold_src_row = -1;
static int g_cone_hold_src_col = -1;
// ── 模型检测结果缓存 ──
static DetectBoxV10 g_boxes[16];
static int g_box_count = 0;
@@ -110,10 +128,10 @@ int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
line_tracking_width = newWidth / calc_scale;
line_tracking_height = newHeight / calc_scale;
if (!model_v10_init("./nanodetv10.bin")) {
if (!model_v10_init("./mild_v12.bin")) {
printf("[MODEL] 警告: 模型加载失败\n");
} else {
printf("[MODEL] v10 模型已加载\n");
printf("[MODEL] Mild v12 模型已加载\n");
}
i2c_audio_open();
@@ -283,6 +301,138 @@ int CameraHandler(void)
break;
}
// ── 5.5 红绿灯状态机 ──
bool red_seen = false;
bool green_seen = false;
for (int i = 0; i < g_box_count; ++i) {
if (g_boxes[i].cls == TL_RED_CLASS) red_seen = true;
if (g_boxes[i].cls == TL_GREEN_CLASS) green_seen = true;
}
if (red_seen) g_tl_red_frames++;
else g_tl_red_frames = std::max(0, g_tl_red_frames - 1);
if (green_seen) g_tl_green_frames++;
else g_tl_green_frames = std::max(0, g_tl_green_frames - 1);
switch (g_tl_state) {
case TL_NORMAL:
if (g_tl_red_frames >= 3) {
g_tl_state = TL_STOP;
if (g_cfg.debug) printf("[TL] 红灯停车\n");
}
break;
case TL_STOP:
if (!red_seen) {
g_tl_state = TL_WAIT_GREEN;
if (g_cfg.debug) printf("[TL] 等待绿灯\n");
}
break;
case TL_WAIT_GREEN:
if (g_tl_green_frames >= 3) {
g_tl_state = TL_NORMAL;
g_tl_red_frames = 0;
g_tl_green_frames = 0;
if (g_cfg.debug) printf("[TL] 绿灯通行\n");
}
break;
}
bool tl_block = (g_tl_state != TL_NORMAL);
// ── 5.6 锥桶检测 + 中线变形 ──
bool cone_seen = false;
int cone_row_keep = -1;
int cone_col_keep = -1;
if (g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
for (int i = 0; i < g_box_count; ++i) {
if (g_boxes[i].cls != CONE_CLASS) continue;
if (g_boxes[i].conf < g_cfg.cone_thresh) continue;
int col_lt = g_boxes[i].cx * line_tracking_width / raw_frame.cols;
int row_lt = g_boxes[i].cy * line_tracking_height / raw_frame.rows;
if (row_lt < 10 || row_lt >= line_tracking_height) continue;
if (left_line[row_lt] == -1 || right_line[row_lt] == -1) continue;
if (g_cfg.cone_margin) {
int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
if (br < left_line[row_lt] || bl > right_line[row_lt]) continue;
} else {
if (col_lt < left_line[row_lt] || col_lt > right_line[row_lt]) continue;
}
if (row_lt > cone_row_keep) { cone_row_keep = row_lt; cone_col_keep = col_lt; }
cone_seen = true;
}
}
{
int row_tol = std::max(2, line_tracking_height / 12);
int col_tol = std::max(3, line_tracking_width / 8);
if (cone_seen) {
if (g_cone_frames == 0) {
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
g_cone_frames = 1;
} else {
if (std::abs(cone_row_keep - g_cone_last_row) <= row_tol &&
std::abs(cone_col_keep - g_cone_last_col) <= col_tol) {
g_cone_frames++;
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
} else {
g_cone_frames = 1;
g_cone_last_row = cone_row_keep;
g_cone_last_col = cone_col_keep;
}
}
} else {
if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1);
}
}
g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames);
if (g_cone_confirmed) {
g_cone_hold_ctr = 0;
g_cone_hold_src_row = cone_row_keep;
g_cone_hold_src_col = cone_col_keep;
} else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
g_cone_hold_ctr++;
}
bool cone_active = g_cone_confirmed ||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
if (cone_active && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
int src_row = g_cone_confirmed ? cone_row_keep : g_cone_hold_src_row;
int src_col = g_cone_confirmed ? cone_col_keep : g_cone_hold_src_col;
int start_row = 10;
if (src_row > start_row) {
double total_rows = src_row - start_row;
double rng = (double)g_cfg.cone_avoid_range;
double half_w = line_tracking_width / 2.0;
double mid_at_cone = mid_line[src_row];
double dir = (src_col < mid_at_cone) ? 1.0 : -1.0;
double decay = g_cone_confirmed
? 1.0
: (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);
for (int row = start_row; row <= src_row; row++) {
double t_raw = (row - start_row) / total_rows;
double t = std::clamp(t_raw * (total_rows / rng), 0.0, 1.0);
double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
double new_mid = mid_line[row] + push;
new_mid = std::clamp(new_mid,
(double)left_line[row] + 2.0,
(double)right_line[row] - 2.0);
mid_line[row] = (int)(new_mid + 0.5);
}
}
}
// ── 6. 舵机 ──
if (g_cfg.start) {
int foresee = (int)g_cfg.foresee;
@@ -304,7 +454,15 @@ int CameraHandler(void)
}
// ── 7. 电机控制 ──
ControlUpdate(target_speed, g_zstate == Z_STOP);
{
double spd = target_speed;
bool cone_slow = g_cone_confirmed ||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
if (cone_slow && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) {
spd *= g_cfg.cone_speed;
}
ControlUpdate(spd, g_zstate == Z_STOP || tl_block);
}
clock_gettime(CLOCK_MONOTONIC,&t4);
// ── 8. LCD ──
@@ -340,14 +498,20 @@ int CameraHandler(void)
x2=std::max(0,std::min(newWidth-1,x2)); y2=std::max(0,std::min(newHeight-1,y2));
cv::Scalar color(0,255,0);
if (g_boxes[i].cls == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
else if (g_boxes[i].cls == CONE_CLASS) color=cv::Scalar(0,165,255);
cv::rectangle(lcd_fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
char lab[16]; std::snprintf(lab,16,"%d %.0f",g_boxes[i].cls,g_boxes[i].conf*100);
cv::putText(lcd_fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
}
const char* ztxt="N";
if (g_zstate==Z_STOP) ztxt="S";
else if (g_zstate==Z_COOLDOWN) ztxt="C";
char ztxt[8];
int zidx = 0;
if (g_tl_state == TL_STOP) ztxt[zidx++] = 'R';
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zidx++] = 'G';
if (g_zstate == Z_STOP) ztxt[zidx++] = 'S';
else if (g_zstate == Z_COOLDOWN) ztxt[zidx++] = 'C';
else ztxt[zidx++] = 'N';
ztxt[zidx] = '\0';
cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
src/control.cpp
+79 -6
View File
@@ -1,23 +1,70 @@
/*
* control — 电机控制层
*
* 开环占空比控制：根据目标速度和偏差计算占空比，直接输出到 PWM。
* 无编码器反馈，无 PID 闭环。
* 开环占空比控制 + 斑马线编码器比例刹车。
* 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
*/
#include "control.h"
#include "global.h"
#include <thread>
#include <atomic>
#include <cmath>
extern double g_steer_deviation;
MotorController *motorController[2] = {nullptr, nullptr};
GPIO mortorEN(73);
static GPIO encoderLSB(67);
static GPIO encoderDIR(72);
static std::thread g_enc_thread;
static std::atomic<bool> g_enc_running{false};
static std::atomic<double> g_enc_speed{0.0};
static std::atomic<int> g_enc_dir{0};
static long enc_now_ns() {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return ts.tv_sec * 1000000000L + ts.tv_nsec;
}
static void encoder_thread() {
int prev_lsb = encoderLSB.readValue() ? 1 : 0;
int pulse_cnt = 0;
long t0 = enc_now_ns();
while (g_enc_running.load()) {
int cur_lsb = encoderLSB.readValue() ? 1 : 0;
int cur_dir = encoderDIR.readValue() ? 1 : 0;
if (cur_lsb != prev_lsb) {
pulse_cnt++;
prev_lsb = cur_lsb;
}
g_enc_dir.store(cur_dir);
long t1 = enc_now_ns();
long dt = t1 - t0;
if (dt >= 100000000L) {
g_enc_speed.store(pulse_cnt / (dt / 1e9));
pulse_cnt = 0;
t0 = t1;
}
}
}
void ControlInit()
{
mortorEN.setDirection("out");
mortorEN.setValue(1);
// 左电机方向: GPIO12 (右电机方向由 MotorController 自管, 但左In2也一样要设)
encoderLSB.setDirection("in");
encoderDIR.setDirection("in");
g_enc_running.store(true);
g_enc_thread = std::thread(encoder_thread);
GPIO leftIn2(13);
leftIn2.setDirection("out");
leftIn2.setValue(1);
@@ -37,15 +84,38 @@ void ControlInit()
void ControlUpdate(double speed, bool zebra_block)
{
if (zebra_block || !g_cfg.start)
if (!g_cfg.start)
{
for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0);
if (!g_cfg.start) mortorEN.setValue(0);
mortorEN.setValue(0);
return;
}
if (zebra_block)
{
double cur_speed = g_enc_speed.load();
int cur_dir = g_enc_dir.load();
if (cur_speed > 0.5)
{
int brake_ns = (int)(cur_speed * g_cfg.brake_scale);
if (brake_ns > g_cfg.brake_max) brake_ns = g_cfg.brake_max;
double brake_pct = brake_ns / 500.0; // ns → % (period=50000ns)
int dir = cur_dir ? 1 : 0;
for (int i = 0; i < 2; ++i)
if (motorController[i])
motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
}
else
{
for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0);
}
return;
}
// 弯道降速: 偏差越大，速度越低 (最低 60%)
double curve = 1.0 - std::abs(g_steer_deviation) * 0.4;
if (curve < 0.6) curve = 0.6;
double spd = speed * curve;
@@ -58,6 +128,9 @@ void ControlUpdate(double speed, bool zebra_block)
void ControlExit()
{
g_enc_running.store(false);
if (g_enc_thread.joinable()) g_enc_thread.join();
for (int i = 0; i < 2; ++i)
{
delete motorController[i];
src/global.cpp
+8
View File
@@ -31,4 +31,12 @@ void cfg_load_all()
g_cfg.start = readFlag(start_file);
g_cfg.showImg = readFlag(showImg_file);
g_cfg.debug = readFlag(debug_file);
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
}
src/model_v10.cpp
+396 -496
View File
@@ -1,602 +1,502 @@
/*
* NanoDetHeat v10: 3.2M MACs, 4-class
* 优化: BN折叠 + conv_bn_relu融合 + 快速exp + 边界修正
* YOLO_Mega_Mild_r2 C++ inference engine v3
* Architecture: 3-9-9-13-13-19-19-19-44-44-64-64 -> head(64-64,dw) -> out(64-9)
*/
#include "model_v10.hpp"
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <ctime>
#include <new>
#include <algorithm>
#include <unordered_map>
#include <string>
#include <vector>
// ============================================================
// 快速 exp — Schraudolph 方法 (IEEE 754 整数近似)
// ============================================================
static inline float fast_exp(float x) {
// clamp to avoid overflow: exp(88) fits in float
x = std::min(x, 88.0f);
x = std::max(x, -87.0f);
// exp(x) ~ 2^(x * log2(e))
// float 按位: 2^23 * (127 + x*log2(e))
union { float f; int i; } u;
u.i = (int)(12102203.0f * x) + 1064866805; // 2^23 * log2(e) = 12102203
return u.f;
}
static inline float fast_sigmoid(float x) {
return 1.0f / (1.0f + fast_exp(-x));
static inline float clamp_f(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
static inline float sigmoid_f(float x) {
x = clamp_f(x, -30.0f, 30.0f);
return 1.0f / (1.0f + std::exp(-x));
}
static inline float relu_maybe(float s, bool relu) { return (relu && s < 0.0f) ? 0.0f : s; }
// ============================================================
// 权重 (新增 BN alpha/beta 预计算)
// ============================================================
struct WT { char n[128]; int nd; int s[4]; float* d; };
static int gn=0; static WT* gw=nullptr;
static int gn = 0;
static WT* gw = nullptr;
static float* wf(const char* k) {
for(int i=0;i<gn;++i) if(!std::strcmp(gw[i].n,k)) return gw[i].d;
static WT* wt(const char* k) {
for (int i = 0; i < gn; ++i)
if (!std::strcmp(gw[i].n, k)) return &gw[i];
return nullptr;
}
static float* wf(const char* k) { WT* t = wt(k); return t ? t->d : nullptr; }
static void init_lut();
// 加载后扫描 BN 参数，预计算 alpha/beta 存入权重表
static void bn_precompute() {
// 扫描所有权重，找 BN 三元组 (weight/bias/running_mean/running_var)
std::unordered_map<std::string, int> base_idx;
for (int i=0; i<gn; ++i) {
std::unordered_map<std::string, int> bn_weight_idx;
for (int i = 0; i < gn; ++i) {
const char* name = gw[i].n;
// 匹配 xxx.weight (BN) 或 xxx.bias (BN) → base = "xxx"
const char* dot = std::strrchr(name, '.');
if (!dot) continue;
std::string base(name, dot - name);
int sz = gw[i].s[0];
if (std::strcmp(dot+1, "weight")==0 && sz>0 && sz <= 256) {
base_idx[base] = i;
}
if (!dot || std::strcmp(dot + 1, "weight") != 0) continue;
if (gw[i].nd != 1) continue;
bn_weight_idx[std::string(name, dot - name)] = i;
}
std::vector<WT> new_wts;
for (auto& [base, wi] : base_idx) {
// 找对应的 bias / running_mean / running_var
std::string w_key = base + ".weight";
std::string b_key = base + ".bias";
std::string m_key = base + ".running_mean";
std::string v_key = base + ".running_var";
float* w = wf(w_key.c_str());
float* b = wf(b_key.c_str());
float* m = wf(m_key.c_str());
float* v = wf(v_key.c_str());
if (!w || !m || !v) continue; // cls_head/size_head 没有 BN
int C = gw[wi].s[0];
for (auto& kv : bn_weight_idx) {
const std::string& base = kv.first;
float* w = gw[kv.second].d;
float* b = wf((base + ".bias").c_str());
float* m = wf((base + ".running_mean").c_str());
float* v = wf((base + ".running_var").c_str());
if (!m || !v) continue;
const int C = gw[kv.second].s[0];
float* alpha = new float[C];
float* beta = new float[C];
const float eps = 1e-5f;
for (int c=0; c<C; ++c) {
for (int c = 0; c < C; ++c) {
alpha[c] = w[c] / std::sqrt(v[c] + eps);
beta[c] = (b ? b[c] : 0.0f) - m[c] * alpha[c];
}
// 存为新的权重条目
new_wts.push_back({});
std::snprintf(new_wts.back().n, 128, "%s._alpha", base.c_str());
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
new_wts.back().d = alpha;
new_wts.push_back({});
std::snprintf(new_wts.back().n, 128, "%s._beta", base.c_str());
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
new_wts.back().d = beta;
WT ta = {}; std::snprintf(ta.n, 128, "%s._alpha", base.c_str());
ta.nd = 1; ta.s[0] = C; ta.s[1] = ta.s[2] = ta.s[3] = 1; ta.d = alpha;
WT tb = {}; std::snprintf(tb.n, 128, "%s._beta", base.c_str());
tb.nd = 1; tb.s[0] = C; tb.s[1] = tb.s[2] = tb.s[3] = 1; tb.d = beta;
new_wts.push_back(ta); new_wts.push_back(tb);
}
// 重新分配权重数组 (旧数组 + 新条目)
WT* old_gw = gw;
int old_gn = gn;
int new_gn = old_gn + (int)new_wts.size();
gw = new WT[new_gn];
std::memcpy(gw, old_gw, old_gn * sizeof(WT));
for (size_t i=0; i<new_wts.size(); ++i) {
gw[old_gn + i] = new_wts[i];
}
gn = new_gn;
delete[] old_gw;
init_lut();
WT* old = gw;
const int old_n = gn, new_n = old_n + (int)new_wts.size();
gw = new WT[new_n];
std::memcpy(gw, old, old_n * sizeof(WT));
for (size_t i = 0; i < new_wts.size(); ++i) gw[old_n + i] = new_wts[i];
gn = new_n;
delete[] old;
}
// ============================================================
// 内存
// ============================================================
struct M10 {
float preproc[3*120*160]; // 预处理 buffer (静态, 避免每帧 new/delete)
float stem[6*60*80];
float b1[8*60*80];
float b2[12*30*40];
float b3[16*15*20];
float b4[32*15*20];
float b5[48*15*20];
float b6[48*15*20];
float sh[24*15*20];
float cl[5*15*20];
float sz[2*15*20];
float dw_out[8*60*80];
float se_buf[80];
uint8 peak_mask[15*20];
};
static M10* m=nullptr;
static constexpr int IN_W = 160, IN_H = 120;
static constexpr int BUF_FLOATS = 9 * 60 * 80;
static float* g_preproc = nullptr;
static float* g_A = nullptr;
static float* g_B = nullptr;
static float* g_T = nullptr;
static float* g_out = nullptr;
static uint8_t g_resized[IN_W * IN_H * 3];
static float g_se[160];
static float g_prob[5 * 15 * 20];
static float g_lut[256];
static char g_dump_dir[256] = {0};
// ============================================================
// Relu
// ============================================================
static void relu_f(float* x, int N) {
for (int i=0; i<N; ++i) if (x[i]<0) x[i]=0;
static void dump_tensor(const char* name, const float* d, int count) {
if (!g_dump_dir[0]) return;
char path[320];
std::snprintf(path, sizeof(path), "%s/%s.f32", g_dump_dir, name);
FILE* f = std::fopen(path, "wb");
if (f) { std::fwrite(d, sizeof(float), count, f); std::fclose(f); }
}
// ============================================================
// 融合: conv + BN + ReLU (使用预计算的 alpha/beta)
// ============================================================
static void conv_bn_relu(float* o, const float* in,
const float* conv_w, const float* conv_b,
const float* bn_alpha, const float* bn_beta,
int H, int W, int iC, int oC, int K, int str, int grp, bool use_relu)
static void conv1x1(float* o, const float* in, const float* cw, const float* cb,
const float* ba, const float* bb,
int H, int W, int iC, int oC, bool relu)
{
if (!conv_w || !in || !o) return;
int Ho=H/str, Wo=W/str, pad=K/2;
int icpg=iC/grp, ocpg=oC/grp;
const int N = H * W;
for (int oc = 0; oc < oC; ++oc) {
float* oo = o + oc * N;
const float* ww = cw + oc * iC;
const float bias = cb ? cb[oc] : 0.0f;
for (int i = 0; i < N; ++i) oo[i] = bias;
for (int ic = 0; ic < iC; ++ic) {
const float w = ww[ic];
const float* ii = in + ic * N;
for (int i = 0; i < N; ++i) oo[i] += w * ii[i];
}
const float a = ba ? ba[oc] : 1.0f;
const float b = bb ? bb[oc] : 0.0f;
for (int i = 0; i < N; ++i)
oo[i] = relu_maybe(oo[i] * a + b, relu);
}
}
if (K==1 && str==1) {
// 1×1 fused: 指针递增避免 y*W+x
for (int g=0; g<grp; ++g) {
for (int oc=0; oc<ocpg; ++oc) {
int occ = g*ocpg + oc;
float* oo = o + occ*H*W;
float cb = conv_b ? conv_b[occ] : 0.0f;
const float* ww = conv_w + occ*icpg;
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
static void dwconv3x3(float* o, const float* in, const float* cw,
const float* ba, const float* bb,
int H, int W, int C, bool relu)
{
for (int c = 0; c < C; ++c) {
const float* ii = in + c * H * W;
float* oo = o + c * H * W;
const float* ww = cw + c * 9;
const float w0=ww[0],w1=ww[1],w2=ww[2],w3=ww[3],w4=ww[4],
w5=ww[5],w6=ww[6],w7=ww[7],w8=ww[8];
const float a = ba ? ba[c] : 1.0f;
const float b = bb ? bb[c] : 0.0f;
for (int y=0; y<H; ++y) {
float* oy = oo + y*W;
const float* iy = in + y*W;
for (int x=0; x<W; ++x) {
float s = cb;
const float* px = iy + x;
for (int ic=0; ic<icpg; ++ic) {
int icc = g*icpg + ic;
s += ww[ic] * px[icc*H*W];
}
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oy[x] = s;
}
}
for (int y = 1; y < H - 1; ++y) {
const float *i0 = ii + (y - 1) * W, *i1 = i0 + W, *i2 = i1 + W;
float* oy = oo + y * W;
for (int x = 1; x < W - 1; ++x) {
float s = w0*i0[x-1] + w1*i0[x] + w2*i0[x+1]
+ w3*i1[x-1] + w4*i1[x] + w5*i1[x+1]
+ w6*i2[x-1] + w7*i2[x] + w8*i2[x+1];
oy[x] = relu_maybe(s * a + b, relu);
}
}
return;
}
if (grp==iC && K==3 && str==1) {
// 3×3 dw fused + interior unrolled / border with checks
for (int c=0; c<iC; ++c) {
float* const oo = o + c*H*W;
const float* const ii = in + c*H*W;
const float* const ww = conv_w + c*9;
float cb = conv_b ? conv_b[c] : 0.0f;
float ba=bn_alpha?bn_alpha[c]:1.0f, bb=bn_beta?bn_beta[c]:0.0f;
float w0=ww[0], w1=ww[1], w2=ww[2], w3=ww[3], w4=ww[4], w5=ww[5], w6=ww[6], w7=ww[7], w8=ww[8];
if (H>=3 && W>=3) {
// interior: 1≤y<H-1, 1≤x<W-1 (no boundary checks)
for (int y=1; y<H-1; ++y) {
float* const oy = oo + y*W + 1;
const float* const i0 = ii + (y-1)*W;
const float* const i1 = i0 + W;
const float* const i2 = i1 + W;
for (int x=1; x<W-1; ++x) {
float s = cb
+ w0*i0[x-1] + w1*i0[x] + w2*i0[x+1]
+ w3*i1[x-1] + w4*i1[x] + w5*i1[x+1]
+ w6*i2[x-1] + w7*i2[x] + w8*i2[x+1];
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oy[x-1] = s;
}
}
// top row (y=0): boundary check — ky=-1 clamps to y=0
{
float* const oy = oo;
const float* const i0 = ii; // y=0 (ky=-1 and ky=0)
const float* const i1 = ii + W; // y=1 (ky=1)
for (int x=0; x<W; ++x) {
float s = cb;
// ky=-1: use i0 (clamped), ky=0: use i0, ky=1: use i1
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
+ w3*i0[xl] + w4*i0[x] + w5*i0[xr] // ky=0
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oy[x] = s;
}
}
// bottom row (y=H-1): boundary check — ky=1 clamps to y=H-1
{
float* const oy = oo + (H-1)*W;
const float* const i0 = ii + (H-2)*W; // y=H-2 (ky=-1)
const float* const i1 = ii + (H-1)*W; // y=H-1 (ky=0 and ky=1)
for (int x=0; x<W; ++x) {
float s = cb;
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
+ w3*i1[xl] + w4*i1[x] + w5*i1[xr] // ky=0
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oy[x] = s;
}
}
// middle rows left border (1≤y<H-1, x=0)
for (int y=1; y<H-1; ++y) {
const float* const i0 = ii + (y-1)*W;
const float* const i1 = i0 + W;
const float* const i2 = i1 + W;
float s = cb
+ w1*i0[0] + w2*i0[1]
+ w4*i1[0] + w5*i1[1]
+ w7*i2[0] + w8*i2[1];
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oo[y*W] = s;
}
// middle rows right border (1≤y<H-1, x=W-1)
for (int y=1; y<H-1; ++y) {
const float* const i0 = ii + (y-1)*W + (W-2);
const float* const i1 = i0 + W;
const float* const i2 = i1 + W;
float s = cb
+ w0*i0[0] + w1*i0[1]
+ w3*i1[0] + w4*i1[1]
+ w6*i2[0] + w7*i2[1];
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oo[y*W+W-1] = s;
}
} else {
// degenerate (small H/W): fallback
for (int y=0; y<H; ++y) {
for (int x=0; x<W; ++x) {
float s = cb;
for (int ky=0; ky<3; ++ky) {
int iy = y + ky - 1;
if (iy<0||iy>=H) continue;
for (int kx=0; kx<3; ++kx) {
int ix = x + kx - 1;
if (ix<0||ix>=W) continue;
s += ww[ky*3+kx] * ii[iy*W+ix];
}
}
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oo[y*W+x] = s;
}
auto edge = [&](int y, int x) {
float s = 0.0f;
for (int ky = 0; ky < 3; ++ky) {
const int iy = y + ky - 1;
if (iy < 0 || iy >= H) continue;
for (int kx = 0; kx < 3; ++kx) {
const int ix = x + kx - 1;
if (ix < 0 || ix >= W) continue;
s += ww[ky * 3 + kx] * ii[iy * W + ix];
}
}
}
return;
oo[y * W + x] = relu_maybe(s * a + b, relu);
};
for (int x = 0; x < W; ++x) { edge(0, x); edge(H - 1, x); }
for (int y = 1; y < H - 1; ++y) { edge(y, 0); edge(y, W - 1); }
}
}
// 通用路径 — 常数外提 + iy*W 预计算
for (int g=0; g<grp; ++g) {
int ic_base = g*icpg;
for (int oc=0; oc<ocpg; ++oc) {
int occ = g*ocpg + oc;
float* oo = o + occ*Ho*Wo;
float cb = conv_b ? conv_b[occ] : 0.0f;
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
for (int y=0; y<Ho; ++y) {
int yi0 = y*str;
for (int x=0; x<Wo; ++x) {
float s = cb;
int xi0 = x*str;
for (int ic=0; ic<icpg; ++ic) {
int icc = ic_base + ic;
const float* ww = conv_w + ((occ*icpg+ic)*K*K);
const float* ii = in + icc*H*W;
for (int ky=0; ky<K; ++ky) {
int iy = yi0 + ky - pad;
if (iy<0||iy>=H) continue;
int iy_off = iy*W;
for (int kx=0; kx<K; ++kx) {
int ix = xi0 + kx - pad;
if (ix<0||ix>=W) continue;
s += ww[ky*K+kx] * ii[iy_off + ix];
}
static void conv_generic(float* o, const float* in, const float* cw, const float* cb,
const float* ba, const float* bb,
int H, int W, int iC, int oC, int K, int str, int grp, bool relu)
{
const int Ho = H / str, Wo = W / str, pad = K / 2;
const int icpg = iC / grp, ocpg = oC / grp;
for (int g = 0; g < grp; ++g) {
const int ic_base = g * icpg;
for (int oc = 0; oc < ocpg; ++oc) {
const int occ = g * ocpg + oc;
float* oo = o + occ * Ho * Wo;
const float bias = cb ? cb[occ] : 0.0f;
const float a = ba ? ba[occ] : 1.0f;
const float b = bb ? bb[occ] : 0.0f;
for (int y = 0; y < Ho; ++y) {
const int yi0 = y * str - pad;
const int ky0 = std::max(0, -yi0);
const int ky1 = std::min(K, H - yi0);
for (int x = 0; x < Wo; ++x) {
const int xi0 = x * str - pad;
const int kx0 = std::max(0, -xi0);
const int kx1 = std::min(K, W - xi0);
float s = bias;
for (int ic = 0; ic < icpg; ++ic) {
const float* ww = cw + ((occ * icpg + ic) * K * K);
const float* ii = in + (ic_base + ic) * H * W;
for (int ky = ky0; ky < ky1; ++ky) {
const float* irow = ii + (yi0 + ky) * W + xi0;
const float* wrow = ww + ky * K;
for (int kx = kx0; kx < kx1; ++kx)
s += wrow[kx] * irow[kx];
}
}
s = s*ba + bb;
if (use_relu && s<0) s = 0;
oo[y*Wo+x] = s;
oo[y * Wo + x] = relu_maybe(s * a + b, relu);
}
}
}
}
}
// ============================================================
// 纯 conv (无 BN/ReLU, cls_head / size_head / skip 的 conv 部分)
// ============================================================
static void conv2d(float* o, const float* in, const float* w, const float* b,
int H, int W, int iC, int oC, int K, int str, int grp) {
conv_bn_relu(o, in, w, b, nullptr, nullptr, H, W, iC, oC, K, str, grp, false);
static void conv_bn_relu(float* o, const float* in,
const float* cw, const float* cb,
const float* ba, const float* bb,
int H, int W, int iC, int oC, int K, int str, int grp, bool relu)
{
if (!cw || !in || !o) return;
if (K == 1 && str == 1 && grp == 1)
conv1x1(o, in, cw, cb, ba, bb, H, W, iC, oC, relu);
else if (K == 3 && str == 1 && grp == iC && iC == oC)
dwconv3x3(o, in, cw, ba, bb, H, W, iC, relu);
else
conv_generic(o, in, cw, cb, ba, bb, H, W, iC, oC, K, str, grp, relu);
}
// ============================================================
// SE 通道注意力 (使用快速 sigmoid)
// ============================================================
static void se_module(float* x, int C, int N,
const float* w1, const float* b1,
const float* w2, const float* b2,
float* buf) {
if (!w1||!w2) return;
int mid=C/4;
float* pool=buf;
const WT* w1t, const float* b1,
const WT* w2t, const float* b2)
{
if (!w1t || !w2t) return;
const float* w1 = w1t->d;
const float* w2 = w2t->d;
const int mid = w1t->s[0];
// AdaptiveAvgPool
for (int c=0; c<C; ++c) {
float s=0;
float* px = x+c*N, *end = px+N;
while (px < end) { s += *px++; }
pool[c]=s/(float)N;
float* pool = g_se;
float* fc1 = g_se + C;
float* fc2 = g_se + C + mid;
const float inv_n = 1.0f / (float)N;
for (int c = 0; c < C; ++c) {
float s = 0.0f;
const float* px = x + c * N;
for (int i = 0; i < N; ++i) s += px[i];
pool[c] = s * inv_n;
}
// fc1: C→mid + ReLU
float* fc1=buf+C;
for (int o=0; o<mid; ++o) {
float s=b1?b1[o]:0;
const float* ww=w1+o*C;
for (int ic=0; ic<C; ++ic) s+=pool[ic]*ww[ic];
fc1[o]=s>0?s:0;
for (int o = 0; o < mid; ++o) {
float s = b1 ? b1[o] : 0.0f;
const float* ww = w1 + o * C;
for (int ic = 0; ic < C; ++ic) s += pool[ic] * ww[ic];
fc1[o] = s > 0.0f ? s : 0.0f;
}
// fc2: mid→C + Sigmoid (fast)
float* fc2=buf+C+mid;
for (int o=0; o<C; ++o) {
float s=b2?b2[o]:0;
const float* ww=w2+o*mid;
for (int ic=0; ic<mid; ++ic) s+=fc1[ic]*ww[ic];
fc2[o]=fast_sigmoid(s);
for (int o = 0; o < C; ++o) {
float s = b2 ? b2[o] : 0.0f;
const float* ww = w2 + o * mid;
for (int ic = 0; ic < mid; ++ic) s += fc1[ic] * ww[ic];
fc2[o] = sigmoid_f(s);
}
for (int c=0; c<C; ++c) {
float* xc=x+c*N; float sc=fc2[c];
for (int i=0; i<N; ++i) xc[i]*=sc;
for (int c = 0; c < C; ++c) {
float* xc = x + c * N;
const float sc = fc2[c];
for (int i = 0; i < N; ++i) xc[i] *= sc;
}
}
// ============================================================
// SE-ResDW Block v10
// ============================================================
static void se_block_v10(float* o, const float* in, int iC, int oC, int H, int W, int str, const char* pfx) {
int Ho=H/str, Wo=W/str, No=Ho*Wo;
char na[128], wr[128], bi[128];
static constexpr int STRIDE = 8, OH = 15, OW = 20, NC = 4;
// dw conv + BN + ReLU (fused)
std::snprintf(na,128,"%s.dw.0.weight",pfx);
std::snprintf(wr,128,"%s.dw.1._alpha",pfx);
std::snprintf(bi,128,"%s.dw.1._beta",pfx);
conv_bn_relu(m->dw_out, in, wf(na), nullptr,
wf(wr), wf(bi), H, W, iC, iC, 3, str, iC, true);
static void model_compute() {
struct Lay { int iC, oC, H, W, s; const char* bn; const char* sn; };
static const Lay lays[] = {
{3, 9, 120, 160, 2, "b1", "se_0"},
{9, 9, 60, 80, 1, "b2", "se_1"},
{9, 13, 60, 80, 2, "b3", "se_2"},
{13, 13, 30, 40, 1, "b4", "se_3"},
{13, 19, 30, 40, 2, "b5", "se_4"},
{19, 19, 15, 20, 1, "b6", "se_5"},
{19, 19, 15, 20, 1, "b7", "se_6"},
{19, 44, 15, 20, 1, "b8", "se_7"},
{44, 44, 15, 20, 1, "b9", "se_8"},
{44, 64, 15, 20, 1, "b10", "se_9"},
{64, 64, 15, 20, 1, "b11", "se_10"},
};
// pw conv + BN + ReLU (fused)
std::snprintf(na,128,"%s.pw.0.weight",pfx);
std::snprintf(wr,128,"%s.pw.1._alpha",pfx);
std::snprintf(bi,128,"%s.pw.1._beta",pfx);
conv_bn_relu(o, m->dw_out, wf(na), nullptr,
wf(wr), wf(bi), Ho, Wo, iC, oC, 1, 1, 1, true);
const float* in = g_preproc;
float* bufs[2] = { g_A, g_B };
int cur = 0;
// skip (BN fused, no ReLU)
bool iden=(str==1 && iC==oC);
float* skip=m->dw_out;
if (iden) {
std::memcpy(skip, in, iC*H*W*4);
} else {
std::snprintf(na,128,"%s.sk.0.weight",pfx);
std::snprintf(wr,128,"%s.sk.1._alpha",pfx);
std::snprintf(bi,128,"%s.sk.1._beta",pfx);
conv_bn_relu(skip, in, wf(na), nullptr,
wf(wr), wf(bi), H, W, iC, oC, 1, str, 1, false);
for (const Lay& l : lays) {
int K, grp;
if (l.s == 2) { K = 3; grp = 1; }
else if (l.iC == l.oC) { K = 3; grp = l.iC; }
else { K = 1; grp = 1; }
const int Ho = l.H / l.s, Wo = l.W / l.s;
float* out = bufs[cur];
char wkey[128], akey[128], bkey[128];
std::snprintf(wkey, 128, "%s.0.weight", l.bn);
std::snprintf(akey, 128, "%s.1._alpha", l.bn);
std::snprintf(bkey, 128, "%s.1._beta", l.bn);
conv_bn_relu(out, in, wf(wkey), nullptr, wf(akey), wf(bkey),
l.H, l.W, l.iC, l.oC, K, l.s, grp, true);
char s1[128], s1b[128], s3[128], s3b[128];
std::snprintf(s1, 128, "%s.1.weight", l.sn);
std::snprintf(s1b, 128, "%s.1.bias", l.sn);
std::snprintf(s3, 128, "%s.3.weight", l.sn);
std::snprintf(s3b, 128, "%s.3.bias", l.sn);
se_module(out, l.oC, Ho * Wo, wt(s1), wf(s1b), wt(s3), wf(s3b));
in = out;
cur ^= 1;
}
for (int i=0; i<oC*No; ++i) o[i]+=skip[i];
// SE
std::snprintf(na,128,"%s.se.1.weight",pfx);
std::snprintf(bi,128,"%s.se.1.bias",pfx);
char se2[128], se2b[128];
std::snprintf(se2,128,"%s.se.3.weight",pfx);
std::snprintf(se2b,128,"%s.se.3.bias",pfx);
se_module(o, oC, No, wf(na), wf(bi), wf(se2), wf(se2b), m->se_buf);
conv_bn_relu(g_T, in, wf("head.0.weight"), nullptr,
wf("head.1._alpha"), wf("head.1._beta"),
15, 20, 64, 64, 3, 1, 64, true);
relu_f(o, oC*No);
conv_bn_relu(g_out, g_T, wf("out.weight"), wf("out.bias"),
nullptr, nullptr, 15, 20, 64, 9, 1, 1, 1, false);
}
// ============================================================
// 解码 — softmax + NMS (预计算 peak mask)
// ============================================================
static constexpr int OH=15, OW=20, STRIDE=8, NC=4;
static constexpr float RS[NC][2] = {
{56.85f,45.39f}, {101.44f,66.53f}, {96.75f,62.86f}, {66.01f,35.49f},
};
static int decode(DetectBoxV10* boxes, int max_boxes, float thr) {
const int N = OH * OW;
int cnt = 0;
static int decode(DetectBoxV10* boxes, int max, const float* th) {
int N=OH*OW, cnt=0;
// 预计算 peak mask: 每个类别做一次 maxpool
uint8* mask = m->peak_mask;
for (int c=0; c<NC; ++c) {
const float* cp = m->cl + c*N;
uint8* mp = mask + c*N;
for (int gy=0; gy<OH; ++gy) {
for (int gx=0; gx<OW; ++gx) {
if (cp[gy*OW+gx] < 0) { mp[gy*OW+gx]=0; continue; }
bool peak=true;
float val=cp[gy*OW+gx];
for (int dy=-1; dy<=1&&peak; ++dy)
for (int dx=-1; dx<=1&&peak; ++dx) {
int ny=gy+dy, nx=gx+dx;
if (ny<0||ny>=OH||nx<0||nx>=OW) continue;
if (cp[ny*OW+nx] > val) peak=false;
}
mp[gy*OW+gx] = peak ? 1 : 0;
}
for (int idx = 0; idx < N; ++idx) {
float mx = -1e30f;
for (int c = 0; c <= NC; ++c) mx = std::max(mx, g_out[c * N + idx]);
float sum = 0.0f;
for (int c = 0; c <= NC; ++c) {
const float e = std::exp(g_out[c * N + idx] - mx);
g_prob[c * N + idx] = e;
sum += e;
}
const float inv = 1.0f / sum;
for (int c = 0; c <= NC; ++c) g_prob[c * N + idx] *= inv;
}
// softmax + check per cell
for (int gy=0; gy<OH && cnt<max; ++gy) {
for (int gx=0; gx<OW && cnt<max; ++gx) {
int idx=gy*OW+gx;
for (int c = 0; c < NC && cnt < max_boxes; ++c) {
for (int gy = 0; gy < OH; ++gy) {
for (int gx = 0; gx < OW; ++gx) {
const int idx = gy * OW + gx;
const float pc = g_prob[c * N + idx];
if (pc <= thr || pc <= g_prob[NC * N + idx]) continue;
// softmax over NC+1 classes (fast exp)
float sm[5], mx=-1e9f, sum=0;
for (int c=0; c<=NC; ++c) {
float v=m->cl[c*N+idx];
sm[c]=v; if(v>mx) mx=v;
bool peak = true;
for (int dy = -1; dy <= 1 && peak; ++dy)
for (int dx = -1; dx <= 1 && peak; ++dx) {
const int ny = gy + dy, nx = gx + dx;
if (ny < 0 || ny >= OH || nx < 0 || nx >= OW) continue;
if (g_prob[c * N + ny * OW + nx] > pc) peak = false;
}
if (!peak) continue;
const float tx = clamp_f(g_out[(NC + 1) * N + idx], 0.0f, 1.0f);
const float ty = clamp_f(g_out[(NC + 2) * N + idx], 0.0f, 1.0f);
const float tw = std::max(g_out[(NC + 3) * N + idx], 0.5f);
const float th = std::max(g_out[(NC + 4) * N + idx], 0.5f);
const float cx = (gx + tx) * STRIDE, cy = (gy + ty) * STRIDE;
const float w = tw * STRIDE, h = th * STRIDE;
boxes[cnt].cls = c;
boxes[cnt].conf = pc;
boxes[cnt].cx = cx;
boxes[cnt].cy = cy;
boxes[cnt].w = w;
boxes[cnt].h = h;
if (++cnt >= max_boxes) return cnt;
}
for (int c=0; c<=NC; ++c) { sm[c]=fast_exp(sm[c]-mx); sum+=sm[c]; }
// best class (excluding bg=NC), per-class threshold
int bc=-1; float bs=0;
for (int c=0; c<NC; ++c) {
if (!mask[c*N+idx]) continue;
float p=sm[c]/sum;
if (p>bs && p>=th[c]) { bs=p; bc=c; }
}
if (bc<0) continue;
float pw=std::max(1.0f, m->sz[0*N+idx]*RS[bc][0]);
float ph=std::max(1.0f, m->sz[1*N+idx]*RS[bc][1]);
boxes[cnt].cls=bc; boxes[cnt].conf=bs;
boxes[cnt].cx=((float)gx+0.5f)*STRIDE;
boxes[cnt].cy=((float)gy+0.5f)*STRIDE;
boxes[cnt].w=pw; boxes[cnt].h=ph;
cnt++;
}
}
return cnt;
}
// ============================================================
// 预处理 LUT: uint8→float normalized
// ============================================================
static float g_lut[256];
static void init_lut() {
for (int i=0; i<256; ++i) g_lut[i] = (float)i / 255.0f;
}
// ============================================================
// 前向推理 — 模型计算体 (preproc 已由调用方完成)
// ============================================================
static void model_compute() {
float* in = m->preproc;
// stem: 3→6, s2 (fused)
conv_bn_relu(m->stem, in, wf("s.0.weight"), nullptr,
wf("s.1._alpha"), wf("s.1._beta"), 120, 160, 3, 6, 3, 2, 1, true);
// blocks
se_block_v10(m->b1, m->stem, 6, 8, 60, 80, 1, "b1");
se_block_v10(m->b2, m->b1, 8, 12, 60, 80, 2, "b2");
se_block_v10(m->b3, m->b2, 12, 16, 30, 40, 2, "b3");
se_block_v10(m->b4, m->b3, 16, 32, 15, 20, 1, "b4");
se_block_v10(m->b5, m->b4, 32, 48, 15, 20, 1, "b5");
se_block_v10(m->b6, m->b5, 48, 48, 15, 20, 1, "b6");
// shared: 48→24, 1×1 (fused)
conv_bn_relu(m->sh, m->b6, wf("sh.0.weight"), nullptr,
wf("sh.1._alpha"), wf("sh.1._beta"), 15, 20, 48, 24, 1, 1, 1, true);
// heads (pure conv, no BN)
conv2d(m->cl, m->sh, wf("ch.weight"), wf("ch.bias"), 15, 20, 24, 5, 1, 1, 1);
conv2d(m->sz, m->sh, wf("sz.weight"), wf("sz.bias"), 15, 20, 24, 2, 1, 1, 1);
}
// ── 自写 640×480 → 3×120×160 float planar (4×4 box avg + BGR→RGB + /255) ──
static void preproc_640(const uint8* bgr) {
float* in = m->preproc;
const int sw=640, sh=480;
for(int c=0;c<3;++c){
int sc=2-c;
float* ch = in + c*120*160;
for(int y=0;y<120;++y){
int iy=y*4;
for(int x=0;x<160;++x){
int ix=x*4, sum=0;
for(int dy=0;dy<4;++dy)
for(int dx=0;dx<4;++dx)
sum += bgr[(iy+dy)*sw*3 + (ix+dx)*3 + sc];
ch[y*160+x] = (float)sum * (1.0f/(16.0f*255.0f));
// cv2.resize INTER_LINEAR compatible
static void resize_bilinear_hwc3(const uint8_t* src, int sw, int sh, uint8_t* dst) {
const float fx = (float)sw / (float)IN_W;
const float fy = (float)sh / (float)IN_H;
static int x0i[IN_W], x1i[IN_W];
static float xw[IN_W];
for (int x = 0; x < IN_W; ++x) {
float sx = (x + 0.5f) * fx - 0.5f;
if (sx < 0) sx = 0;
int x0 = (int)sx;
if (x0 > sw - 1) x0 = sw - 1;
int x1 = std::min(x0 + 1, sw - 1);
x0i[x] = x0 * 3; x1i[x] = x1 * 3; xw[x] = sx - (float)x0;
}
for (int y = 0; y < IN_H; ++y) {
float sy = (y + 0.5f) * fy - 0.5f;
if (sy < 0) sy = 0;
int y0 = (int)sy;
if (y0 > sh - 1) y0 = sh - 1;
const int y1 = std::min(y0 + 1, sh - 1);
const float wy = sy - (float)y0;
const uint8_t* r0 = src + (size_t)y0 * sw * 3;
const uint8_t* r1 = src + (size_t)y1 * sw * 3;
uint8_t* d = dst + y * IN_W * 3;
for (int x = 0; x < IN_W; ++x) {
const float wx = xw[x];
const int a = x0i[x], b = x1i[x];
for (int c = 0; c < 3; ++c) {
const float top = r0[a + c] + (r0[b + c] - r0[a + c]) * wx;
const float bot = r1[a + c] + (r1[b + c] - r1[a + c]) * wx;
const float v = top + (bot - top) * wy;
d[x * 3 + c] = (uint8_t)(v + 0.5f);
}
}
}
}
// ── 原版: 160×120 BGR → 3×120×160 float planar ──
static void preproc_160(const uint8* bgr) {
float* in = m->preproc;
for (int c=0; c<3; ++c) {
int sc=2-c; float* ch=in+c*120*160;
for (int y=0; y<120; ++y) {
const uint8* row=bgr+y*160*3;
for (int x=0; x<160; ++x) ch[y*160+x]=g_lut[row[x*3+sc]];
static void preproc_160(const uint8_t* img, YoloPixFmt fmt) {
for (int c = 0; c < 3; ++c) {
const int sc = (fmt == YOLO_FMT_BGR) ? (2 - c) : c;
float* ch = g_preproc + c * IN_H * IN_W;
for (int y = 0; y < IN_H; ++y) {
const uint8_t* row = img + y * IN_W * 3;
for (int x = 0; x < IN_W; ++x) ch[y * IN_W + x] = g_lut[row[x * 3 + sc]];
}
}
}
// ── 内部: preproc + compute ──
static void forward(const uint8* bgr) { preproc_160(bgr); model_compute(); }
// ============================================================
// 接口
// ============================================================
static bool g_rdy=false;
static bool g_rdy = false;
bool model_v10_init(const char* path) {
FILE* f=fopen(path,"rb");
if(!f) return false;
fread(&gn,sizeof(int),1,f);
gw=new WT[gn];
for (int i=0; i<gn; ++i) {
WT& t=gw[i]; int nl; fread(&nl,4,1,f); fread(t.n,1,nl,f); t.n[nl]=0;
fread(&t.nd,4,1,f); int tot=1;
for (int d=0; d<t.nd; ++d) { fread(&t.s[d],4,1,f); tot*=t.s[d]; }
for (int d=t.nd; d<4; ++d) t.s[d]=1;
t.d=new float[tot]; fread(t.d,4,tot,f);
FILE* f = std::fopen(path, "rb");
if (!f) return false;
if (std::fread(&gn, sizeof(int), 1, f) != 1 || gn <= 0 || gn > 4096) { std::fclose(f); return false; }
gw = new WT[gn];
for (int i = 0; i < gn; ++i) {
WT& t = gw[i];
int nl = 0;
bool ok = std::fread(&nl, 4, 1, f) == 1 && nl > 0 && nl < 127
&& std::fread(t.n, 1, nl, f) == (size_t)nl;
if (ok) {
t.n[nl] = 0;
ok = std::fread(&t.nd, 4, 1, f) == 1 && t.nd >= 0 && t.nd <= 4;
}
if (ok) {
int tot = 1;
for (int d = 0; d < t.nd && ok; ++d) {
ok = std::fread(&t.s[d], 4, 1, f) == 1 && t.s[d] > 0;
tot *= t.s[d];
}
for (int d = t.nd; d < 4; ++d) t.s[d] = 1;
if (ok) {
t.d = new float[tot];
ok = std::fread(t.d, 4, tot, f) == (size_t)tot;
}
}
if (!ok) { gn = (t.d ? i + 1 : i); std::fclose(f); model_v10_deinit(); return false; }
}
fclose(f);
std::fclose(f);
bn_precompute();
m=new(std::nothrow) M10();
if(!m) { delete[] gw; gw=nullptr; return false; }
std::memset(m,0,sizeof(M10));
g_rdy=true;
for (int i = 0; i < 256; ++i) g_lut[i] = (float)i / 255.0f;
g_preproc = new(std::nothrow) float[3 * IN_H * IN_W];
g_A = new(std::nothrow) float[BUF_FLOATS];
g_B = new(std::nothrow) float[BUF_FLOATS];
g_T = new(std::nothrow) float[BUF_FLOATS];
g_out = new(std::nothrow) float[9 * 15 * 20];
if (!g_preproc || !g_A || !g_B || !g_T || !g_out) { model_v10_deinit(); return false; }
g_rdy = true;
return true;
}
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh) {
if(!g_rdy) return 0;
if(w==640 && h==480) {
preproc_640(bgr);
model_compute();
return decode(boxes,max,thresh);
int model_v10_detect_fmt(const uint8_t* img, int w, int h, YoloPixFmt fmt,
DetectBoxV10* boxes, int max_boxes, float thr) {
if (!g_rdy || !img || !boxes || max_boxes <= 0 || w <= 1 || h <= 1) return 0;
const uint8_t* net_in = img;
if (w != IN_W || h != IN_H) {
resize_bilinear_hwc3(img, w, h, g_resized);
net_in = g_resized;
}
if(w!=160||h!=120) return 0;
forward(bgr);
return decode(boxes,max,thresh);
preproc_160(net_in, fmt);
model_compute();
const int n = decode(boxes, max_boxes, thr);
const float sx = (float)w / (float)IN_W;
const float sy = (float)h / (float)IN_H;
for (int i = 0; i < n; ++i) {
boxes[i].cx *= sx; boxes[i].w *= sx;
boxes[i].cy *= sy; boxes[i].h *= sy;
}
return n;
}
int model_v10_detect(const uint8_t* img, int w, int h, DetectBoxV10* boxes, int max_boxes, const float* thresh) {
float thr = thresh ? thresh[0] : 0.6f;
return model_v10_detect_fmt(img, w, h, YOLO_FMT_BGR, boxes, max_boxes, thr);
}
const float* model_v10_forward_debug(const float* chw) {
if (!g_rdy || !chw) return nullptr;
std::memcpy(g_preproc, chw, 3 * IN_H * IN_W * sizeof(float));
model_compute();
return g_out;
}
void model_v10_set_dump_dir(const char* dir) {
if (dir) { std::strncpy(g_dump_dir, dir, sizeof(g_dump_dir) - 1); g_dump_dir[sizeof(g_dump_dir)-1] = 0; }
else g_dump_dir[0] = 0;
}
void model_v10_deinit() {
if(gw) { for(int i=0; i<gn; ++i) delete[] gw[i].d; delete[] gw; gw=nullptr; }
delete m; m=nullptr; g_rdy=false;
if (gw) { for (int i = 0; i < gn; ++i) delete[] gw[i].d; delete[] gw; gw = nullptr; }
gn = 0;
delete[] g_preproc; delete[] g_A; delete[] g_B; delete[] g_T; delete[] g_out;
g_preproc = g_A = g_B = g_T = g_out = nullptr;
g_rdy = false;
}
bool model_v10_ready() { return g_rdy; }
src/model_v10.hpp
+3 -1
View File
@@ -2,11 +2,13 @@
#include "types_model.hpp"
struct DetectBoxV10 {
int cls; // 0=锥 1=红灯 2=绿灯 3=行人
int cls;
float conf;
float cx, cy, w, h;
};
typedef enum { YOLO_FMT_BGR = 0, YOLO_FMT_RGB = 1 } YoloPixFmt;
bool model_v10_init(const char* path);
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh);
void model_v10_deinit();