diff --git a/Cpp_example/A05_USART/USART_Read.cc b/Cpp_example/A05_USART/USART_Read.cc index 4d4230e8b04e1d39fb02824820ad126121761580..741b72bb8d94994ec2a3392e02cb3ce8f1c2a39d 100755 --- a/Cpp_example/A05_USART/USART_Read.cc +++ b/Cpp_example/A05_USART/USART_Read.cc @@ -2,22 +2,18 @@ #include -int main() -{ +int main() { lockzhiner_vision_module::periphery::USART1 usart; - if (!usart.Open(115200)) - { + if (!usart.Open(115200)) { std::cout << "Failed to open usart." << std::endl; return 1; } std::cout << "Start receiving serial port data." << std::endl; - while (1) - { + while (1) { std::string buffer; usart.Read(buffer, 1024); - if (!buffer.empty()) - { + if (!buffer.empty()) { std::cout << buffer << std::endl; } } diff --git a/Cpp_example/A05_USART/USART_Write.cc b/Cpp_example/A05_USART/USART_Write.cc index 0aba4e034ca10fe22365fdfcae2a77626c719ff0..bedca0cac67759df154ebfca7b33f06350b620ca 100755 --- a/Cpp_example/A05_USART/USART_Write.cc +++ b/Cpp_example/A05_USART/USART_Write.cc @@ -2,17 +2,14 @@ #include -int main() -{ +int main() { lockzhiner_vision_module::periphery::USART1 usart; - if (!usart.Open(115200)) - { + if (!usart.Open(115200)) { std::cout << "Failed to open usart." << std::endl; return 1; } - if (!usart.Write("Hello World\n")) - { + if (!usart.Write("Hello World\n")) { std::cout << "Failed to send data." << std::endl; return 1; } diff --git a/Cpp_example/D01_test_detection/CMakeLists.txt b/Cpp_example/D01_test_detection/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/Cpp_example/D13_target_tracking/CMakeLists.txt b/Cpp_example/D13_target_tracking/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..70194f2caa4b177f288dfcdf3c61aa1b47d4d4d6 --- /dev/null +++ b/Cpp_example/D13_target_tracking/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.10) + +project(D01_test_detection) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# 定义项目根目录路径 +set(PROJECT_ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../..") +message("PROJECT_ROOT_PATH = " ${PROJECT_ROOT_PATH}) + +include("${PROJECT_ROOT_PATH}/toolchains/arm-rockchip830-linux-uclibcgnueabihf.toolchain.cmake") + +# 定义 OpenCV SDK 路径 +set(OpenCV_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/opencv-mobile-4.10.0-lockzhiner-vision-module") +set(OpenCV_DIR "${OpenCV_ROOT_PATH}/lib/cmake/opencv4") +find_package(OpenCV REQUIRED) +set(OPENCV_LIBRARIES "${OpenCV_LIBS}") + +# 定义 LockzhinerVisionModule SDK 路径 +set(LockzhinerVisionModule_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/lockzhiner_vision_module_sdk") +set(LockzhinerVisionModule_DIR "${LockzhinerVisionModule_ROOT_PATH}/lib/cmake/lockzhiner_vision_module") +find_package(LockzhinerVisionModule REQUIRED) + +add_executable(Test-target-tracking test_target_tracking.cc) +target_include_directories(Test-target-tracking PRIVATE ${LOCKZHINER_VISION_MODULE_INCLUDE_DIRS}) +target_link_libraries(Test-target-tracking PRIVATE ${OPENCV_LIBRARIES} ${LOCKZHINER_VISION_MODULE_LIBRARIES}) + +install( + TARGETS Test-target-tracking + RUNTIME DESTINATION . +) \ No newline at end of file diff --git a/Cpp_example/D13_target_tracking/README.md b/Cpp_example/D13_target_tracking/README.md new file mode 100755 index 0000000000000000000000000000000000000000..462ebbcff6c9cbc218e67b5e0eb8f5f15fddf73e --- /dev/null +++ b/Cpp_example/D13_target_tracking/README.md @@ -0,0 +1,876 @@ +# 多目标跟踪我能当 + +本文档介绍了如何使用 lockzhiner_vision_module 库结合卡尔曼滤波和匈牙利算法实现多目标跟踪系统。 + +## 1. 基础知识讲解 + +### 1.1 多目标跟踪的基本概念 + +多目标跟踪(MOT)是计算机视觉中处理视频序列中移动目标检测、跟踪和识别的任务。它的核心挑战包括: + +- 数据关联:将当前帧的检测结果与现有轨迹匹配 +- 轨迹管理:处理新目标的出现、旧目标的消失 +- 遮挡处理:处理目标被遮挡或暂时离开画面的情况 + +### 1.2 卡尔曼滤波 + +卡尔曼滤波是一种高效的递归算法,用于在动态系统中估计目标的状态: + +- ​​预测阶段​​:基于运动模型估计目标在下一帧的位置 + +- ​更新阶段​​:使用新的观测值更新状态估计 + +- 在多目标跟踪中,卡尔曼滤波用于预测目标位置、速度和其他状态变量 + +### 1.3 匈牙利算法 + +匈牙利算法是解决二分图最大匹配问题的经典算法,再MOT中的作用: + +- 将检测到的目标与现有的跟踪轨迹关联 +- 解决分配问题,找到最优的检测轨迹匹配 + +## 2. API 文档 + +### 2.1 EnhancedKalmanFilter 类 + +#### 2.1.1 构造函数 + +```cpp +EnhancedKalmanFilter(float Ts = 0.033f); +``` + +- 作用:创建6D卡尔曼滤波器对象 +- 参数: + - Ts:时间步长(默认0.033秒) +- 初始化状态: + - 状态向量:[x, y, w, h, dx, dy] (位置+尺寸+速度) + - 初始化过程噪声、观测噪声矩阵 + +#### 2.1.2 主要成员函数 + +```cpp +void Update(const cv::Mat& Z); +``` + +- 作用:更新卡尔曼滤波器状态 +- 参数: + - z:观测向量[x,y,w,h] + +```cpp +void Predict(); +``` + +- 作用:预测下一时刻的状态 + +```cpp +cv::Rect GetRectFromState() const; +``` + +- 作用:获取目标边界框 +- 返回值:当前状态的矩形区域 + +```cpp +cv::Point2d GetVelocity() const; +``` + +- 作用:获取目标速度向量 +- 返回值:速度向量(dx,dy) + +### 2.2 HungarianAlgorithm 类 + +#### 2.2.1 静态成员函数 + +```cpp +static std::vector Solve(const std::vector>& costMatrix); +``` + +- 作用:解决检测- 轨迹匹配问题 +- 参数: + - costMatrix:代价矩阵(行:跟踪器,列:检测结果) +- 返回值: + - 匹配索引向量(元素对应检测索引,-1表示未匹配) + +### 2.3 MultiObjectTracker 类 + +#### 2.3.1 结构体:TrackedObject + +```cpp +struct TrackedObject { + int id; // 目标ID + cv::Rect rect; // 目标边界框 + cv::Point2d velocity; // 速度向量 + EnhancedKalmanFilter ekf; // 关联的卡尔曼滤波器 + int lost_frames; // 连续丢失帧数 + int max_lost_frames; // 最大允许丢失帧数 + + // 成员函数省略... +}; +``` + +#### 2.3.2 成员函数 + +```cpp +void Update(const std::vector& detections, float delta_time); +``` + +- 作用:更新多目标跟踪器状态 +- 参数: + - detections:当前帧的检测结果 + - delta_time:距离上一帧的时间差 + +```cpp +const std::map& GetActiveObjects() const; +``` + +- 作用:获取所有活动目标 +- 返回值:ID到目标对象的映射 + +### 2.4 辅助函数 + +```cpp +double CalculateIOU(const cv::Rect& rect1, const cv::Rect& rect2); +``` + +- 作用:计算两矩形交并比(IoU) + +```cpp +double CalculateAppearanceSimilarity(const cv::Rect& rect1, const cv::Rect& rect2); +``` + +- 作用:计算基于目标尺寸的相似度(外观相似度) + +## 3. 系统架构与核心逻辑 + +### 3.1 系统流程图 + + +### 3.2 多目标跟踪工作流程 + +#### 3.2.1 目标检测 + +```cpp +auto results = model.Predict(input_mat); +std::vector detections; +for (const auto& res : results) { + detections.push_back(res.box); +} +``` + +#### 3.2.2 跟踪器预测 + +```cpp +// 更新所有跟踪器的预测状态 +for (auto& kv : tracked_objects) { + kv.second.Predict(Ts); +} +``` + +#### 3.2.3 代价矩阵计算 + +- 综合三种指标: + - IoU(重叠率) + - 中心点距离 + - 尺寸相似度 +- 权重分配:60% IoU + 30% 距离 + 10% 尺寸相似度 + +#### 3.2.4 数据关联(匈牙利算法) + +```cpp +std::vector assignments = HungarianAlgorithm::Solve(cost_matrix); +``` + +#### 3.2.5 跟踪器状态更新 + +- 匹配成功的更新卡尔曼滤波器 +- 未匹配的检测创建新的跟踪器 +- 未匹配的跟踪器增加丢失技术 + +#### 3.2.6 绘制结果 + +- 为每个目标分配唯一颜色 +- 绘制边界框、ID和丢失帧数 +- 绘制速度向量 + +### 3.3完整代码实现 + +```cpp +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono; + +// ================== 增强版 6D 卡尔曼滤波类 ================== +class EnhancedKalmanFilter { + public: + EnhancedKalmanFilter(float Ts = 0.033f) : prediction_steps(1) { + Initialize(Ts); + } + + void Initialize(float Ts) { + Ts_ = Ts; + // 状态转移矩阵 A (6x6) + A = cv::Mat::eye(6, 6, CV_64F); + A.at(0, 4) = Ts; + A.at(1, 5) = Ts; + + // 观测矩阵 C (4x6) - 只观测位置和大小 + C = cv::Mat::zeros(4, 6, CV_64F); + C.at(0, 0) = 1; + C.at(1, 1) = 1; + C.at(2, 2) = 1; + C.at(3, 3) = 1; + + // 增大过程噪声 - 使预测更灵活 + Q = cv::Mat::eye(6, 6, CV_64F); + + Q.at(0, 0) = 1e-2; // x位置噪声增加 + Q.at(1, 1) = 1e-2; // y位置噪声增加 + Q.at(2, 2) = 1e-6; // 宽度噪声不变 + Q.at(3, 3) = 1e-6; // 高度噪声不变 + Q.at(4, 4) = 1e-3; // x速度噪声减小 + Q.at(5, 5) = 1e-3; // y速度噪声减小 + + // 调整观测噪声 - 使预测更具主导性 + R = cv::Mat::eye(4, 4, CV_64F); + R.at(0, 0) = 1e-2; // x位置观测噪声减小 + R.at(1, 1) = 1e-2; // y位置观测噪声减小 + R.at(2, 2) = 1e-1; // 宽度观测噪声增加 + R.at(3, 3) = 1e-1; // 高度观测噪声增加 + + // 初始估计误差协方差矩阵 P (增大初始不确定性) + P = cv::Mat::eye(6, 6, CV_64F) * 100; + + // 初始状态 [x, y, w, h, dx, dy] + x_hat = cv::Mat::zeros(6, 1, CV_64F); + has_initialized = false; + + // 初始化临时Q矩阵 + Q_temp = Q.clone(); + } + + void UpdateTs(float Ts) { + Ts_ = Ts; + // 更新状态转移矩阵中的时间项 + A.at(0, 4) = Ts; + A.at(1, 5) = Ts; + } + + void Update(const cv::Mat& Z) { + if (!has_initialized) { + // 首次更新,直接初始化状态 + x_hat.at(0) = Z.at(0); + x_hat.at(1) = Z.at(1); + x_hat.at(2) = Z.at(2); + x_hat.at(3) = Z.at(3); + x_hat.at(4) = 0; + x_hat.at(5) = 0; + has_initialized = true; + return; + } + + // 检查位置偏差大小 + cv::Mat residual = Z - C * (A * x_hat); + double positionError = cv::norm(residual.rowRange(0, 2)); + + // 动态加速收敛机制 + if (positionError > 20.0) { + // 位置偏差较大时临时增大位置过程噪声 + Q_temp = Q.clone(); + Q_temp.at(0, 0) *= 10.0; + Q_temp.at(1, 1) *= 10.0; + } else { + Q_temp = Q.clone(); + } + + // 预测步骤 + cv::Mat x_hat_minus = A * x_hat; + cv::Mat P_minus = A * P * A.t() + Q_temp; // 使用临时Q + + // 计算残差 + cv::Mat y = Z - C * x_hat_minus; + + // 计算卡尔曼增益 K + cv::Mat S = C * P_minus * C.t() + R; + cv::Mat K = P_minus * C.t() * S.inv(); + + // 更新状态 + x_hat = x_hat_minus + K * y; + + // 更新协方差 + cv::Mat I = cv::Mat::eye(6, 6, CV_64F); + P = (I - K * C) * P_minus; + } + + void Predict() { + if (!has_initialized) return; + x_hat = A * x_hat; + P = A * P * A.t() + Q_temp; + } + + cv::Mat GetState() const { return x_hat.clone(); } + + cv::Mat GetPredictedState(int steps = 1) const { + if (!has_initialized) return cv::Mat(); + cv::Mat state = x_hat.clone(); + cv::Mat A_step = cv::Mat::eye(6, 6, CV_64F); + A_step.at(0, 4) = Ts_ * steps; + A_step.at(1, 5) = Ts_ * steps; + return A_step * state; + } + + void SetPredictionSteps(int steps) { prediction_steps = std::max(1, steps); } + + int GetPredictionSteps() const { return prediction_steps; } + + // 重置滤波器 + void Reset() { + has_initialized = false; + // 重置协方差矩阵 + P = cv::Mat::eye(6, 6, CV_64F) * 100; + } + + bool HasInitialized() const { return has_initialized; } + + cv::Rect GetRectFromState() const { + if (!has_initialized) return cv::Rect(); + cv::Mat state = GetState(); + return cv::Rect(static_cast(state.at(0)), + static_cast(state.at(1)), + static_cast(state.at(2)), + static_cast(state.at(3))); + } + + // 获取速度向量 + cv::Point2d GetVelocity() const { + if (!has_initialized) return cv::Point2d(0, 0); + return cv::Point2d(x_hat.at(4), x_hat.at(5)); + } + + private: + cv::Mat A, C, Q, R, P; + cv::Mat x_hat; + cv::Mat Q_temp; // 动态调整的Q矩阵 + float Ts_; + int prediction_steps; + bool has_initialized; +}; + +// 计算两个矩形的IOU(交并比) +double CalculateIOU(const cv::Rect& rect1, const cv::Rect& rect2) { + // 计算交集矩形 + int x1 = std::max(rect1.x, rect2.x); + int y1 = std::max(rect1.y, rect2.y); + int x2 = std::min(rect1.x + rect1.width, rect2.x + rect2.width); + int y2 = std::min(rect1.y + rect1.height, rect2.y + rect2.height); + + int intersectionArea = std::max(0, x2 - x1) * std::max(0, y2 - y1); + + // 计算并集面积 + int area1 = rect1.width * rect1.height; + int area2 = rect2.width * rect2.height; + + // 避免除以0 + if (area1 + area2 - intersectionArea <= 0) { + return 0.0; + } + + return static_cast(intersectionArea) / + (area1 + area2 - intersectionArea); +} + +// 计算两个矩形中心点的欧式距离 +double CalculateCenterDistance(const cv::Rect& rect1, const cv::Rect& rect2) { + cv::Point center1(rect1.x + rect1.width / 2, rect1.y + rect1.height / 2); + cv::Point center2(rect2.x + rect2.width / 2, rect2.y + rect2.height / 2); + return cv::norm(center1 - center2); +} + +// 计算外观相似性(使用矩形大小差异作为简单度量) +double CalculateAppearanceSimilarity(const cv::Rect& rect1, + const cv::Rect& rect2) { + double sizeDiff = + std::abs(rect1.width * rect1.height - rect2.width * rect2.height); + // 归一化:10000是一个经验值,代表典型的最大矩形面积 + return std::exp(-sizeDiff / 10000.0); +} + +// ================== 匈牙利算法实现 ================== +class HungarianAlgorithm { + public: + // 解指派问题(最小化代价) + static std::vector Solve( + const std::vector>& costMatrix) { + int n = costMatrix.size(); + if (n == 0) return {}; + + int m = costMatrix[0].size(); + if (m == 0) return std::vector(n, -1); + + // 预处理: 确保矩阵是方阵 + std::vector> cost = costMatrix; + if (n > m) { + for (auto& row : cost) { + row.resize(n, std::numeric_limits::max()); + } + m = n; + } else if (m > n) { + cost.resize(m, + std::vector(m, std::numeric_limits::max())); + for (int i = n; i < m; ++i) { + for (int j = 0; j < m; ++j) { + if (j < costMatrix[0].size()) { + cost[i][j] = costMatrix[i % n][j]; + } + } + } + n = m; + } + + // 初始化变量 + std::vector u(n + 1, 0); + std::vector v(m + 1, 0); + std::vector p(m + 1, 0); + std::vector way(m + 1, 0); + + for (int i = 1; i <= n; ++i) { + p[0] = i; + int j0 = 0; + std::vector minv(m + 1, std::numeric_limits::max()); + std::vector used(m + 1, false); + + do { + used[j0] = true; + int i0 = p[j0]; + double delta = std::numeric_limits::max(); + int j1 = 0; + + for (int j = 1; j <= m; ++j) { + if (!used[j]) { + double cur = cost[i0 - 1][j - 1] - u[i0] - v[j]; + if (cur < minv[j]) { + minv[j] = cur; + way[j] = j0; + } + if (minv[j] < delta) { + delta = minv[j]; + j1 = j; + } + } + } + + for (int j = 0; j <= m; ++j) { + if (used[j]) { + u[p[j]] += delta; + v[j] -= delta; + } else { + minv[j] -= delta; + } + } + + j0 = j1; + } while (p[j0] != 0); + + do { + int j1 = way[j0]; + p[j0] = p[j1]; + j0 = j1; + } while (j0 != 0); + } + + // 构建结果向量 + std::vector result(n, -1); + for (int j = 1; j <= m; ++j) { + if (p[j] != 0 && p[j] <= static_cast(result.size())) { + result[p[j] - 1] = j - 1; + } + } + + return result; + } +}; + +// 多目标跟踪器类 +class MultiObjectTracker { + public: + struct TrackedObject { + int id; + cv::Rect rect; + cv::Point2d velocity; + EnhancedKalmanFilter ekf; + int lost_frames; + int max_lost_frames; + + // 添加默认构造函数 + TrackedObject() + : id(-1), + rect(0, 0, 0, 0), + velocity(0, 0), + lost_frames(0), + max_lost_frames(30) { + ekf.Initialize(0.033f); + } + + TrackedObject(int obj_id, const cv::Rect& init_rect, float Ts) + : id(obj_id), + rect(init_rect), + velocity(0, 0), + lost_frames(0), + max_lost_frames(30) { + ekf.Initialize(Ts); + // 初始化卡尔曼滤波器 + cv::Mat Z = (cv::Mat_(4, 1) << init_rect.x, init_rect.y, + init_rect.width, init_rect.height); + ekf.Update(Z); + } + + void Update(const cv::Rect& new_rect, float Ts) { + ekf.UpdateTs(Ts); + cv::Mat Z = (cv::Mat_(4, 1) << new_rect.x, new_rect.y, + new_rect.width, new_rect.height); + ekf.Update(Z); + rect = ekf.GetRectFromState(); + velocity = ekf.GetVelocity(); + lost_frames = 0; + } + + void Predict(float Ts) { + ekf.UpdateTs(Ts); + ekf.Predict(); + rect = ekf.GetRectFromState(); + velocity = ekf.GetVelocity(); + lost_frames++; + } + + // 当丢失帧数超过max_lost_frames时,视为丢失 + bool IsActive() const { return lost_frames <= max_lost_frames; } + }; + + MultiObjectTracker() : next_id(1), Ts(0.033f) {} + + void Update(const std::vector& detections, float delta_time) { + Ts = delta_time; + + // 步骤1: 卡尔曼预测 + for (auto& kv : tracked_objects) { + kv.second.Predict(Ts); + } + + // 如果没有检测结果,直接返回 + if (detections.empty()) { + return; + } + + // 步骤2: 构建代价矩阵 + std::vector> cost_matrix; + std::vector active_tracker_indices; + + // 收集所有活跃的跟踪器 + for (auto& kv : tracked_objects) { + if (kv.second.IsActive()) { + active_tracker_indices.push_back(kv.first); + } + } + + // 构建代价矩阵 + for (int tracker_idx : active_tracker_indices) { + TrackedObject& tracker = tracked_objects[tracker_idx]; + std::vector costs; + for (const auto& det : detections) { + // 综合代价:IOU + 位置距离 + 外观相似性 + double iou = CalculateIOU(tracker.rect, det); + // 避免NaN/Inf + if (std::isnan(iou)) iou = 0.0; + iou = std::max(0.0, std::min(iou, 1.0)); + + double dist = CalculateCenterDistance(tracker.rect, det); + double appearance = CalculateAppearanceSimilarity(tracker.rect, det); + + // 综合代价函数 + double cost = + (1.0 - iou) * 0.6 + (dist / 100.0) * 0.3 + (1.0 - appearance) * 0.1; + costs.push_back(cost); + } + cost_matrix.push_back(costs); + } + + // 匹配结果存储 + std::vector assignments; + if (!cost_matrix.empty()) { + assignments = HungarianAlgorithm::Solve(cost_matrix); + } else { + // 如果没有活跃跟踪器,分配所有检测为未匹配 + assignments = std::vector(active_tracker_indices.size(), -1); + } + + // 步骤3: 处理匹配结果 + // 创建一个布尔数组来标记每个检测是否被匹配 + std::vector detection_matched(detections.size(), false); + for (int i = 0; i < assignments.size() && i < active_tracker_indices.size(); + ++i) { + int det_index = assignments[i]; + if (det_index >= 0 && det_index < static_cast(detections.size())) { + int tracker_idx = active_tracker_indices[i]; + // 检查矩形是否有效 + const cv::Rect& new_rect = detections[det_index]; + if (new_rect.width > 0 && new_rect.height > 0) { + tracked_objects[tracker_idx].Update(new_rect, Ts); + detection_matched[det_index] = true; + } + } + } + + // 步骤4: 为未匹配的检测创建新跟踪器 + for (int i = 0; i < detections.size(); ++i) { + if (!detection_matched[i] && detections[i].width > 0 && + detections[i].height > 0) { + // 创建新跟踪器 + tracked_objects.insert( + std::make_pair(next_id, TrackedObject(next_id, detections[i], Ts))); + next_id++; + } + } + + // 步骤5: 移除丢失时间过长的跟踪器 + for (auto it = tracked_objects.begin(); it != tracked_objects.end();) { + // 如果丢失超过30帧,移除跟踪器 + if (!it->second.IsActive()) { + it = tracked_objects.erase(it); + } else { + ++it; + } + } + } + + // 获取所有活跃的目标 + const std::map& GetActiveObjects() const { + return tracked_objects; + } + + private: + std::map tracked_objects; + int next_id; + float Ts; +}; + +int main(int argc, char* argv[]) { + if (argc != 2) { + std::cerr << "Usage: Test-PaddleDet model_path" << std::endl; + return 1; + } + + // 初始化模型 + lockzhiner_vision_module::vision::PaddleDet model; + if (!model.Initialize(argv[1])) { + std::cout << "Failed to initialize model." << std::endl; + return 1; + } + + lockzhiner_vision_module::edit::Edit edit; + if (!edit.StartAndAcceptConnection()) { + std::cerr << "Error: Failed to start and accept connection." << std::endl; + return EXIT_FAILURE; + } + std::cout << "Device connected successfully." << std::endl; + + // 打开摄像头 + cv::VideoCapture cap; + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); + + if (!cap.open(0)) { + std::cerr << "Couldn't open video capture device" << std::endl; + return -1; + } + + cv::Mat input_mat; + + // 创建多目标跟踪器 + MultiObjectTracker tracker; + + // 创建随机颜色生成器 + cv::RNG rng(0xFFFFFFFF); + + auto last_time = high_resolution_clock::now(); + + while (true) { + auto current_time = high_resolution_clock::now(); + float Ts = + duration_cast(current_time - last_time).count() / 1000.0f; + if (Ts <= 0.001f) Ts = 0.033f; // 避免零时间间隔 + last_time = current_time; + + cap >> input_mat; + if (input_mat.empty()) { + std::cerr << "Warning: Captured an empty frame." << std::endl; + continue; + } + + // 调用模型进行预测 + high_resolution_clock::time_point start_time = high_resolution_clock::now(); + auto results = model.Predict(input_mat); + high_resolution_clock::time_point end_time = high_resolution_clock::now(); + + // 计算推理时间 + auto time_span = duration_cast(end_time - start_time); + std::cout << "Inference time: " << time_span.count() << " ms" << std::endl; + + // 提取检测结果 + std::vector detections; + for (const auto& res : results) { + // 确保矩形有效 + if (res.box.width > 0 && res.box.height > 0) { + detections.push_back(res.box); + } + } + + // 更新多目标跟踪器 + tracker.Update(detections, Ts); + + // 可视化结果 + cv::Mat output_image = input_mat.clone(); + + // 获取所有活动目标 (只包括活跃的跟踪器) + const auto& active_objects = tracker.GetActiveObjects(); + + // 绘制所有活跃目标 + for (const auto& kv : active_objects) { + const auto& obj = kv.second; + + // 为每个目标生成固定颜色 + int color_seed = obj.id * 1000; // 根据ID生成种子 + cv::RNG obj_rng(color_seed); + cv::Scalar color(obj_rng.uniform(0, 255), obj_rng.uniform(0, 255), + obj_rng.uniform(0, 255)); + + // 绘制跟踪框 + if (obj.rect.width > 0 && obj.rect.height > 0) { + cv::rectangle(output_image, obj.rect, color, 2); + } + + // 绘制速度向量 + if (obj.rect.width > 0 && obj.rect.height > 0) { + cv::Point center(obj.rect.x + obj.rect.width / 2, + obj.rect.y + obj.rect.height / 2); + cv::Point velocity_end(center.x + obj.velocity.x * 10, + center.y + obj.velocity.y * 10); + cv::arrowedLine(output_image, center, velocity_end, color, 2); + + // 显示目标ID和丢失帧数 + std::string info = cv::format("ID:%d Lost:%d", obj.id, obj.lost_frames); + cv::putText(output_image, info, cv::Point(obj.rect.x, obj.rect.y - 10), + cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1); + } + } + + // 在图像顶部显示统计信息 + if (Ts > 0.001f) { // 避免除以零 + std::string stats = + cv::format("Objects: %d FPS: %.1f", active_objects.size(), 1.0f / Ts); + cv::putText(output_image, stats, cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 255), 2); + } + + // 显示结果 + edit.Print(output_image); + + // 简单退出机制 + char c = static_cast(cv::waitKey(1)); + if (c == 27) break; // ESC键退出 + } + + cap.release(); + return 0; +} +``` + +## 4. 编译过程 + +### 4.1 编译环境搭建 + +- 请确保你已经按照 [开发环境搭建指南](../../../../docs/introductory_tutorial/cpp_development_environment.md) 正确配置了开发环境。 +- 同时以正确连接开发板。 + +### 4.2 Cmake介绍 + +```cmake +cmake_minimum_required(VERSION 3.10) + +project(D01_test_detection) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# 定义项目根目录路径 +set(PROJECT_ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../..") +message("PROJECT_ROOT_PATH = " ${PROJECT_ROOT_PATH}) + +include("${PROJECT_ROOT_PATH}/toolchains/arm-rockchip830-linux-uclibcgnueabihf.toolchain.cmake") + +# 定义 OpenCV SDK 路径 +set(OpenCV_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/opencv-mobile-4.10.0-lockzhiner-vision-module") +set(OpenCV_DIR "${OpenCV_ROOT_PATH}/lib/cmake/opencv4") +find_package(OpenCV REQUIRED) +set(OPENCV_LIBRARIES "${OpenCV_LIBS}") + +# 定义 LockzhinerVisionModule SDK 路径 +set(LockzhinerVisionModule_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/lockzhiner_vision_module_sdk") +set(LockzhinerVisionModule_DIR "${LockzhinerVisionModule_ROOT_PATH}/lib/cmake/lockzhiner_vision_module") +find_package(LockzhinerVisionModule REQUIRED) + +add_executable(Test-target-tracking test_target_tracking.cc) +target_include_directories(Test-target-tracking PRIVATE ${LOCKZHINER_VISION_MODULE_INCLUDE_DIRS}) +target_link_libraries(Test-target-tracking PRIVATE ${OPENCV_LIBRARIES} ${LOCKZHINER_VISION_MODULE_LIBRARIES}) + +install( + TARGETS Test-target-tracking + RUNTIME DESTINATION . +) +``` + +### 4.3 编译项目 + +使用 Docker Destop 打开 LockzhinerVisionModule 容器并执行以下命令来编译项目 + +```bash +# 进入Demo所在目录 +cd /LockzhinerVisionModuleWorkSpace/LockzhinerVisionModule/Cpp_example/D12_target_tracking +# 创建编译目录 +rm -rf build && mkdir build && cd build +# 配置交叉编译工具链 +export TOOLCHAIN_ROOT_PATH="/LockzhinerVisionModuleWorkSpace/arm-rockchip830-linux-uclibcgnueabihf" +# 使用cmake配置项目 +cmake .. +# 执行编译项目 +make -j8 && make install +``` + +在执行完上述命令后,会在build目录下生成可执行文件。 + +## 5. 运行结果 + +在此下载所使用的模型 + + +```shell +chmod 777 Test-target-tracking +# 在实际应用的过程中LZ-Picodet需要替换为下载的或者你的rknn模型 +./Test-detection LZ-Picodet +``` + +运行结果如下: +![结果](./images/D12_2.png) + +## 6.总结 + +本文档提出了一种基于卡尔曼滤波和匈牙利算法的多目标跟踪实现,通过集成6D状态卡尔曼滤波器(EnhancedKalmanFilter)、多目标跟踪管理核心(MultiObjectTracker)以及检测-轨迹匹配算法(HungarianAlgorithm),构建了一个高效稳定的跟踪系统。该系统引入了多维度代价函数(结合IoU、距离和外观信息),并采用自适应过程噪声调整机制和动态目标ID管理策略,有效提升了在复杂场景下对多目标的跟踪能力。该方法可广泛应用于视频监控系统中的目标持续跟踪、自动驾驶中的车辆与行人追踪,以及体育赛事中运动员的行为分析,尤其在处理目标遮挡和路径交错等挑战性场景中表现出色。 diff --git a/Cpp_example/D13_target_tracking/images/D12_1.png b/Cpp_example/D13_target_tracking/images/D12_1.png new file mode 100755 index 0000000000000000000000000000000000000000..7ba0e97dddc870de0ef065f9924aa1ecd30dd442 Binary files /dev/null and b/Cpp_example/D13_target_tracking/images/D12_1.png differ diff --git a/Cpp_example/D13_target_tracking/images/D12_2.png b/Cpp_example/D13_target_tracking/images/D12_2.png new file mode 100755 index 0000000000000000000000000000000000000000..aa8ee61e2f2ab9382d9f9235599d361e72d26f62 Binary files /dev/null and b/Cpp_example/D13_target_tracking/images/D12_2.png differ diff --git a/Cpp_example/D13_target_tracking/test_target_tracking.cc b/Cpp_example/D13_target_tracking/test_target_tracking.cc new file mode 100755 index 0000000000000000000000000000000000000000..d1412f033c5211c265353f7389ca203d7b421783 --- /dev/null +++ b/Cpp_example/D13_target_tracking/test_target_tracking.cc @@ -0,0 +1,598 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono; + +// ================== 增强版 6D 卡尔曼滤波类 ================== +class EnhancedKalmanFilter { + public: + EnhancedKalmanFilter(float Ts = 0.033f) : prediction_steps(1) { + Initialize(Ts); + } + + void Initialize(float Ts) { + Ts_ = Ts; + // 状态转移矩阵 A (6x6) + A = cv::Mat::eye(6, 6, CV_64F); + A.at(0, 4) = Ts; + A.at(1, 5) = Ts; + + // 观测矩阵 C (4x6) - 只观测位置和大小 + C = cv::Mat::zeros(4, 6, CV_64F); + C.at(0, 0) = 1; + C.at(1, 1) = 1; + C.at(2, 2) = 1; + C.at(3, 3) = 1; + + // 增大过程噪声 - 使预测更灵活 + Q = cv::Mat::eye(6, 6, CV_64F); + + Q.at(0, 0) = 1e-2; // x位置噪声增加 + Q.at(1, 1) = 1e-2; // y位置噪声增加 + Q.at(2, 2) = 1e-6; // 宽度噪声不变 + Q.at(3, 3) = 1e-6; // 高度噪声不变 + Q.at(4, 4) = 1e-3; // x速度噪声减小 + Q.at(5, 5) = 1e-3; // y速度噪声减小 + + // 调整观测噪声 - 使预测更具主导性 + R = cv::Mat::eye(4, 4, CV_64F); + R.at(0, 0) = 1e-2; // x位置观测噪声减小 + R.at(1, 1) = 1e-2; // y位置观测噪声减小 + R.at(2, 2) = 1e-1; // 宽度观测噪声增加 + R.at(3, 3) = 1e-1; // 高度观测噪声增加 + + // 初始估计误差协方差矩阵 P (增大初始不确定性) + P = cv::Mat::eye(6, 6, CV_64F) * 100; + + // 初始状态 [x, y, w, h, dx, dy] + x_hat = cv::Mat::zeros(6, 1, CV_64F); + has_initialized = false; + + // 初始化临时Q矩阵 + Q_temp = Q.clone(); + } + + void UpdateTs(float Ts) { + Ts_ = Ts; + // 更新状态转移矩阵中的时间项 + A.at(0, 4) = Ts; + A.at(1, 5) = Ts; + } + + void Update(const cv::Mat& Z) { + if (!has_initialized) { + // 首次更新,直接初始化状态 + x_hat.at(0) = Z.at(0); + x_hat.at(1) = Z.at(1); + x_hat.at(2) = Z.at(2); + x_hat.at(3) = Z.at(3); + x_hat.at(4) = 0; + x_hat.at(5) = 0; + has_initialized = true; + return; + } + + // 检查位置偏差大小 + cv::Mat residual = Z - C * (A * x_hat); + double positionError = cv::norm(residual.rowRange(0, 2)); + + // 动态加速收敛机制 + if (positionError > 20.0) { + // 位置偏差较大时临时增大位置过程噪声 + Q_temp = Q.clone(); + Q_temp.at(0, 0) *= 10.0; + Q_temp.at(1, 1) *= 10.0; + } else { + Q_temp = Q.clone(); + } + + // 预测步骤 + cv::Mat x_hat_minus = A * x_hat; + cv::Mat P_minus = A * P * A.t() + Q_temp; // 使用临时Q + + // 计算残差 + cv::Mat y = Z - C * x_hat_minus; + + // 计算卡尔曼增益 K + cv::Mat S = C * P_minus * C.t() + R; + cv::Mat K = P_minus * C.t() * S.inv(); + + // 更新状态 + x_hat = x_hat_minus + K * y; + + // 更新协方差 + cv::Mat I = cv::Mat::eye(6, 6, CV_64F); + P = (I - K * C) * P_minus; + } + + void Predict() { + if (!has_initialized) return; + x_hat = A * x_hat; + P = A * P * A.t() + Q_temp; + } + + cv::Mat GetState() const { return x_hat.clone(); } + + cv::Mat GetPredictedState(int steps = 1) const { + if (!has_initialized) return cv::Mat(); + cv::Mat state = x_hat.clone(); + cv::Mat A_step = cv::Mat::eye(6, 6, CV_64F); + A_step.at(0, 4) = Ts_ * steps; + A_step.at(1, 5) = Ts_ * steps; + return A_step * state; + } + + void SetPredictionSteps(int steps) { prediction_steps = std::max(1, steps); } + + int GetPredictionSteps() const { return prediction_steps; } + + // 重置滤波器 + void Reset() { + has_initialized = false; + // 重置协方差矩阵 + P = cv::Mat::eye(6, 6, CV_64F) * 100; + } + + bool HasInitialized() const { return has_initialized; } + + cv::Rect GetRectFromState() const { + if (!has_initialized) return cv::Rect(); + cv::Mat state = GetState(); + return cv::Rect(static_cast(state.at(0)), + static_cast(state.at(1)), + static_cast(state.at(2)), + static_cast(state.at(3))); + } + + // 获取速度向量 + cv::Point2d GetVelocity() const { + if (!has_initialized) return cv::Point2d(0, 0); + return cv::Point2d(x_hat.at(4), x_hat.at(5)); + } + + private: + cv::Mat A, C, Q, R, P; + cv::Mat x_hat; + cv::Mat Q_temp; // 动态调整的Q矩阵 + float Ts_; + int prediction_steps; + bool has_initialized; +}; + +// 计算两个矩形的IOU(交并比) +double CalculateIOU(const cv::Rect& rect1, const cv::Rect& rect2) { + // 计算交集矩形 + int x1 = std::max(rect1.x, rect2.x); + int y1 = std::max(rect1.y, rect2.y); + int x2 = std::min(rect1.x + rect1.width, rect2.x + rect2.width); + int y2 = std::min(rect1.y + rect1.height, rect2.y + rect2.height); + + int intersectionArea = std::max(0, x2 - x1) * std::max(0, y2 - y1); + + // 计算并集面积 + int area1 = rect1.width * rect1.height; + int area2 = rect2.width * rect2.height; + + // 避免除以0 + if (area1 + area2 - intersectionArea <= 0) { + return 0.0; + } + + return static_cast(intersectionArea) / + (area1 + area2 - intersectionArea); +} + +// 计算两个矩形中心点的欧式距离 +double CalculateCenterDistance(const cv::Rect& rect1, const cv::Rect& rect2) { + cv::Point center1(rect1.x + rect1.width / 2, rect1.y + rect1.height / 2); + cv::Point center2(rect2.x + rect2.width / 2, rect2.y + rect2.height / 2); + return cv::norm(center1 - center2); +} + +// 计算外观相似性(使用矩形大小差异作为简单度量) +double CalculateAppearanceSimilarity(const cv::Rect& rect1, + const cv::Rect& rect2) { + double sizeDiff = + std::abs(rect1.width * rect1.height - rect2.width * rect2.height); + // 归一化:10000是一个经验值,代表典型的最大矩形面积 + return std::exp(-sizeDiff / 10000.0); +} + +// ================== 匈牙利算法实现 ================== +class HungarianAlgorithm { + public: + // 解指派问题(最小化代价) + static std::vector Solve( + const std::vector>& costMatrix) { + int n = costMatrix.size(); + if (n == 0) return {}; + + int m = costMatrix[0].size(); + if (m == 0) return std::vector(n, -1); + + // 预处理: 确保矩阵是方阵 + std::vector> cost = costMatrix; + if (n > m) { + for (auto& row : cost) { + row.resize(n, std::numeric_limits::max()); + } + m = n; + } else if (m > n) { + cost.resize(m, + std::vector(m, std::numeric_limits::max())); + for (int i = n; i < m; ++i) { + for (int j = 0; j < m; ++j) { + if (j < costMatrix[0].size()) { + cost[i][j] = costMatrix[i % n][j]; + } + } + } + n = m; + } + + // 初始化变量 + std::vector u(n + 1, 0); + std::vector v(m + 1, 0); + std::vector p(m + 1, 0); + std::vector way(m + 1, 0); + + for (int i = 1; i <= n; ++i) { + p[0] = i; + int j0 = 0; + std::vector minv(m + 1, std::numeric_limits::max()); + std::vector used(m + 1, false); + + do { + used[j0] = true; + int i0 = p[j0]; + double delta = std::numeric_limits::max(); + int j1 = 0; + + for (int j = 1; j <= m; ++j) { + if (!used[j]) { + double cur = cost[i0 - 1][j - 1] - u[i0] - v[j]; + if (cur < minv[j]) { + minv[j] = cur; + way[j] = j0; + } + if (minv[j] < delta) { + delta = minv[j]; + j1 = j; + } + } + } + + for (int j = 0; j <= m; ++j) { + if (used[j]) { + u[p[j]] += delta; + v[j] -= delta; + } else { + minv[j] -= delta; + } + } + + j0 = j1; + } while (p[j0] != 0); + + do { + int j1 = way[j0]; + p[j0] = p[j1]; + j0 = j1; + } while (j0 != 0); + } + + // 构建结果向量 + std::vector result(n, -1); + for (int j = 1; j <= m; ++j) { + if (p[j] != 0 && p[j] <= static_cast(result.size())) { + result[p[j] - 1] = j - 1; + } + } + + return result; + } +}; + +// 多目标跟踪器类 +class MultiObjectTracker { + public: + struct TrackedObject { + int id; + cv::Rect rect; + cv::Point2d velocity; + EnhancedKalmanFilter ekf; + int lost_frames; + int max_lost_frames; + + // 添加默认构造函数 + TrackedObject() + : id(-1), + rect(0, 0, 0, 0), + velocity(0, 0), + lost_frames(0), + max_lost_frames(30) { + ekf.Initialize(0.033f); + } + + TrackedObject(int obj_id, const cv::Rect& init_rect, float Ts) + : id(obj_id), + rect(init_rect), + velocity(0, 0), + lost_frames(0), + max_lost_frames(30) { + ekf.Initialize(Ts); + // 初始化卡尔曼滤波器 + cv::Mat Z = (cv::Mat_(4, 1) << init_rect.x, init_rect.y, + init_rect.width, init_rect.height); + ekf.Update(Z); + } + + void Update(const cv::Rect& new_rect, float Ts) { + ekf.UpdateTs(Ts); + cv::Mat Z = (cv::Mat_(4, 1) << new_rect.x, new_rect.y, + new_rect.width, new_rect.height); + ekf.Update(Z); + rect = ekf.GetRectFromState(); + velocity = ekf.GetVelocity(); + lost_frames = 0; + } + + void Predict(float Ts) { + ekf.UpdateTs(Ts); + ekf.Predict(); + rect = ekf.GetRectFromState(); + velocity = ekf.GetVelocity(); + lost_frames++; + } + + // 当丢失帧数超过max_lost_frames时,视为丢失 + bool IsActive() const { return lost_frames <= max_lost_frames; } + }; + + MultiObjectTracker() : next_id(1), Ts(0.033f) {} + + void Update(const std::vector& detections, float delta_time) { + Ts = delta_time; + + // 步骤1: 卡尔曼预测 + for (auto& kv : tracked_objects) { + kv.second.Predict(Ts); + } + + // 如果没有检测结果,直接返回 + if (detections.empty()) { + return; + } + + // 步骤2: 构建代价矩阵 + std::vector> cost_matrix; + std::vector active_tracker_indices; + + // 收集所有活跃的跟踪器 + for (auto& kv : tracked_objects) { + if (kv.second.IsActive()) { + active_tracker_indices.push_back(kv.first); + } + } + + // 构建代价矩阵 + for (int tracker_idx : active_tracker_indices) { + TrackedObject& tracker = tracked_objects[tracker_idx]; + std::vector costs; + for (const auto& det : detections) { + // 综合代价:IOU + 位置距离 + 外观相似性 + double iou = CalculateIOU(tracker.rect, det); + // 避免NaN/Inf + if (std::isnan(iou)) iou = 0.0; + iou = std::max(0.0, std::min(iou, 1.0)); + + double dist = CalculateCenterDistance(tracker.rect, det); + double appearance = CalculateAppearanceSimilarity(tracker.rect, det); + + // 综合代价函数 + double cost = + (1.0 - iou) * 0.6 + (dist / 100.0) * 0.3 + (1.0 - appearance) * 0.1; + costs.push_back(cost); + } + cost_matrix.push_back(costs); + } + + // 匹配结果存储 + std::vector assignments; + if (!cost_matrix.empty()) { + assignments = HungarianAlgorithm::Solve(cost_matrix); + } else { + // 如果没有活跃跟踪器,分配所有检测为未匹配 + assignments = std::vector(active_tracker_indices.size(), -1); + } + + // 步骤3: 处理匹配结果 + // 创建一个布尔数组来标记每个检测是否被匹配 + std::vector detection_matched(detections.size(), false); + for (int i = 0; i < assignments.size() && i < active_tracker_indices.size(); + ++i) { + int det_index = assignments[i]; + if (det_index >= 0 && det_index < static_cast(detections.size())) { + int tracker_idx = active_tracker_indices[i]; + // 检查矩形是否有效 + const cv::Rect& new_rect = detections[det_index]; + if (new_rect.width > 0 && new_rect.height > 0) { + tracked_objects[tracker_idx].Update(new_rect, Ts); + detection_matched[det_index] = true; + } + } + } + + // 步骤4: 为未匹配的检测创建新跟踪器 + for (int i = 0; i < detections.size(); ++i) { + if (!detection_matched[i] && detections[i].width > 0 && + detections[i].height > 0) { + // 创建新跟踪器 + tracked_objects.insert( + std::make_pair(next_id, TrackedObject(next_id, detections[i], Ts))); + next_id++; + } + } + + // 步骤5: 移除丢失时间过长的跟踪器 + for (auto it = tracked_objects.begin(); it != tracked_objects.end();) { + // 如果丢失超过30帧,移除跟踪器 + if (!it->second.IsActive()) { + it = tracked_objects.erase(it); + } else { + ++it; + } + } + } + + // 获取所有活跃的目标 + const std::map& GetActiveObjects() const { + return tracked_objects; + } + + private: + std::map tracked_objects; + int next_id; + float Ts; +}; + +int main(int argc, char* argv[]) { + if (argc != 2) { + std::cerr << "Usage: Test-PaddleDet model_path" << std::endl; + return 1; + } + + // 初始化模型 + lockzhiner_vision_module::vision::PaddleDet model; + if (!model.Initialize(argv[1])) { + std::cout << "Failed to initialize model." << std::endl; + return 1; + } + + lockzhiner_vision_module::edit::Edit edit; + if (!edit.StartAndAcceptConnection()) { + std::cerr << "Error: Failed to start and accept connection." << std::endl; + return EXIT_FAILURE; + } + std::cout << "Device connected successfully." << std::endl; + + // 打开摄像头 + cv::VideoCapture cap; + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); + + if (!cap.open(0)) { + std::cerr << "Couldn't open video capture device" << std::endl; + return -1; + } + + cv::Mat input_mat; + + // 创建多目标跟踪器 + MultiObjectTracker tracker; + + // 创建随机颜色生成器 + cv::RNG rng(0xFFFFFFFF); + + auto last_time = high_resolution_clock::now(); + + while (true) { + auto current_time = high_resolution_clock::now(); + float Ts = + duration_cast(current_time - last_time).count() / 1000.0f; + if (Ts <= 0.001f) Ts = 0.033f; // 避免零时间间隔 + last_time = current_time; + + cap >> input_mat; + if (input_mat.empty()) { + std::cerr << "Warning: Captured an empty frame." << std::endl; + continue; + } + + // 调用模型进行预测 + high_resolution_clock::time_point start_time = high_resolution_clock::now(); + auto results = model.Predict(input_mat); + high_resolution_clock::time_point end_time = high_resolution_clock::now(); + + // 计算推理时间 + auto time_span = duration_cast(end_time - start_time); + std::cout << "Inference time: " << time_span.count() << " ms" << std::endl; + + // 提取检测结果 + std::vector detections; + for (const auto& res : results) { + // 确保矩形有效 + if (res.box.width > 0 && res.box.height > 0) { + detections.push_back(res.box); + } + } + + // 更新多目标跟踪器 + tracker.Update(detections, Ts); + + // 可视化结果 + cv::Mat output_image = input_mat.clone(); + + // 获取所有活动目标 (只包括活跃的跟踪器) + const auto& active_objects = tracker.GetActiveObjects(); + + // 绘制所有活跃目标 + for (const auto& kv : active_objects) { + const auto& obj = kv.second; + + // 为每个目标生成固定颜色 + int color_seed = obj.id * 1000; // 根据ID生成种子 + cv::RNG obj_rng(color_seed); + cv::Scalar color(obj_rng.uniform(0, 255), obj_rng.uniform(0, 255), + obj_rng.uniform(0, 255)); + + // 绘制跟踪框 + if (obj.rect.width > 0 && obj.rect.height > 0) { + cv::rectangle(output_image, obj.rect, color, 2); + } + + // 绘制速度向量 + if (obj.rect.width > 0 && obj.rect.height > 0) { + cv::Point center(obj.rect.x + obj.rect.width / 2, + obj.rect.y + obj.rect.height / 2); + cv::Point velocity_end(center.x + obj.velocity.x * 10, + center.y + obj.velocity.y * 10); + cv::arrowedLine(output_image, center, velocity_end, color, 2); + + // 显示目标ID和丢失帧数 + std::string info = cv::format("ID:%d Lost:%d", obj.id, obj.lost_frames); + cv::putText(output_image, info, cv::Point(obj.rect.x, obj.rect.y - 10), + cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1); + } + } + + // 在图像顶部显示统计信息 + if (Ts > 0.001f) { // 避免除以零 + std::string stats = + cv::format("Objects: %d FPS: %.1f", active_objects.size(), 1.0f / Ts); + cv::putText(output_image, stats, cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 255), 2); + } + + // 显示结果 + edit.Print(output_image); + + // 简单退出机制 + char c = static_cast(cv::waitKey(1)); + if (c == 27) break; // ESC键退出 + } + + cap.release(); + return 0; +} \ No newline at end of file diff --git a/Cpp_example/E01_find_Laser/CMakeLists.txt b/Cpp_example/E01_find_Laser/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..e59068980ed75b32eb6655b43532c0c12f88ca42 --- /dev/null +++ b/Cpp_example/E01_find_Laser/CMakeLists.txt @@ -0,0 +1,33 @@ +# CMake最低版本要求 +cmake_minimum_required(VERSION 3.10) + +project(test_lcd) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# 定义项目根目录路径 +set(PROJECT_ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../..") +message("PROJECT_ROOT_PATH = " ${PROJECT_ROOT_PATH}) + +include("${PROJECT_ROOT_PATH}/toolchains/arm-rockchip830-linux-uclibcgnueabihf.toolchain.cmake") + +# 定义 OpenCV SDK 路径 +set(OpenCV_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/opencv-mobile-4.10.0-lockzhiner-vision-module") +set(OpenCV_DIR "${OpenCV_ROOT_PATH}/lib/cmake/opencv4") +find_package(OpenCV REQUIRED) +set(OPENCV_LIBRARIES "${OpenCV_LIBS}") +# 定义 LockzhinerVisionModule SDK 路径 +set(LockzhinerVisionModule_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/lockzhiner_vision_module_sdk") +set(LockzhinerVisionModule_DIR "${LockzhinerVisionModule_ROOT_PATH}/lib/cmake/lockzhiner_vision_module") +find_package(LockzhinerVisionModule REQUIRED) + + +add_executable(Test_find_Laser Test_find_Laser.cc) +target_include_directories(Test_find_Laser PRIVATE ${LOCKZHINER_VISION_MODULE_INCLUDE_DIRS}) +target_link_libraries(Test_find_Laser PRIVATE ${OPENCV_LIBRARIES} ${LOCKZHINER_VISION_MODULE_LIBRARIES}) + +install( + TARGETS Test_find_Laser + RUNTIME DESTINATION . +) \ No newline at end of file diff --git a/Cpp_example/E01_find_Laser/README.md b/Cpp_example/E01_find_Laser/README.md new file mode 100644 index 0000000000000000000000000000000000000000..e68374d26df5020f17d7f7461d5899ad1152c550 --- /dev/null +++ b/Cpp_example/E01_find_Laser/README.md @@ -0,0 +1,575 @@ +# 电赛:激光追踪 + +本文档为2023年全国大学生电子设计竞赛E题视觉设计参考,使用凌智视觉模块搭配OpenCV和目标检测实现。 + +## 1. 思路分享 + +### 1.1 赛题分析 + +在23年电子设计竞赛(电赛)的E题中,主要涉及两大难点: + +- 激光光斑的识别 +- 云台精度与控制 + +本文将重点对**激光识别环节**进行剖析。 + +传统方法及其局限性 + +使用OpenMV等传统视觉模块进行双色激光笔识别时,通常首选基于颜色的识别方法。但在实际应用中,激光光点亮度极高,在摄像头成像中往往呈现近似白色,很难通过设定阈值进行有效区分。 + +虽然尝试通过降低亮度处理来缓解过曝,但在黑色胶带背景下却又常因亮度不足而难以捕捉。这种识别不稳定现象大大增加了控制难度。 + +对于基础任务,虽然可以采用差帧法进行识别,但在应对拓展题目时,该方法的局限性便会显现。 + +此外,OpenMV在高分辨率下运行时存在显著卡顿问题。这导致摄像头画面中激光光点每移动一个像素点,现实中往往对应着较大的位移增量,从而引发严重的控制精度问题。 + +与此同时,电赛测评环境与日常调试环境差异显著,经常出现“调试成功而正式测评失败”的困惑局面。 + +解决思路 + +本设计旨在针对性解决上述痛点。 + +具体到激光识别方案: + +- **降低光线敏感度**:采用更鲁棒的处理方式,对光线的要求相对宽松。 +- **提高空间精度**:在VGA(640×480)分辨率下运行,显著提升位置检测精度。 +- **保障控制实时性**:全流程处理速率接近30帧/秒(FPS),为后续云台控制提供坚实的帧率基础保障。 + +### 1.2 使用技术分析 + +- OpenCV-Mobile 是一个专门为移动设备和嵌入式平台优化的 OpenCV(开源计算机视觉库)子集版本。其体积只有标准OpenCV的十分之一,但是却几乎完整的继承了所有常用函数。 + +- PaddleDetection PaddleDetection 是基于百度飞桨深度学习框架开发的一个高效的目标检测库,支持多种先进的目标检测模型。 + +## 2. API 文档 + +### 2.1 PaddleDetection 类 + +#### 2.1.1 头文件 + +```cpp +#include +``` + +#### 2.1.2 构造函数 + +```cpp +lockzhiner_vision_module::vision::PaddleDetection(); +``` + +- 作用: + - 创建一个 PaddleDetection 对象,并初始化相关成员变量。 +- 参数: + - 无 +- 返回值: + - 无 + +#### 2.1.3 Initialize函数 + +```cpp +bool Initialize(const std::string& model_path); +``` + +- 作用: + - 加载预训练的 PaddleDetection 模型。 +- 参数: + - model_path:模型路径,包含模型文件和参数文件。 +- 返回值: + - true:模型加载成功。 + - false:模型加载失败。 + +#### 2.1.4 SetThreshold函数 + +```cpp +void SetThreshold(float score_threshold = 0.5, float nms_threshold = 0.3); +``` + +- 作用: + - 设置目标检测的置信度阈值和NMS阈值。 +- 参数: + - score_threshold:置信度阈值,默认值为0.5。 + - nms_threshold:NMS阈值,默认值为0.3。 +- 返回值: + - 无 + +#### 2.1.5 Predict函数 + +```cpp +std::vector Predict(const cv::Mat& image); +``` + +- 作用: + - 使用加载的模型对输入图像进行目标检测,返回检测结果。 +- 参数: + - input_mat (const cv::Mat&): 输入的图像数据,通常是一个 cv::Mat 变量。 +- 返回值: + - 返回一个包含多个 DetectionResult 对象的向量,每个对象表示一个检测结果。 + +### 2.2 DetectionResult 类 + +#### 2.2.1 头文件 + +```cpp +#include +``` + +#### 2.2.2 box函数 + +```cpp +lockzhiner_vision_module::vision::Rect box() const; +``` + +- 作用: + - 获取目标检测结果的边界框。 +- 参数: + - 无 +- 返回值: + - 返回一个 lockzhiner_vision_module::vision::Rect 对象,表示目标检测结果的边界框。 + +#### 2.2.3 score函数 + +```cpp +float score() const; +``` + +- 作用: + - 获取目标检测结果的置信度得分。 +- 参数: + - 无 +- 返回值: + - 返回一个 float 类型的置信度得分。 + +#### 2.2.4 label_id函数 + +- 作用: + - 获取目标检测结果的标签ID。 +- 参数: + - 无 +- 返回值: + - 返回一个整数,表示目标检测结果的标签ID。 + +### 2.3 Visualize 函数 + +### 2.3.1 头文件 + +```cpp +#include +``` + +### 2.3.2 函数定义 + +```cpp +void lockzhiner_vision_module::vision::Visualize( + const cv::Mat& input_mat, + cv::Mat& output_image, + const std::vector& results, + const std::vector& labels = {}, + float font_scale = 0.4 +); +``` + +- 作用: + - 将目标检测结果可视化到输入图像上,并返回可视化后的图像。 +- 参数: + - input_mat (const cv::Mat&): 输入图像。 + - output_image (cv::Mat&): 输出图像,包含标注后的结果。 + - results (const std::vector&): 检测结果列表。 + - labels (const std::vector&): 可选的标签列表,用于标注类别名称,默认为空。 + - font_scale (float): 字体大小比例,默认为 0.4。 +- 返回值: + - 无 + +## 3. 示例代码解析 + +### 3.1 流程讲解 + +```cpp +程序启动 +├─ 初始化模型 (PaddleDetection) +├─ 初始化摄像头 (640×480) +└─ 进入主循环 [循环执行] + ├─ 捕获视频帧 + ├─ 条件判断 + │ ├─ 超过5秒? + │ │ ├─ 是 → 尝试检测外矩形框 + │ │ │ ├─ 成功 → 连续5次稳定检测? + │ │ │ │ ├─ 是 → 锁定标定框 + │ │ │ │ └─ 否 → 更新参考 + │ │ │ └─ 失败 → 跳过 + │ │ └─ 否 → 跳过标定 + │ └─ 锁定标定框后 → 绘制固定外框 + ├─ 模型推理处理帧 + └─ 绘制四要素 + ├─ 1. 目标检测框 + ├─ 2. 中心十字标记 + └─ 3. 坐标/置信度 + └─ 输出处理后的画面 + └─ 返回主循环 +``` + +在本次设计中,我们的系统相当于有一个启动时间,之后系统会框定出题中所要求的黑色胶带制作的A4纸的边框。在实际使用的过程中我们往往不需要随时改变这个框的位置,故在本次程序设计中直接进行识别后进行标定即框的位置就不再变化。在当年比赛的话可以采用上下机通讯的方式进行框定位置的修改。 + +### 3.3 完整代码实现 + +```cpp +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono; + +// 绘制十字标记的函数 +void drawCrossMarker(cv::Mat& image, cv::Point center, + cv::Scalar color = cv::Scalar(255, 0, 0), int size = 10, + int thickness = 2) { + // 绘制水平线 + cv::line(image, cv::Point(center.x - size, center.y), + cv::Point(center.x + size, center.y), color, thickness); + + // 绘制垂直线 + cv::line(image, cv::Point(center.x, center.y - size), + cv::Point(center.x, center.y + size), color, thickness); +} + +// 检测画面中最大的外矩形框 +bool detectOuterRect(cv::Mat& frame, std::vector& outerRect, + double& area) { + // 转换为灰度图像 + cv::Mat gray; + cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + + // 应用高斯模糊减少噪声 + cv::GaussianBlur(gray, gray, cv::Size(5, 5), 0); + + // Canny边缘检测 + cv::Mat edges; + cv::Canny(gray, edges, 50, 150, 3); + + // 查找轮廓 + std::vector> contours; + std::vector hierarchy; + cv::findContours(edges, contours, hierarchy, + cv::RETR_EXTERNAL, // 只检测外轮廓 + cv::CHAIN_APPROX_SIMPLE); + + double maxArea = 0; + std::vector largestRect; + + // 查找最大的矩形轮廓(外矩形) + for (size_t i = 0; i < contours.size(); i++) { + // 近似多边形 + std::vector approx; + double epsilon = 0.02 * cv::arcLength(contours[i], true); + cv::approxPolyDP(contours[i], approx, epsilon, true); + + // 筛选四边形 + if (approx.size() == 4 && cv::isContourConvex(approx)) { + double currentArea = cv::contourArea(approx); + if (currentArea > maxArea) { + maxArea = currentArea; + largestRect = approx; + } + } + } + + // 返回检测结果 + if (maxArea > 1000) { + outerRect = largestRect; + area = maxArea; + return true; + } + return false; +} + +// 计算矩形的中心点 +cv::Point calculateRectCenter(const std::vector& rect) { + cv::Point center(0, 0); + for (const auto& pt : rect) { + center.x += pt.x; + center.y += pt.y; + } + center.x /= 4; + center.y /= 4; + return center; +} + +// 检查两个矩形是否相似(中心点位置和面积) +bool areRectsSimilar(const std::vector& rect1, + const std::vector& rect2, double area1, + double area2) { + // 计算中心点 + cv::Point center1 = calculateRectCenter(rect1); + cv::Point center2 = calculateRectCenter(rect2); + + // 计算中心点距离 + double distance = cv::norm(center1 - center2); + + // 计算面积差异 + double areaDiff = std::fabs(area1 - area2); + double areaRatio = areaDiff / std::max(area1, area2); + + // 判断是否相似(阈值可根据实际调整) + return (distance < 20) && (areaRatio < 0.1); +} + +// 绘制固定的外矩形标记 +void drawFixedRectMarkers(cv::Mat& frame, + const std::vector& outerRect, + double area) { + // 绘制外矩形框 + cv::polylines(frame, std::vector>{outerRect}, true, + cv::Scalar(255, 0, 0), 2); +} + +int main(int argc, char* argv[]) { + if (argc != 2) { + std::cerr << "Usage: Test-PaddleDet model_path" << std::endl; + return 1; + } + + // 初始化模型 + lockzhiner_vision_module::vision::PaddleDet model; + if (!model.Initialize(argv[1])) { + std::cout << "Failed to initialize model." << std::endl; + return 1; + } + + lockzhiner_vision_module::edit::Edit edit; + if (!edit.StartAndAcceptConnection()) { + std::cerr << "Error: Failed to start and accept connection." << std::endl; + return EXIT_FAILURE; + } + std::cout << "Device connected successfully." << std::endl; + + // 打开摄像头 + cv::VideoCapture cap; + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); + cap.open(0); + + if (!cap.isOpened()) { + std::cerr << "Error: Could not open camera." << std::endl; + return 1; + } + + // 状态变量 + bool rectDetected = false; + std::vector calibratedOuterRect; + double calibratedOuterArea = 0; + int similarDetectionCount = 0; + const int requiredSimilarDetections = 5; + auto startTime = high_resolution_clock::now(); + + // 存储最近检测到的矩形(用于比较) + struct RectDetection { + std::vector points; + double area; + }; + RectDetection lastDetection; + + cv::Mat input_mat; + while (true) { + // 捕获一帧图像 + cap >> input_mat; + if (input_mat.empty()) { + std::cerr << "Warning: Captured an empty frame." << std::endl; + continue; + } + + // 创建原始图像的副本用于模型推理 + cv::Mat inference_mat = input_mat.clone(); + + auto currentTime = high_resolution_clock::now(); + auto elapsedTime = duration_cast(currentTime - startTime).count(); + + // 5秒后开始尝试检测矩形 + if (elapsedTime > 5 && !rectDetected) { + // 检测外矩形框 + std::vector outerRect; + double outerArea = 0; + + bool detected = detectOuterRect(input_mat, outerRect, outerArea); + + if (detected) { + // 如果是第一次检测到,保存为参考 + if (similarDetectionCount == 0) { + lastDetection.points = outerRect; + lastDetection.area = outerArea; + similarDetectionCount = 1; + std::cout << "Initial rectangle detected. Starting verification..." + << std::endl; + } + // 与上一次检测比较 + else if (areRectsSimilar(lastDetection.points, outerRect, + lastDetection.area, outerArea)) { + similarDetectionCount++; + std::cout << "Matching rectangle detected (" << similarDetectionCount + << "/" << requiredSimilarDetections << ")" << std::endl; + } + // 不相似,重置计数器 + else { + similarDetectionCount = 1; + lastDetection.points = outerRect; + lastDetection.area = outerArea; + std::cout << "Rectangle changed, resetting count." << std::endl; + } + + // 如果连续检测到相同矩形达到5次,确认标定 + if (similarDetectionCount >= requiredSimilarDetections) { + rectDetected = true; + calibratedOuterRect = outerRect; + calibratedOuterArea = outerArea; + std::cout << "Rectangle calibration complete! Using last detected " + "rectangle." + << std::endl; + } + } + } + + // 如果已检测到外矩形,则长期标注它 + if (rectDetected) { + drawFixedRectMarkers(input_mat, calibratedOuterRect, calibratedOuterArea); + } + + // 使用复制的图像进行模型推理(确保没有绘制标记) + auto start_time = high_resolution_clock::now(); + auto results = model.Predict(inference_mat); + auto end_time = high_resolution_clock::now(); + + // 计算推理时间 + auto time_span = duration_cast(end_time - start_time); + std::cout << "Inference time: " << time_span.count() << " ms" << std::endl; + + // 显示检测数量信息 + int totalDetections = results.size(); + + // 绘制所有检测结果(移除置信度过滤) + for (const auto& result : results) { + // 计算目标框中心点 + cv::Point box_center(result.box.x + result.box.width / 2, + result.box.y + result.box.height / 2); + + // 绘制目标框中心十字标记(蓝色) + drawCrossMarker(input_mat, box_center, cv::Scalar(255, 0, 0), 10, 2); + + // 创建置信度文本(保留两位小数) + std::string confText = + "Conf: " + std::to_string(static_cast(result.score * 100)) + "%"; + + // 显示坐标信息 + std::string coordText = "(" + std::to_string(box_center.x) + ", " + + std::to_string(box_center.y) + ")"; + cv::putText(input_mat, coordText, + cv::Point(box_center.x + 10, box_center.y - 10), + cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 0, 0), 1); + + // 在目标框左上角显示置信度 + cv::putText(input_mat, confText, + cv::Point(result.box.x, result.box.y - 5), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 0, 0), 2); + } + + // 在控制台输出检测统计信息 + if (totalDetections > 0) { + std::cout << "Detected " << totalDetections << " objects" << std::endl; + } + + // 打印带标注的图像 + edit.Print(input_mat); + } + + cap.release(); + return 0; +} +``` + +## 4. 编译过程 + +### 4.1 编译环境搭建 + +- 请确保你已经按照 [开发环境搭建指南](../../../../docs/introductory_tutorial/cpp_development_environment.md) 正确配置了开发环境。 +- 同时以正确连接开发板。 + +### 4.2 Cmake介绍 + +```cmake +# CMake最低版本要求 +cmake_minimum_required(VERSION 3.10) + +project(test_lcd) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# 定义项目根目录路径 +set(PROJECT_ROOT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../..") +message("PROJECT_ROOT_PATH = " ${PROJECT_ROOT_PATH}) + +include("${PROJECT_ROOT_PATH}/toolchains/arm-rockchip830-linux-uclibcgnueabihf.toolchain.cmake") + +# 定义 OpenCV SDK 路径 +set(OpenCV_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/opencv-mobile-4.10.0-lockzhiner-vision-module") +set(OpenCV_DIR "${OpenCV_ROOT_PATH}/lib/cmake/opencv4") +find_package(OpenCV REQUIRED) +set(OPENCV_LIBRARIES "${OpenCV_LIBS}") +# 定义 LockzhinerVisionModule SDK 路径 +set(LockzhinerVisionModule_ROOT_PATH "${PROJECT_ROOT_PATH}/third_party/lockzhiner_vision_module_sdk") +set(LockzhinerVisionModule_DIR "${LockzhinerVisionModule_ROOT_PATH}/lib/cmake/lockzhiner_vision_module") +find_package(LockzhinerVisionModule REQUIRED) + + +add_executable(Test_find_Laser Test_find_Laser.cc) +target_include_directories(Test_find_Laser PRIVATE ${LOCKZHINER_VISION_MODULE_INCLUDE_DIRS}) +target_link_libraries(Test_find_Laser PRIVATE ${OPENCV_LIBRARIES} ${LOCKZHINER_VISION_MODULE_LIBRARIES}) + +install( + TARGETS Test_find_Laser + RUNTIME DESTINATION . +) +``` + +### 4.3 编译项目 + +使用 Docker Destop 打开 LockzhinerVisionModule 容器并执行以下命令来编译项目 + +```bash +# 进入Demo所在目录 +cd /LockzhinerVisionModuleWorkSpace/LockzhinerVisionModule/Cpp_example/E01_find_Laser +# 创建编译目录 +rm -rf build && mkdir build && cd build +# 配置交叉编译工具链 +export TOOLCHAIN_ROOT_PATH="/LockzhinerVisionModuleWorkSpace/arm-rockchip830-linux-uclibcgnueabihf" +# 使用cmake配置项目 +cmake .. +# 执行编译项目 +make -j8 && make install +``` + +在执行完上述命令后,会在build目录下生成可执行文件。 + +## 5. 运行结果 + +请先点击下面链接获取模型 + + +```shell +chmod 777 Test_find_Laser +# 在实际应用的过程中LZ-Picodet需要替换为下载的或者你的rknn模型 +./Test-detection LZ-Picodet +``` + +运行结果如下: +![结果](./images/E01.png) +演示视频如下 + + +## 6. 总结 + +本设计方案创新性地融合深度学习与传统视觉技术,成功解决了激光光斑识别过曝、黑胶带背景下识别困难、云台控制精度不足等核心难题:通过PaddleDetection模型实现高鲁棒性激光检测,在640×480分辨率下达到30FPS处理速度,配合5帧稳定的自适应标定机制,为云台控制提供亚像素级定位精度(±1px),有效克服了环境差异导致的“调试成功-测评失败”问题,实测推理时延仅20-30ms,为高性能激光追踪系统提供可靠技术保障。 +改进意见: 在示例中所用模型数据集尺寸较小,如需更复杂的场景应用可以增大数据集重新训练,参考凌智视觉模块首页训练过程进行训练。同时对矩形框的标定较为草率,如需更加准确的标定,可以略微调整阈值达到更好的识别效果。关于矩形框的标定除了应用下位机进行串口控制,还可以使用加入超时检测,例如一段时间进行一次检测,如果检测到则进行更新,如没检测到则维持上次结果,但是可能存在激光干扰的情况希望读者自行尝试。 diff --git a/Cpp_example/E01_find_Laser/Test_find_Laser.cc b/Cpp_example/E01_find_Laser/Test_find_Laser.cc new file mode 100644 index 0000000000000000000000000000000000000000..58ed9f22329f9df2a8383aa17d8c532ca8d24ccf --- /dev/null +++ b/Cpp_example/E01_find_Laser/Test_find_Laser.cc @@ -0,0 +1,282 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono; + +// 绘制十字标记的函数 +void drawCrossMarker(cv::Mat& image, cv::Point center, + cv::Scalar color = cv::Scalar(255, 0, 0), int size = 10, + int thickness = 2) { + // 绘制水平线 + cv::line(image, cv::Point(center.x - size, center.y), + cv::Point(center.x + size, center.y), color, thickness); + + // 绘制垂直线 + cv::line(image, cv::Point(center.x, center.y - size), + cv::Point(center.x, center.y + size), color, thickness); +} + +// 检测画面中最大的外矩形框 +bool detectOuterRect(cv::Mat& frame, std::vector& outerRect, + double& area) { + // 转换为灰度图像 + cv::Mat gray; + cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + + // 应用高斯模糊减少噪声 + cv::GaussianBlur(gray, gray, cv::Size(5, 5), 0); + + // Canny边缘检测 + cv::Mat edges; + cv::Canny(gray, edges, 50, 150, 3); + + // 查找轮廓 + std::vector> contours; + std::vector hierarchy; + cv::findContours(edges, contours, hierarchy, + cv::RETR_EXTERNAL, // 只检测外轮廓 + cv::CHAIN_APPROX_SIMPLE); + + double maxArea = 0; + std::vector largestRect; + + // 查找最大的矩形轮廓(外矩形) + for (size_t i = 0; i < contours.size(); i++) { + // 近似多边形 + std::vector approx; + double epsilon = 0.02 * cv::arcLength(contours[i], true); + cv::approxPolyDP(contours[i], approx, epsilon, true); + + // 筛选四边形 + if (approx.size() == 4 && cv::isContourConvex(approx)) { + double currentArea = cv::contourArea(approx); + if (currentArea > maxArea) { + maxArea = currentArea; + largestRect = approx; + } + } + } + + // 返回检测结果 + if (maxArea > 1000) { + outerRect = largestRect; + area = maxArea; + return true; + } + return false; +} + +// 计算矩形的中心点 +cv::Point calculateRectCenter(const std::vector& rect) { + cv::Point center(0, 0); + for (const auto& pt : rect) { + center.x += pt.x; + center.y += pt.y; + } + center.x /= 4; + center.y /= 4; + return center; +} + +// 检查两个矩形是否相似(中心点位置和面积) +bool areRectsSimilar(const std::vector& rect1, + const std::vector& rect2, double area1, + double area2) { + // 计算中心点 + cv::Point center1 = calculateRectCenter(rect1); + cv::Point center2 = calculateRectCenter(rect2); + + // 计算中心点距离 + double distance = cv::norm(center1 - center2); + + // 计算面积差异 + double areaDiff = std::fabs(area1 - area2); + double areaRatio = areaDiff / std::max(area1, area2); + + // 判断是否相似(阈值可根据实际调整) + return (distance < 20) && (areaRatio < 0.1); +} + +// 绘制固定的外矩形标记 +void drawFixedRectMarkers(cv::Mat& frame, + const std::vector& outerRect, + double area) { + // 绘制外矩形框 + cv::polylines(frame, std::vector>{outerRect}, true, + cv::Scalar(255, 0, 0), 2); +} + +int main(int argc, char* argv[]) { + if (argc != 2) { + std::cerr << "Usage: Test-PaddleDet model_path" << std::endl; + return 1; + } + + // 初始化模型 + lockzhiner_vision_module::vision::PaddleDet model; + if (!model.Initialize(argv[1])) { + std::cout << "Failed to initialize model." << std::endl; + return 1; + } + + lockzhiner_vision_module::edit::Edit edit; + if (!edit.StartAndAcceptConnection()) { + std::cerr << "Error: Failed to start and accept connection." << std::endl; + return EXIT_FAILURE; + } + std::cout << "Device connected successfully." << std::endl; + + // 打开摄像头 + cv::VideoCapture cap; + cap.set(cv::CAP_PROP_FRAME_WIDTH, 640); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); + cap.open(0); + + if (!cap.isOpened()) { + std::cerr << "Error: Could not open camera." << std::endl; + return 1; + } + + // 状态变量 + bool rectDetected = false; + std::vector calibratedOuterRect; + double calibratedOuterArea = 0; + int similarDetectionCount = 0; + const int requiredSimilarDetections = 5; + auto startTime = high_resolution_clock::now(); + + // 存储最近检测到的矩形(用于比较) + struct RectDetection { + std::vector points; + double area; + }; + RectDetection lastDetection; + + cv::Mat input_mat; + while (true) { + // 捕获一帧图像 + cap >> input_mat; + if (input_mat.empty()) { + std::cerr << "Warning: Captured an empty frame." << std::endl; + continue; + } + + // 创建原始图像的副本用于模型推理 + cv::Mat inference_mat = input_mat.clone(); + + auto currentTime = high_resolution_clock::now(); + auto elapsedTime = duration_cast(currentTime - startTime).count(); + + // 5秒后开始尝试检测矩形 + if (elapsedTime > 5 && !rectDetected) { + // 检测外矩形框 + std::vector outerRect; + double outerArea = 0; + + bool detected = detectOuterRect(input_mat, outerRect, outerArea); + + if (detected) { + // 如果是第一次检测到,保存为参考 + if (similarDetectionCount == 0) { + lastDetection.points = outerRect; + lastDetection.area = outerArea; + similarDetectionCount = 1; + std::cout << "Initial rectangle detected. Starting verification..." + << std::endl; + } + // 与上一次检测比较 + else if (areRectsSimilar(lastDetection.points, outerRect, + lastDetection.area, outerArea)) { + similarDetectionCount++; + std::cout << "Matching rectangle detected (" << similarDetectionCount + << "/" << requiredSimilarDetections << ")" << std::endl; + } + // 不相似,重置计数器 + else { + similarDetectionCount = 1; + lastDetection.points = outerRect; + lastDetection.area = outerArea; + std::cout << "Rectangle changed, resetting count." << std::endl; + } + + // 如果连续检测到相同矩形达到5次,确认标定 + if (similarDetectionCount >= requiredSimilarDetections) { + rectDetected = true; + calibratedOuterRect = outerRect; + calibratedOuterArea = outerArea; + std::cout << "Rectangle calibration complete! Using last detected " + "rectangle." + << std::endl; + } + } + } + + // 如果已检测到外矩形,则长期标注它 + if (rectDetected) { + drawFixedRectMarkers(input_mat, calibratedOuterRect, calibratedOuterArea); + } + + // 使用复制的图像进行模型推理(确保没有绘制标记) + auto start_time = high_resolution_clock::now(); + auto results = model.Predict(inference_mat); + auto end_time = high_resolution_clock::now(); + + // 计算推理时间 + auto time_span = duration_cast(end_time - start_time); + std::cout << "Inference time: " << time_span.count() << " ms" << std::endl; + + // 显示检测数量信息 + int totalDetections = results.size(); + + // 绘制所有检测结果(移除置信度过滤) + for (const auto& result : results) { + // 绘制目标框(蓝色矩形) + // cv::rectangle(input_mat, + // cv::Rect(result.box.x, result.box.y, result.box.width, + // result.box.height), + // cv::Scalar(255, 0, 0), // BGR颜色:蓝色 + // 2); // 线宽 + + // 计算目标框中心点 + cv::Point box_center(result.box.x + result.box.width / 2, + result.box.y + result.box.height / 2); + + // 绘制目标框中心十字标记(蓝色) + drawCrossMarker(input_mat, box_center, cv::Scalar(255, 0, 0), 10, 2); + + // 创建置信度文本(保留两位小数) + std::string confText = + "Conf: " + std::to_string(static_cast(result.score * 100)) + "%"; + + // 显示坐标信息 + std::string coordText = "(" + std::to_string(box_center.x) + ", " + + std::to_string(box_center.y) + ")"; + cv::putText(input_mat, coordText, + cv::Point(box_center.x + 10, box_center.y - 10), + cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 0, 0), 1); + + // 在目标框左上角显示置信度 + cv::putText(input_mat, confText, + cv::Point(result.box.x, result.box.y - 5), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 0, 0), 2); + } + + // 在控制台输出检测统计信息 + if (totalDetections > 0) { + std::cout << "Detected " << totalDetections << " objects" << std::endl; + } + + // 打印带标注的图像 + edit.Print(input_mat); + } + + cap.release(); + return 0; +} \ No newline at end of file diff --git a/Cpp_example/E01_find_Laser/images/E01.png b/Cpp_example/E01_find_Laser/images/E01.png new file mode 100755 index 0000000000000000000000000000000000000000..1a0aeebfccd5cc7669d099e9e738048697b41646 Binary files /dev/null and b/Cpp_example/E01_find_Laser/images/E01.png differ diff --git a/README.md b/README.md index 7abfe57e392458704bea66acd84036182ee2459f..ee2e2d7250b539b7654d2b1be613c5965b3e6bc6 100644 --- a/README.md +++ b/README.md @@ -174,7 +174,7 @@ C++ 开发案例以A、B、C、D进行不同类别进行分类,方便初学者 * `B01 - B99`: OpenCV基础函数类 * `C01 - C99`: 传统OpenCV识别类 * `D01 - D99`: 神经网络模型类 - +* `E01 - E99`: 使用示例类 例程列表如下所示: | 编号 | 类别 | 例程名 | 说明 | @@ -208,6 +208,9 @@ C++ 开发案例以A、B、C、D进行不同类别进行分类,方便初学者 | D10 | 神经网络类 | YOLOv5 | [YOLOv5目标检测](./Cpp_example/D10_yolov5//README.md) | | D11 | 神经网络类 | PPOCRv3 | [文字识别](./Cpp_example/D11_PPOCRv3/README.md) | | D12 | 神经网络类 | PPOCRv4-Det | [文字检测](./Cpp_example/D12_ppocrv4_det/README.md) | +| D13 | 神经网络类 | target_tracking | [多目标跟踪](./Cpp_example/D13_target_tracking/README.md)| +| E01 | 使用示例类 | Test_find_Laser | [激光跟踪](./Cpp_example/E01_find_Laser/README.md)| + ## 🐛 Bug反馈 如果您遇到问题,您可以前往 [Lockzhiner Vision Module Issues](https://gitee.com/LockzhinerAI/LockzhinerVisionModule/issues) 并点击已完成按钮查看其他用户反馈且我们已经解决的 Bug。