cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add wust typr mpc and mutipule x

Browse Source
This commit is contained in:
cyy_mac
2026-03-27 03:41:42 +08:00
parent 2c64655fae
commit 7dcb53bb77
192 changed files with 29571 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
wust_vision-main/tasks/auto_guidance/auto_guidance.cpp Normal file
View File

@@ -0,0 +1,198 @@
#include "auto_guidance.hpp"
#include "tasks/auto_guidance/guidance_detector/detector_base.hpp"
#include "tasks/auto_guidance/guidance_detector/detector_factory.hpp"
#include "tasks/auto_guidance/guidance_tracker/guidance_tracker.hpp"
#include "tasks/utils/utils.hpp"
#include "wust_vl/common/concurrency/queues.hpp"
#include "wust_vl/common/utils/logger.hpp"
#include "wust_vl/common/utils/timer.hpp"
namespace wust_vision {
namespace auto_guidance {
struct AutoGuidance::Impl {
~Impl() {
lights_queue_->stop();
if (processing_thread_) {
processing_thread_->stop();
wust_vl::common::concurrency::ThreadManager::instance().unregisterThread(
processing_thread_->getName()
);
}
}
void init(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
camera_info_ = camera_info;
std::string backend = config["backend"].as<std::string>();
std::cout << "backend: " << backend << std::endl;
auto detector_cfg = config["detector"];
detector_ = DetectorFactory::createDetector(backend, detector_cfg, debug_);
detector_->setCallback(std::bind(
&AutoGuidance::Impl::detectCallback,
this,
std::placeholders::_1,
std::placeholders::_2
));
tracker_ = GuidanceTracker::create(config["tracker"]);
lights_queue_ =
std::make_unique<wust_vl::common::concurrency::OrderedQueue<GreenLights>>(100, 500);
latency_averager_ =
std::make_unique<wust_vl::common::concurrency::Averager<double>>(100);
}
void pushInput(CommonFrame& frame) {
img_recv_count_++;
if (detector_) {
detector_->pushInput(frame);
}
}
void detectCallback(const std::vector<GreenLight>& objs, const CommonFrame& frame) {
detect_finish_count_++;
GreenLights lights;
lights.lights = objs;
lights.timestamp = frame.img_frame.timestamp;
lights.id = frame.id;
for (auto& light: lights.lights) {
light.solvePnP(camera_info_.first, camera_info_.second);
light.timestamp = frame.img_frame.timestamp;
light.image_size = frame.img_frame.src_img.size();
}
green_lights_ = lights;
lights_queue_->enqueue(lights);
if (debug_) {
std::lock_guard<std::mutex> lock(dbg_mutex_);
dbg_.lights = lights;
dbg_.img_frame = frame.img_frame;
}
}
void lightsCallback(const GreenLights& lights) {
if (lights.timestamp <= tracker_->getLastTime()) {
WUST_WARN(logger_) << "Received out-of-order armor data, discarded.";
return;
}
GuidanceTarget target;
target = tracker_->track(lights);
{
std::lock_guard<std::mutex> lock(target_mutex_);
guidance_target_ = target;
}
auto now = std::chrono::steady_clock::now();
auto latency_ms = wust_vl::common::utils::time_utils::durationMs(lights.timestamp, now);
latency_averager_->add(latency_ms);
dbg_.latency_ms = latency_averager_->average();
if (debug_) {
std::lock_guard<std::mutex> lock(dbg_mutex_);
dbg_.target = target;
}
}
void start() {
processing_thread_ = wust_vl::common::concurrency::MonitoredThread::create(
"AutoAimProcessingThread",
[this](wust_vl::common::concurrency::MonitoredThread::Ptr self) {
this->processingLoop(self);
}
);
wust_vl::common::concurrency::ThreadManager::instance().registerThread(
processing_thread_
);
run_flag_ = true;
}
void processingLoop(wust_vl::common::concurrency::MonitoredThread::Ptr self) {
while (!self->isAlive()) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
while (self->isAlive()) {
if (!self->waitPoint())
break;
self->heartbeat();
printStats();
GreenLights lights;
bool skip;
// if (lights_queue_->dequeue_wait(lights, skip)) {
// lightsCallback(lights);
// tracker_finish_count_++;
// if (skip) {
// WUST_DEBUG(logger_) << "OrderQueue skip";
// }
// }
if (!lights_queue_->try_dequeue(lights)) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
continue;
}
lightsCallback(lights);
tracker_finish_count_++;
}
}
GuidanceTarget getTarget() {
timer_count_++;
std::lock_guard<std::mutex> lock(target_mutex_);
return guidance_target_;
}
void printStats() {
utils::XSecOnce(
[&] {
WUST_INFO(logger_)
<< "Rec: " << img_recv_count_ << ", Det: " << detect_finish_count_
<< ", Fin: " << tracker_finish_count_ << ", Lat: " << dbg_.latency_ms
<< "ms"
<< ", Tc:" << timer_count_;
img_recv_count_ = 0;
detect_finish_count_ = 0;
tracker_finish_count_ = 0;
timer_count_ = 0;
},
1.0
);
}
std::unique_ptr<detector_base> detector_;
std::string logger_ = "auto_guidance";
std::chrono::steady_clock::time_point last_stat_time_steady_ =
std::chrono::steady_clock::now();
GuidanceTracker::Ptr tracker_;
bool run_flag_ = false;
int detect_finish_count_ = 0;
int img_recv_count_ = 0;
int tracker_finish_count_ = 0;
int timer_count_ = 0;
bool debug_ = false;
GuidanceTarget guidance_target_;
GreenLights green_lights_;
std::shared_ptr<wust_vl::common::concurrency::MonitoredThread> processing_thread_;
std::unique_ptr<wust_vl::common::concurrency::OrderedQueue<GreenLights>> lights_queue_;
std::unique_ptr<wust_vl::common::concurrency::Averager<double>> latency_averager_;
std::pair<cv::Mat, cv::Mat> camera_info_;
AutoGuidanceDebug dbg_;
std::mutex target_mutex_;
std::mutex dbg_mutex_;
};
AutoGuidance::AutoGuidance(): _impl(std::make_unique<Impl>()) {}
AutoGuidance::~AutoGuidance() {
_impl.reset();
}
void
AutoGuidance::init(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info) {
_impl->init(config, camera_info);
}
void AutoGuidance::start() {
_impl->start();
}
void AutoGuidance::pushInput(CommonFrame& frame) {
_impl->pushInput(frame);
}
void AutoGuidance::setDebug(bool debug) {
_impl->debug_ = debug;
}
AutoGuidanceDebug AutoGuidance::getDebug() {
std::lock_guard<std::mutex> lock(_impl->dbg_mutex_);
return _impl->dbg_;
}
GuidanceTarget AutoGuidance::getTarget() {
return _impl->getTarget();
}
} // namespace auto_guidance
} // namespace wust_vision

24
wust_vision-main/tasks/auto_guidance/auto_guidance.hpp Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include "debug.hpp"
#include "tasks/type_common.hpp"
namespace wust_vision {
namespace auto_guidance {
class AutoGuidance {
public:
static inline std::unique_ptr<AutoGuidance> create() {
return std::make_unique<AutoGuidance>();
}
AutoGuidance();
~AutoGuidance();
void init(const YAML::Node& config, const std::pair<cv::Mat, cv::Mat>& camera_info);
void start();
void pushInput(CommonFrame& frame);
void setDebug(bool debug);
GuidanceTarget getTarget();
AutoGuidanceDebug getDebug();
struct Impl;
std::unique_ptr<Impl> _impl;
};
} // namespace auto_guidance
} // namespace wust_vision

295
wust_vision-main/tasks/auto_guidance/debug.cpp Normal file
View File

@@ -0,0 +1,295 @@
#include "debug.hpp"
#include <fcntl.h>
#include <fmt/format.h>
#include <fstream>
#include <nlohmann/json.hpp>
#include <sys/mman.h>
#include <sys/stat.h>
namespace wust_vision {
namespace auto_guidance {
void drawAutoGuidanceDebugContent(cv::Mat& debug_img, const AutoGuidanceDebug& dbg) {
auto target = dbg.target;
auto lights = dbg.lights;
lights.drawFront(debug_img);
if (target.is_tracking_) {
auto now = std::chrono::steady_clock::now();
target.predict(now);
target.draw(debug_img);
}
std::string latency_str = fmt::format("Latency: {:.2f}ms", dbg.latency_ms);
cv::putText(
debug_img,
latency_str,
cv::Point(10, 30),
cv::FONT_HERSHEY_SIMPLEX,
0.8,
cv::Scalar(255, 255, 255),
2
);
cv::circle(
debug_img,
cv::Point2i(debug_img.cols / 2, debug_img.rows / 2),
5,
cv::Scalar(255, 255, 255),
2
);
double cx_norm = target.center().x / target.image_size_.width * 2.0 - 1.0;
double diff_center_norm = (target.is_tracking_) ? cx_norm : 0;
{
std::string diff_str = fmt::format("diff_cx_norm: {:.3f}", diff_center_norm);
int margin = 10;
int font_face = cv::FONT_HERSHEY_SIMPLEX;
double font_scale = 0.7;
int thickness = 2;
int baseline = 0;
cv::Size text_size =
cv::getTextSize(diff_str, font_face, font_scale, thickness, &baseline);
// 右上角文本左下角坐标
int x = debug_img.cols - margin - text_size.width;
int y = margin + text_size.height;
// 背景框，可选
cv::rectangle(
debug_img,
cv::Rect(
x - 5,
y - text_size.height - 5,
text_size.width + 10,
text_size.height + 10
),
cv::Scalar(0, 0, 0),
cv::FILLED,
cv::LINE_AA
);
cv::putText(
debug_img,
diff_str,
cv::Point(x, y),
font_face,
font_scale,
cv::Scalar(0, 255, 255),
thickness,
cv::LINE_AA
);
}
if (target.is_tracking_) {
const auto& s = target.target_state_;
std::string line1 =
fmt::format("pos: {:.1f} {:.1f} {:.1f} {:.1f}", s(0), s(2), s(4), s(6));
std::string line2 =
fmt::format("vel: {:.2f} {:.2f} {:.2f} {:.2f}", s(1), s(3), s(5), s(7));
int x = 10;
int y = debug_img.rows - 30; // 左下角位置
int dy = 28;
cv::putText(
debug_img,
line1,
cv::Point(x, y),
cv::FONT_HERSHEY_SIMPLEX,
0.75,
cv::Scalar(0, 255, 0),
2
);
cv::putText(
debug_img,
line2,
cv::Point(x, y + dy),
cv::FONT_HERSHEY_SIMPLEX,
0.75,
cv::Scalar(0, 255, 0),
2
);
double vx = s(1);
double vy = s(3);
cv::Point2f p0 = target.center();
double scale = 0.5;
cv::Point2f p1(p0.x + vx * scale, p0.y + vy * scale);
cv::arrowedLine(
debug_img,
p0,
p1,
cv::Scalar(0, 255, 0),
2,
cv::LINE_AA,
0,
0.25 // 箭头比例
);
}
}
void drawDebugOverlayWrite(const AutoGuidanceDebug& dbg, bool auto_fps) {
static auto last_show_time = std::chrono::steady_clock::now();
if (dbg.img_frame.src_img.empty())
return;
cv::Mat src_img = dbg.img_frame.src_img;
auto now = std::chrono::steady_clock::now();
const double min_interval_ms = 1000.0 / 30.0;
if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
< min_interval_ms
&& auto_fps)
return;
last_show_time = now;
// 图像构造
cv::Mat debug_img;
src_img.convertTo(debug_img, -1, 1, 0);
cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
if (debug_img.empty())
return;
// 封装后的绘图函数
drawAutoGuidanceDebugContent(debug_img, dbg);
cv::cvtColor(debug_img, debug_img, cv::COLOR_RGB2BGR);
// 编码写入共享内存路径
std::vector<uchar> buf;
cv::imencode(".jpg", debug_img, buf);
std::ofstream ofs("/dev/shm/debug_frame.jpg.tmp", std::ios::binary);
ofs.write(reinterpret_cast<const char*>(buf.data()), buf.size());
ofs.close();
std::rename("/dev/shm/debug_frame.jpg.tmp", "/dev/shm/debug_frame.jpg");
}
void drawDebugOverlayShm(const AutoGuidanceDebug& dbg, bool auto_fps) {
static auto last_show_time = std::chrono::steady_clock::now();
const char* shm_name = "/debug_frame";
const size_t shm_max_size = 2 * 1024 * 1024; // 2MB 最大图像编码缓存
if (dbg.img_frame.src_img.empty())
return;
cv::Mat src_img = dbg.img_frame.src_img;
auto now = std::chrono::steady_clock::now();
const double min_interval_ms = 1000.0 / 30.0;
if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
< min_interval_ms
&& auto_fps)
return;
last_show_time = now;
// 复制并转RGB
cv::Mat debug_img;
src_img.convertTo(debug_img, -1, 1, 0);
cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
if (debug_img.empty())
return;
// 绘制内容
drawAutoGuidanceDebugContent(debug_img, dbg);
// 编码为 JPG
std::vector<uchar> buf;
cv::imencode(".jpg", debug_img, buf);
size_t img_size = buf.size();
if (img_size > shm_max_size) {
std::cerr << "[drawDebugOverlayWrite] 图像过大: " << img_size << " bytes\n";
return;
}
// 创建/打开共享内存
int fd = shm_open(shm_name, O_CREAT | O_RDWR, 0666);
if (fd == -1) {
perror("shm_open failed");
return;
}
// 设置共享内存大小
if (ftruncate(fd, shm_max_size) == -1) {
perror("ftruncate failed");
close(fd);
return;
}
// 映射共享内存
void* ptr = mmap(nullptr, shm_max_size, PROT_WRITE, MAP_SHARED, fd, 0);
if (ptr == MAP_FAILED) {
perror("mmap failed");
close(fd);
return;
}
// 写入图像数据
uint32_t size = static_cast<uint32_t>(img_size);
std::memcpy(ptr, &size, 4); // 前4字节写入长度
std::memcpy(static_cast<char*>(ptr) + 4, buf.data(), img_size);
// 关闭映射和文件描述符
munmap(ptr, shm_max_size);
close(fd);
}
void drawDebugOverlayShow(const AutoGuidanceDebug& dbg, bool auto_fps) {
static auto last_show_time = std::chrono::steady_clock::now();
if (dbg.img_frame.src_img.empty())
return;
cv::Mat src_img = dbg.img_frame.src_img;
auto now = std::chrono::steady_clock::now();
const double min_interval_ms = 1000.0 / 30.0;
if (std::chrono::duration<double, std::milli>(now - last_show_time).count()
< min_interval_ms
&& auto_fps)
return;
last_show_time = now;
// 图像构造
cv::Mat debug_img;
src_img.convertTo(debug_img, -1, 1, 0);
cv::cvtColor(debug_img, debug_img, cv::COLOR_BGR2RGB);
if (debug_img.empty())
return;
// 封装后的绘图函数
drawAutoGuidanceDebugContent(debug_img, dbg);
cv::imshow("debug_armor", debug_img);
cv::waitKey(1);
}
void debuglog(const GuidanceTarget& target) {
static bool first_log = true;
static std::chrono::steady_clock::time_point start_time;
static DebugLogs log;
if (first_log) {
start_time = std::chrono::steady_clock::now();
first_log = false;
}
auto now = std::chrono::steady_clock::now();
double t = std::chrono::duration<double>(now - start_time).count();
log.time_log.push_back(t);
double cx_norm = target.center().x / target.image_size_.width * 2.0 - 1.0;
log.cx_log.push_back(cx_norm);
auto trim = [](std::vector<double>& v) {
if (v.size() > 1000)
v.erase(v.begin());
};
trim(log.time_log);
trim(log.cx_log);
nlohmann::json j;
{
j["time"] = log.time_log;
j["yaw"] = log.cx_log;
}
std::ofstream file("/dev/shm/cmd_log.json");
if (file.is_open()) {
file << j.dump();
}
}
} // namespace auto_guidance
} // namespace wust_vision

24
wust_vision-main/tasks/auto_guidance/debug.hpp Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include "tasks/auto_guidance/guidance_tracker/guidance_target.hpp"
#include "wust_vl/video/icamera.hpp"
namespace wust_vision {
namespace auto_guidance {
struct AutoGuidanceDebug {
wust_vl::video::ImageFrame img_frame;
double latency_ms;
GuidanceTarget target;
GreenLights lights;
cv::Mat mask;
};
struct DebugLogs {
std::vector<double> time_log;
std::vector<double> cx_log;
};
void debuglog(const GuidanceTarget& target);
void drawDebugOverlayShm(const AutoGuidanceDebug& dbg, bool auto_fps);
void drawDebugOverlayWrite(const AutoGuidanceDebug& dbg, bool auto_fps);
void drawDebugOverlayShow(const AutoGuidanceDebug& dbg, bool auto_fps);
void drawAutoGuidanceDebugContent(cv::Mat& debug_img, const AutoGuidanceDebug& dbg);
} // namespace auto_guidance
} // namespace wust_vision

18
wust_vision-main/tasks/auto_guidance/guidance_detector/detector_base.hpp Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "tasks/auto_guidance/type.hpp"
#include "tasks/type_common.hpp"
namespace wust_vision {
namespace auto_guidance {
class detector_base {
public:
virtual ~detector_base() = default;
virtual void pushInput(CommonFrame& frame) = 0;
using DetectorCallback =
std::function<void(const std::vector<GreenLight>&, const CommonFrame&)>;
virtual void setCallback(DetectorCallback cb) = 0;
};
} // namespace auto_guidance
} // namespace wust_vision

28
wust_vision-main/tasks/auto_guidance/guidance_detector/detector_factory.hpp Normal file
View File

@@ -0,0 +1,28 @@
#pragma once
#include "tasks/auto_guidance/guidance_detector/opencv/guidance_detector_opencv.hpp"
#ifdef USE_OPENVINO
#include "openvino/guidance_detector_openvino.hpp"
#endif
namespace wust_vision {
namespace auto_guidance {
class DetectorFactory {
public:
static std::unique_ptr<detector_base>
createDetector(const std::string& backend, const YAML::Node& config, bool debug) {
#if defined(USE_OPENVINO)
if (backend == "openvino") {
return std::make_unique<GuidanceDetectorOpenVino>(config);
}
#endif
if (backend == "opencv") {
return std::make_unique<GuidanceDetectorOpenCV>(config, debug);
}
throw std::runtime_error("Unsupported detector backend (or not compiled): " + backend);
}
};
} // namespace auto_guidance
} // namespace wust_vision

100
wust_vision-main/tasks/auto_guidance/guidance_detector/green_light_infer.cpp Normal file
View File

@@ -0,0 +1,100 @@
#include "green_light_infer.hpp"
namespace wust_vision {
namespace auto_guidance {
GreenLightInfer::GreenLightInfer(const Params& params) {
params_ = params;
}
std::vector<GreenLight> GreenLightInfer::postProcess(
const cv::Mat& output_buffer,
const Eigen::Matrix<float, 3, 3>& transform_matrix
) {
std::vector<GreenLight> Lights;
std::vector<float> confidences;
std::vector<cv::Rect> boxes;
const int num_boxes = output_buffer.rows;
const int attr = output_buffer.cols;
for (int i = 0; i < num_boxes; ++i) {
float confidence = output_buffer.at<float>(i, 4);
if (confidence < params_.conf_threshold)
continue;
cv::Mat class_scores = output_buffer.row(i).colRange(5, 5 + 9);
cv::Mat color_scores = output_buffer.row(i).colRange(5 + 9, 5 + 9 + 4);
double maxClassConfidence;
cv::Point classIdPoint;
cv::minMaxLoc(class_scores, nullptr, &maxClassConfidence, nullptr, &classIdPoint);
if (maxClassConfidence < params_.conf_threshold)
continue;
if (classIdPoint.x != 8)
continue;
float cx = output_buffer.at<float>(i, 0);
float cy = output_buffer.at<float>(i, 1);
float w = output_buffer.at<float>(i, 2);
float h = output_buffer.at<float>(i, 3);
// === coordinate transform ===
Eigen::Vector3f pt(cx, cy, 1.0f);
Eigen::Vector3f pt_trans = transform_matrix * pt;
float cx_t = pt_trans(0);
float cy_t = pt_trans(1);
// compute scale for bbox
float scale_x = std::sqrt(transform_matrix.row(0).head<2>().squaredNorm());
float scale_y = std::sqrt(transform_matrix.row(1).head<2>().squaredNorm());
float w_t = w * scale_x;
float h_t = h * scale_y;
cv::Rect2d bbox(cx_t - w_t / 2.0f, cy_t - h_t / 2.0f, w_t, h_t);
GreenLight light;
light.id = classIdPoint.x;
light.score = confidence;
light.center_point = cv::Point2f(cx_t, cy_t);
light.box = bbox;
Lights.emplace_back(light);
confidences.emplace_back(confidence);
boxes.emplace_back(bbox);
}
std::vector<int> nms_result;
cv::dnn::NMSBoxes(
boxes,
confidences,
params_.conf_threshold,
params_.nms_threshold,
nms_result,
0.5f,
params_.top_k
);
auto IoU = [](const cv::Rect2d& a, const cv::Rect2d& b) {
double inter = (a & b).area();
double uni = a.area() + b.area() - inter;
return inter / uni;
};
std::vector<GreenLight> final_result;
for (int i = 0; i < nms_result.size(); i++) {
bool keep = true;
for (int j = 0; j < final_result.size(); j++) {
if (IoU(final_result[j].box, Lights[nms_result[i]].box) > 0.3) {
keep = false;
break;
}
}
if (keep)
final_result.push_back(Lights[nms_result[i]]);
}
return final_result;
}
} // namespace auto_guidance
} // namespace wust_vision

35
wust_vision-main/tasks/auto_guidance/guidance_detector/green_light_infer.hpp Normal file
View File

@@ -0,0 +1,35 @@
#pragma once
#include "tasks/auto_guidance/type.hpp"
namespace wust_vision {
namespace auto_guidance {
class GreenLightInfer {
public:
using GreenLightInferPtr = std::unique_ptr<GreenLightInfer>;
struct Params {
int input_w;
int input_h;
float conf_threshold;
float nms_threshold;
int top_k;
bool use_norm;
} params_;
GreenLightInfer(const Params& params);
static inline GreenLightInferPtr makeGreenLightInfer(const Params& params) {
return std::make_unique<GreenLightInfer>(params);
}
std::vector<GreenLight> postProcess(
const cv::Mat& output_buffer,
const Eigen::Matrix<float, 3, 3>& transform_matrix
);
int getInputW() {
return params_.input_w;
}
int getInputH() {
return params_.input_h;
}
bool getUseNorm() {
return params_.use_norm;
}
};
} // namespace auto_guidance
} // namespace wust_vision

162
wust_vision-main/tasks/auto_guidance/guidance_detector/opencv/guidance_detector_opencv.cpp Normal file
View File

@@ -0,0 +1,162 @@
#include "guidance_detector_opencv.hpp"
bool initializing = true;
int lowH = 35, highH = 85;
int lowS = 50, highS = 255;
int lowV = 80, highV = 255;
static void onTrackbar(int, void*) {
if (initializing)
return; // 初始化阶段不更新
lowH = cv::getTrackbarPos("LowH", "mask");
highH = cv::getTrackbarPos("HighH", "mask");
lowS = cv::getTrackbarPos("LowS", "mask");
highS = cv::getTrackbarPos("HighS", "mask");
lowV = cv::getTrackbarPos("LowV", "mask");
highV = cv::getTrackbarPos("HighV", "mask");
}
void initGUI() {
cv::namedWindow("mask", cv::WINDOW_NORMAL); // 允许调整大小
cv::resizeWindow("mask", 400, 600); // 设置初始窗口大小
cv::createTrackbar("LowH", "mask", nullptr, 179, onTrackbar);
cv::createTrackbar("HighH", "mask", nullptr, 179, onTrackbar);
cv::createTrackbar("LowS", "mask", nullptr, 255, onTrackbar);
cv::createTrackbar("HighS", "mask", nullptr, 255, onTrackbar);
cv::createTrackbar("LowV", "mask", nullptr, 255, onTrackbar);
cv::createTrackbar("HighV", "mask", nullptr, 255, onTrackbar);
cv::setTrackbarPos("LowH", "mask", lowH);
cv::setTrackbarPos("HighH", "mask", highH);
cv::setTrackbarPos("LowS", "mask", lowS);
cv::setTrackbarPos("HighS", "mask", highS);
cv::setTrackbarPos("LowV", "mask", lowV);
cv::setTrackbarPos("HighV", "mask", highV);
initializing = false;
}
namespace wust_vision {
namespace auto_guidance {
struct GuidanceDetectorOpenCV::Impl {
public:
Impl(const YAML::Node& config_gobal, bool debug) {
debug_ = debug;
const auto config = config_gobal["opencv"];
lowH = config["HSV"]["lowH"].as<int>();
highH = config["HSV"]["highH"].as<int>();
lowS = config["HSV"]["lowS"].as<int>();
highS = config["HSV"]["highS"].as<int>();
highV = config["HSV"]["highV"].as<int>();
lowV = config["HSV"]["lowV"].as<int>();
max_area_ = config["contours"]["max_area"].as<double>();
min_area_ = config["contours"]["min_area"].as<double>();
min_aspect_ratio = config["contours"]["min_aspect_ratio"].as<double>();
min_fill_ratio_ = config["contours"]["min_fill_ratio"].as<double>();
use_gui_ = config["gui"].as<bool>();
if (debug_ && use_gui_) {
initGUI();
}
}
void setCallback(DetectorCallback callback) {
infer_callback_ = callback;
}
void processCallback(const CommonFrame& frame) {
std::vector<GreenLight> lights;
cv::Mat img = frame.img_frame.src_img.clone();
cv::Mat hsv;
cv::cvtColor(img, hsv, cv::COLOR_BGR2HSV);
cv::Scalar lower_green(lowH, lowS, lowV);
cv::Scalar upper_green(highH, highS, highV);
cv::Mat mask;
cv::inRange(hsv, lower_green, upper_green, mask);
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
cv::morphologyEx(mask, mask, cv::MORPH_OPEN, kernel);
cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel);
std::vector<std::vector<cv::Point>> contours;
cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
std::vector<int> valid_indices;
for (size_t i = 0; i < contours.size(); i++) {
const double area = cv::contourArea(contours[i]);
const double perimeter = cv::arcLength(contours[i], true);
if (perimeter == 0)
continue;
const double circularity = 4 * CV_PI * area / (perimeter * perimeter);
const cv::RotatedRect rRect = cv::minAreaRect(contours[i]);
const double width = rRect.size.width;
const double height = rRect.size.height;
if (width <= 0 || height <= 0)
continue;
const double rect_area = width * height;
const double fill_ratio = area / rect_area;
const double aspect_ratio = std::min(width, height) / std::max(width, height);
if (area > min_area_ && area < max_area_ && fill_ratio > min_fill_ratio_
&& aspect_ratio > min_aspect_ratio)
{
cv::Point2f center;
float radius;
cv::minEnclosingCircle(contours[i], center, radius);
GreenLight gl;
gl.center_point = center;
gl.box = cv::boundingRect(contours[i]);
gl.score = circularity;
gl.radius = radius;
lights.push_back(gl);
}
}
static auto last = std::chrono::steady_clock::now();
const auto now = std::chrono::steady_clock::now();
const double dt = std::chrono::duration<double, std::milli>(now - last).count();
if (debug_ && dt > 33.3 && use_gui_) { // 30Hz 刷新
cv::imshow("mask", mask);
cv::waitKey(1); // 非阻塞
last = now;
}
if (infer_callback_) {
infer_callback_(lights, frame);
}
}
void pushInput(CommonFrame& frame) {
frame.id = current_id_++;
processCallback(frame);
}
DetectorCallback infer_callback_;
int current_id_ = 0;
double min_area_ = 100;
double max_area_ = 10000;
double min_fill_ratio_ = 0.5;
double min_aspect_ratio = 0.7;
bool debug_ = false;
bool use_gui_ = false;
};
GuidanceDetectorOpenCV::GuidanceDetectorOpenCV(const YAML::Node& config_gobal, bool debug) {
_impl = std::make_unique<Impl>(config_gobal, debug);
}
GuidanceDetectorOpenCV::~GuidanceDetectorOpenCV() {
_impl.reset();
}
void GuidanceDetectorOpenCV::pushInput(CommonFrame& frame) {
_impl->pushInput(frame);
}
void GuidanceDetectorOpenCV::setCallback(DetectorCallback callback) {
_impl->setCallback(callback);
}
} // namespace auto_guidance
} // namespace wust_vision

18
wust_vision-main/tasks/auto_guidance/guidance_detector/opencv/guidance_detector_opencv.hpp Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
#include "tasks/auto_guidance/guidance_detector/detector_base.hpp"
namespace wust_vision {
namespace auto_guidance {
class GuidanceDetectorOpenCV: public detector_base {
public:
GuidanceDetectorOpenCV(const YAML::Node& config, bool debug);
~GuidanceDetectorOpenCV();
void pushInput(CommonFrame& frame) override;
void setCallback(DetectorCallback callback) override;
private:
struct Impl;
std::unique_ptr<Impl> _impl;
};
} // namespace auto_guidance
} // namespace wust_vision

116
wust_vision-main/tasks/auto_guidance/guidance_detector/openvino/guidance_detector_openvino.cpp Normal file
View File

@@ -0,0 +1,116 @@
#ifdef USE_OPENVINO
#include "guidance_detector_openvino.hpp"
#include "tasks/auto_guidance/guidance_detector/green_light_infer.hpp"
#include "tasks/utils/utils.hpp"
#include "wust_vl/ml_net/openvino/openvino_net.hpp"
namespace wust_vision {
namespace auto_guidance {
struct GuidanceDetectorOpenVino::Impl {
public:
Impl(const YAML::Node& config_gobal) {
auto config = config_gobal["openvino"];
std::string model_path = utils::expandEnv(config["model_path"].as<std::string>());
std::string device_name = config["device_type"].as<std::string>();
int top_k = config["top_k"].as<int>();
float nms_threshold = config["nms_threshold"].as<float>();
float conf_threshold = config["conf_threshold"].as<float>();
green_light_infer_ = GreenLightInfer::makeGreenLightInfer(GreenLightInfer::Params {
.input_w = 640,
.input_h = 384,
.conf_threshold = conf_threshold,
.nms_threshold = nms_threshold,
.top_k = top_k,
.use_norm = true });
openvino_net_ = std::make_unique<wust_vl::ml_net::OpenvinoNet>();
auto ppp_init_fun = [this](ov::preprocess::PrePostProcessor& ppp) {
ppp.input()
.tensor()
.set_element_type(ov::element::u8)
.set_layout("NHWC")
.set_color_format(ov::preprocess::ColorFormat::BGR);
ppp.input()
.preprocess()
.convert_element_type(ov::element::f32)
.convert_color(ov::preprocess::ColorFormat::RGB)
.scale(255.f);
ppp.input().model().set_layout("NCHW");
ppp.output(0).tensor().set_element_type(ov::element::f32);
ppp.output(1).tensor().set_element_type(ov::element::f32);
ppp.output(2).tensor().set_element_type(ov::element::f32);
};
wust_vl::ml_net::OpenvinoNet::Params params;
params.model_path = model_path;
params.device_name = device_name;
params.mode = config["use_throughputmode"].as<bool>()
? ov::hint::PerformanceMode::THROUGHPUT
: ov::hint::PerformanceMode::LATENCY;
openvino_net_->init(params, ppp_init_fun);
}
~Impl() {
openvino_net_.reset();
green_light_infer_.reset();
}
void setCallback(DetectorCallback callback) {
infer_callback_ = callback;
}
void processCallback(const CommonFrame& frame) const {
Eigen::Matrix3f transform_matrix;
const cv::Mat resized_img = utils::letterbox(
frame.img_frame.src_img,
transform_matrix,
green_light_infer_->getInputW(),
green_light_infer_->getInputH()
);
const auto input_info = openvino_net_->getInputInfo();
const auto input_tensor =
ov::Tensor(input_info.first, input_info.second, resized_img.data);
auto infer_request = openvino_net_->createInferRequest();
infer_request.set_input_tensor(input_tensor);
infer_request.infer();
const auto output = infer_request.get_output_tensor(0);
// Process output data
const auto output_shape = output.get_shape();
const float* ptr = output.data<const float>();
cv::Mat
output_buffer(output_shape[1], output_shape[2], CV_32F, const_cast<float*>(ptr));
const auto objs_result =
green_light_infer_->postProcess(output_buffer, transform_matrix);
if (infer_callback_) {
infer_callback_(objs_result, frame);
}
}
void pushInput(CommonFrame& frame) {
frame.id = current_id_++;
processCallback(frame);
}
std::unique_ptr<wust_vl::ml_net::OpenvinoNet> openvino_net_;
std::unique_ptr<GreenLightInfer> green_light_infer_;
DetectorCallback infer_callback_;
int current_id_ = 0;
};
GuidanceDetectorOpenVino::GuidanceDetectorOpenVino(const YAML::Node& config_gobal) {
_impl = std::make_unique<Impl>(config_gobal);
}
GuidanceDetectorOpenVino::~GuidanceDetectorOpenVino() {
_impl.reset();
}
void GuidanceDetectorOpenVino::pushInput(CommonFrame& frame) {
_impl->pushInput(frame);
}
void GuidanceDetectorOpenVino::setCallback(DetectorCallback callback) {
_impl->setCallback(callback);
}
} // namespace auto_guidance
} // namespace wust_vision
#endif

19
wust_vision-main/tasks/auto_guidance/guidance_detector/openvino/guidance_detector_openvino.hpp Normal file
View File

@@ -0,0 +1,19 @@
#pragma once
#include "tasks/auto_guidance/guidance_detector/detector_base.hpp"
namespace wust_vision {
namespace auto_guidance {
class GuidanceDetectorOpenVino: public detector_base {
public:
GuidanceDetectorOpenVino(const YAML::Node& config);
~GuidanceDetectorOpenVino();
void pushInput(CommonFrame& frame) override;
void setCallback(DetectorCallback callback) override;
private:
struct Impl;
std::unique_ptr<Impl> _impl;
};
} // namespace auto_guidance
} // namespace wust_vision

143
wust_vision-main/tasks/auto_guidance/guidance_tracker/guidance_target.cpp Normal file
View File

@@ -0,0 +1,143 @@
#include "guidance_target.hpp"
namespace wust_vision {
namespace auto_guidance {
GuidanceTarget::GuidanceTarget() {
target_state_ = Eigen::VectorXd::Zero(imgbox_model::X_N);
}
GuidanceTarget::GuidanceTarget(const GreenLight& light, TargetConfig target_config) {
target_config_ = target_config;
auto yfv2 = imgbox_model::Predict(0.01);
auto yhv2 = imgbox_model::Measure();
auto yu_qv2 = [this]() { return computeProcessNoise(0.01); };
auto yu_rv2 = [this](const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) {
return this->computeMeasurementCovariance(z);
};
Eigen::DiagonalMatrix<double, imgbox_model::X_N> p0;
p0.diagonal() << 1000, 1000, 1000, 1000, 64000, 64000, 64000, 64000;
esekf_ = imgbox_model::BBox8ESEKF(yfv2, yhv2, yu_qv2, yu_rv2, p0);
esekf_.setResidualFunc([this](
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z_pred,
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z
) {
Eigen::Matrix<double, imgbox_model::Z_N, 1> r = z - z_pred;
return r;
});
esekf_.setIterationNum(target_config_.iter_num);
esekf_.setInjectFunc([this](
const Eigen::Matrix<double, imgbox_model::X_N, 1>& delta,
Eigen::Matrix<double, imgbox_model::X_N, 1>& nominal
) {
for (int i = 0; i < imgbox_model::X_N; i++) {
nominal[i] += delta[i];
}
});
double cx = light.center_point.x;
double cy = light.center_point.y;
double w = light.box.width;
double h = light.box.height;
target_state_ << cx, 0, cy, 0, w, 0, h, 0;
esekf_.setState(target_state_);
last_t_ = light.timestamp;
position_ = light.position;
timestamp_ = light.timestamp;
image_size_ = light.image_size;
is_inited_ = true;
}
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N>
GuidanceTarget::computeMeasurementCovariance(
const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z
) const {
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N> r;
// clang-format off
r <<target_config_.xy_r, 0, 0, 0,
0, target_config_.xy_r , 0, 0,
0, 0, target_config_.wh_r, 0,
0, 0, 0,target_config_.wh_r;
// clang-format on
return r;
}
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N>
GuidanceTarget::computeProcessNoise(double dt) const {
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N> q;
double t = dt;
double q_pp = pow(t, 4) / 4.0 * target_config_.q_xy;
double q_pv = pow(t, 3) / 2.0 * target_config_.q_xy;
double q_vv = pow(t, 2) * target_config_.q_xy;
double q_ss = pow(t, 4) / 4.0 * target_config_.q_wh;
double q_sv = pow(t, 3) / 2.0 * target_config_.q_wh;
double q_vvs = pow(t, 2) * target_config_.q_wh;
// clang-format off
// cx vx cy vy w vw h vh
q << q_pp, q_pv, 0, 0, 0, 0, 0, 0,
q_pv, q_vv, 0, 0, 0, 0, 0, 0,
0, 0, q_pp, q_pv, 0, 0, 0, 0,
0, 0, q_pv, q_vv, 0, 0, 0, 0,
0, 0, 0, 0, q_ss, q_sv, 0, 0,
0, 0, 0, 0, q_sv, q_vvs, 0, 0,
0, 0, 0, 0, 0, 0, q_ss, q_sv,
0, 0, 0, 0, 0, 0, q_sv, q_vvs;
// clang-format on
return q;
}
void GuidanceTarget::predict(std::chrono::steady_clock::time_point t) {
double dt = wust_vl::common::utils::time_utils::durationSec(last_t_, t);
predict(dt);
last_t_ = t;
}
void GuidanceTarget::predict(double dt) {
dt_ = dt;
esekf_.setPredictFunc(imgbox_model::Predict { dt });
auto yu_qv2 = [dt, this]() { return computeProcessNoise(dt); };
esekf_.setUpdateQ(yu_qv2);
target_state_ = esekf_.predict();
}
bool GuidanceTarget::update(const GreenLights& lights) {
auto ls = lights.lights;
timestamp_ = lights.timestamp;
auto yu_rv2 = [this](const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) {
return this->computeMeasurementCovariance(z);
};
esekf_.setUpdateR(yu_rv2);
int best_id = -1;
double min_error = std::numeric_limits<double>::max();
for (int i = 0; i < ls.size(); i++) {
double centor_error = cv::norm(ls[i].center_point - center());
double pos_error = (ls[i].position - position_).norm();
if (centor_error < min_error && pos_error < target_config_.max_dis_diff) {
min_error = centor_error;
best_id = i;
}
}
if (best_id == -1) {
return false;
}
measurement_ = Eigen::Vector4d(
ls[best_id].center_point.x,
ls[best_id].center_point.y,
ls[best_id].box.width,
ls[best_id].box.height
);
esekf_.setMeasureFunc(imgbox_model::Measure());
target_state_ = esekf_.update(measurement_);
position_ = ls[best_id].position;
image_size_ = ls[best_id].image_size;
last_t_ = timestamp_;
return true;
}
} // namespace auto_guidance
} // namespace wust_vision

89
wust_vision-main/tasks/auto_guidance/guidance_tracker/guidance_target.hpp Normal file
View File

@@ -0,0 +1,89 @@
#pragma once
#include "motion_models/imgbox_model.hpp"
#include "tasks/auto_guidance/type.hpp"
#include <wust_vl/common/utils/timer.hpp>
#include <yaml-cpp/yaml.h>
namespace wust_vision {
namespace auto_guidance {
struct TargetConfig {
void load(const YAML::Node& config) {
xy_r = config["xy_r"].as<double>();
wh_r = config["wh_r"].as<double>();
q_xy = config["q_xy"].as<double>();
q_wh = config["q_wh"].as<double>();
iter_num = config["iter_num"].as<int>();
max_dis_diff = config["max_dis_diff"].as<double>();
}
double xy_r = 0.05;
double wh_r = 0.05;
double q_xy = 10;
double q_wh = 10;
int iter_num = 2;
double max_dis_diff = 2.0;
};
class GuidanceTarget {
public:
GuidanceTarget();
GuidanceTarget(const GreenLight& light, TargetConfig target_config);
GuidanceTarget& operator=(const GuidanceTarget&) = default;
bool update(const GreenLights& lights);
void predict(std::chrono::steady_clock::time_point t);
void predict(double dt);
Eigen::Matrix<double, imgbox_model::Z_N, imgbox_model::Z_N>
computeMeasurementCovariance(const Eigen::Matrix<double, imgbox_model::Z_N, 1>& z) const;
Eigen::Matrix<double, imgbox_model::X_N, imgbox_model::X_N> computeProcessNoise(double dt
) const;
std::chrono::steady_clock::time_point last_t_;
std::chrono::steady_clock::time_point timestamp_;
double dt_;
cv::Size2d image_size_;
imgbox_model::BBox8ESEKF esekf_;
Eigen::Matrix<double, imgbox_model::Z_N, 1> measurement_ =
Eigen::Matrix<double, imgbox_model::Z_N, 1>::Zero();
Eigen::Matrix<double, imgbox_model::X_N, 1> target_state_ =
Eigen::Matrix<double, imgbox_model::X_N, 1>::Zero();
Eigen::Vector3d position_;
TargetConfig target_config_;
bool is_inited_ = false;
bool is_tracking_ = false;
bool checkappear() {
return is_tracking_
&& wust_vl::common::utils::time_utils::durationSec(
timestamp_,
wust_vl::common::utils::time_utils::now()
)
< 3.0;
}
cv::Point2d center() const {
return cv::Point2d(target_state_(0), target_state_(2));
}
cv::Rect2d box() const {
return cv::Rect2d(
target_state_(0) - target_state_(4) / 2,
target_state_(2) - target_state_(6) / 2,
target_state_(4),
target_state_(6)
);
}
void draw(cv::Mat& img) const {
cv::rectangle(img, box(), cv::Scalar(255, 50, 0), 2);
cv::circle(img, center(), 3, cv::Scalar(255, 255, 255), -1);
cv::line(
img,
cv::Point(center().x, center().y),
cv::Point(img.cols / 2.0, center().y),
cv::Scalar(0, 0, 255),
2
);
cv::line(
img,
cv::Point2f(img.cols / 2.0, 0),
cv::Point2f(img.cols / 2.0, img.rows),
cv::Scalar(255, 255, 255),
2
);
}
};
} // namespace auto_guidance
} // namespace wust_vision

109
wust_vision-main/tasks/auto_guidance/guidance_tracker/guidance_tracker.cpp Normal file
View File

@@ -0,0 +1,109 @@
#include "guidance_tracker.hpp"
namespace wust_vision {
namespace auto_guidance {
struct GuidanceTracker::Impl {
public:
Impl(const YAML::Node& config) {
target_config_.load(config["target"]);
tracking_thres_ = config["tracking_thres"].as<int>(5);
lost_dt_ = config["lost_time_thres"].as<double>();
}
GuidanceTarget track(const GreenLights& lights) {
double dt = std::chrono::duration<double>(lights.timestamp - last_time_).count();
last_time_ = lights.timestamp;
lost_thres_ = std::abs(static_cast<int>(lost_dt_ / dt));
bool found;
if (tracker_state == LOST) {
found = initTarget(lights);
} else {
found = updateTarget(lights);
}
updateFsm(found);
return guidance_target_;
}
void updateFsm(bool found) {
if (tracker_state == DETECTING) {
if (found) {
detect_count_++;
if (detect_count_ > tracking_thres_) {
detect_count_ = 0;
tracker_state = TRACKING;
}
} else {
detect_count_ = 0;
tracker_state = LOST;
}
} else if (tracker_state == TRACKING) {
if (!found) {
tracker_state = TEMP_LOST;
lost_count_++;
}
} else if (tracker_state == TEMP_LOST) {
if (!found) {
lost_count_++;
if (lost_count_ > lost_thres_) {
lost_count_ = 0;
tracker_state = LOST;
}
} else {
tracker_state = TRACKING;
lost_count_ = 0;
}
}
if (tracker_state == LOST || tracker_state == DETECTING) {
guidance_target_.is_tracking_ = false;
} else {
guidance_target_.is_tracking_ = true;
}
}
bool initTarget(const GreenLights& lights) {
int best_id = -1;
double max_score = -1e9;
for (int i = 0; i < lights.lights.size(); i++) {
if (lights.lights[i].score > max_score) {
max_score = lights.lights[i].score;
best_id = i;
}
}
if (best_id == -1) {
return false;
}
tracker_state = DETECTING;
guidance_target_ = GuidanceTarget(lights.lights[best_id], target_config_);
return true;
}
bool updateTarget(const GreenLights& lights) {
guidance_target_.predict(lights.timestamp);
return guidance_target_.update(lights);
}
enum State {
LOST,
DETECTING,
TRACKING,
TEMP_LOST,
} tracker_state = LOST;
GuidanceTarget guidance_target_;
int tracking_thres_;
int lost_thres_;
int detect_count_ = 0;
int lost_count_ = 0;
double lost_dt_;
std::chrono::steady_clock::time_point last_time_;
TargetConfig target_config_;
};
GuidanceTracker::GuidanceTracker(const YAML::Node& config) {
_impl = std::make_unique<Impl>(config);
}
GuidanceTracker::~GuidanceTracker() {
_impl.reset();
}
GuidanceTarget GuidanceTracker::track(const GreenLights& lights) {
return _impl->track(lights);
}
std::chrono::steady_clock::time_point GuidanceTracker::getLastTime() const {
return _impl->last_time_;
}
} // namespace auto_guidance
} // namespace wust_vision

21
wust_vision-main/tasks/auto_guidance/guidance_tracker/guidance_tracker.hpp Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#include "tasks/auto_guidance/guidance_tracker/guidance_target.hpp"
namespace wust_vision {
namespace auto_guidance {
class GuidanceTracker {
public:
using Ptr = std::unique_ptr<GuidanceTracker>;
GuidanceTracker(const YAML::Node& config);
~GuidanceTracker();
static inline Ptr create(const YAML::Node& config) {
return std::make_unique<GuidanceTracker>(config);
}
GuidanceTarget track(const GreenLights& lights);
std::chrono::steady_clock::time_point getLastTime() const;
private:
struct Impl;
std::unique_ptr<Impl> _impl;
};
} // namespace auto_guidance
} // namespace wust_vision

45
wust_vision-main/tasks/auto_guidance/guidance_tracker/motion_models/imgbox_model.hpp Normal file
View File

@@ -0,0 +1,45 @@
#pragma once
#include "KalmanHyLib/kalman_hybird_lib.hpp"
#include <ceres/ceres.h>
namespace imgbox_model {
static constexpr int X_N = 8; // cx vx cy vy w vw h vh
static constexpr int Z_N = 4; // measured cx cy w h
// ========================== Predict Model ==========================
struct Predict {
Predict() = default;
explicit Predict(double dt): dt_(dt) {}
template<typename T>
void operator()(const T x0[X_N], T x1[X_N]) {
for (int i = 0; i < X_N; i++) {
x1[i] = x0[i];
}
x1[0] += x0[1] * dt_; // cx
x1[2] += x0[3] * dt_; // cy
x1[4] += x0[5] * dt_; // w
x1[6] += x0[7] * dt_; // h
}
double dt_;
};
// ========================== Measurement Model ==========================
struct Measure {
Measure() = default;
template<typename T>
void operator()(const T x[X_N], T z[Z_N]) const {
z[0] = x[0]; // cx
z[1] = x[2]; // cy
z[2] = x[4]; // w
z[3] = x[6]; // h
}
};
using BBox8EKF = kalman_hybird_lib::ExtendedKalmanFilter<X_N, Z_N, Predict, Measure>;
using BBox8ESEKF = kalman_hybird_lib::ErrorStateEKF<X_N, Z_N, Predict, Measure>;
} // namespace imgbox_model

107
wust_vision-main/tasks/auto_guidance/type.hpp Normal file
View File

@@ -0,0 +1,107 @@
#pragma once
#include "Eigen/Dense"
#include "opencv2/opencv.hpp"
namespace wust_vision {
namespace auto_guidance {
struct GreenLight {
int id = -1;
double score = 0.;
cv::Rect2d box; // bounding box in pixel coordinates
cv::Point2d center_point; // center in pixel coordinates
double radius;
Eigen::Vector3d position;
std::chrono::steady_clock::time_point timestamp;
cv::Size2d image_size;
// PnP 估计位姿
bool solvePnP(const cv::Mat& K, const cv::Mat& distCoeffs) {
constexpr float half_w = 0.07;
// 真实世界点，单位米
std::vector<cv::Point3f> objectPoints = { { -half_w, -half_w, 0.f },
{ half_w, -half_w, 0.f },
{ half_w, half_w, 0.f },
{ -half_w, half_w, 0.f } };
// 像素点
std::vector<cv::Point2f> imagePoints = {
cv::Point2f(box.x, box.y),
cv::Point2f(box.x + box.width, box.y),
cv::Point2f(box.x + box.width, box.y + box.height),
cv::Point2f(box.x, box.y + box.height)
};
cv::Mat rvec, tvec;
bool ok = cv::solvePnP(
objectPoints,
imagePoints,
K,
distCoeffs,
rvec,
tvec,
false,
cv::SOLVEPNP_ITERATIVE
);
if (!ok)
return false;
// 转换到 Eigen 向量
position = Eigen::Vector3d(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
return true;
}
};
struct GreenLights {
public:
std::vector<GreenLight> lights;
std::chrono::steady_clock::time_point timestamp;
int id;
void drawFront(cv::Mat& img) const {
for (const auto& light: lights) {
cv::rectangle(img, light.box, cv::Scalar(0, 255, 255), 2);
cv::circle(img, light.center_point, light.radius, cv::Scalar(0, 255, 0), 2);
cv::circle(img, light.center_point, 3, cv::Scalar(255, 0, 0), -1);
cv::putText(
img,
std::to_string(light.score),
light.center_point
+ cv::Point2d(light.box.width / 2.0, -light.box.height / 2.0),
cv::FONT_HERSHEY_SIMPLEX,
0.5,
cv::Scalar(255, 0, 0),
2
);
cv::putText(
img,
std::to_string(light.position.norm()),
light.center_point + cv::Point2d(light.box.width / 2.0, light.box.height / 2.0),
cv::FONT_HERSHEY_SIMPLEX,
0.5,
cv::Scalar(255, 255, 255),
2
);
}
}
void drawBack(cv::Mat& img) const {
for (const auto& light: lights) {
cv::line(
img,
cv::Point(light.center_point.x, light.center_point.y),
cv::Point(img.cols / 2.0, light.center_point.y),
cv::Scalar(0, 0, 255),
2
);
}
cv::line(
img,
cv::Point2f(img.cols / 2.0, 0),
cv::Point2f(img.cols / 2.0, img.rows),
cv::Scalar(255, 255, 255),
2
);
}
};
} // namespace auto_guidance
} // namespace wust_vision