commit 56985997aef24bec99572c66e7afc28277500cb0
Author: lyf <169361657@qq.com>
Date: Sat Mar 21 11:57:34 2026 +0800
对上交代码进行修改,主要将能量机关去掉,添加了同济的PnP位姿解算,但是同济有个四元数,获取IMU部分没有启用,可能导致精度不够。当前还存在反陀螺功能,修改为逻辑和弹道预测相结合,主要在时间关系上进行调整。
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0a74973
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,15 @@
+cmake-build-debug
+build
+.idea
+.xmake
+.vscode/.cache
+.vscode/compile_commands.json
+Mark
+armor_box_photo
+tools/TrainCNN/.idea
+tools/TrainCNN/model
+tools/TrainCNN/__pycache__
+others/include/config/config.h
+others/MV-UB31-Group0.config
+.DS_Store
+video
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..aa46b6d
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,8 @@
+{
+ "clangd.arguments": [
+ "--compile-commands-dir=${workspaceFolder}/build",
+ "--completion-style=detailed",
+ "--query-driver=/usr/bin/clang",
+ "--header-insertion=never"
+ ],
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..5123de0
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,61 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
+
+PROJECT(SJTU-RM-CV)
+SET(CMAKE_CXX_STANDARD 11)
+SET(CMAKE_BUILD_TYPE RELEASE)
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")
+SET(BIN_NAME "run")
+
+SET(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+
+FIND_FILE(CONFIG_FOUND "config.h" "others/include/config")
+if (CONFIG_FOUND)
+ MESSAGE("Found config.h")
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CONFIG")
+endif()
+
+
+FIND_PROGRAM(CCACHE_FOUND ccache)
+IF(CCACHE_FOUND)
+ SET_PROPERTY(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+ SET_PROPERTY(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
+ MESSAGE("< Use ccache for compiler >")
+ENDIF()
+
+FIND_PACKAGE(Eigen3 REQUIRED)
+FIND_PACKAGE(OpenCV 4 REQUIRED)
+FIND_PACKAGE(Threads)
+FIND_PACKAGE(yaml-cpp REQUIRED)
+
+
+LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others)
+
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include)
+INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include)
+
+FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "armor/src/*.cpp")
+ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles})
+
+TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
+TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS})
+TARGET_LINK_LIBRARIES(${BIN_NAME} yaml-cpp)
+
+IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
+ MESSAGE(STATUS "current platform: Linux ")
+ TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so")
+ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Windows")
+ MESSAGE(STATUS "current platform: Windows")
+ TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")
+ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Darwin")
+ MESSAGE(STATUS "current platform: Mac")
+ TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libmvsdk.dylib")
+ELSE ()
+ MESSAGE(STATUS "Unsupport platform: ${CMAKE_SYSTEM_NAME}")
+ENDIF()
+
+ADD_CUSTOM_TARGET(create-startup COMMAND "${PROJECT_SOURCE_DIR}/tools/create-startup.sh" "${PROJECT_SOURCE_DIR}" "${CMAKE_BINARY_DIR}")
+ADD_CUSTOM_TARGET(train-cnn COMMAND "gnome-terminal" "--" "bash" "-c" "${PROJECT_SOURCE_DIR}/tools/TrainCNN/backward.py")
\ No newline at end of file
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..8510013
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 xinyang
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..ef1a302
--- /dev/null
+++ b/README.md
@@ -0,0 +1,187 @@
+# 上海交通大学 RoboMaster 2019赛季 视觉代码
+
+本代码是上海交通大学RoboMaster2019赛季步兵车辆的视觉部分,分为三个模块:**装甲板识别**,**能量机关**,以及**封装的设备驱动和配置文件**。可以提取能量机关以外的模块并修改main函数直接作为哨兵识别代码。
+
+本代码统一使用**640×480**大小的图像进行处理
+
+| 作者 | 负责部分 | 微信号 |
+| ------ | -------------- | -------------------- |
+| 唐欣阳 | 自瞄装甲板识别 | xinyang_tang |
+| 卫志坤 | 自瞄装甲板识别 | |
+| 孙加桐 | 能量机关识别 | SJTxixiliadorabarryU |
+| 罗嘉鸣 | 能量机关识别 | |
+
+**如有BUG或者想交流的朋友欢迎积极联系我们**
+
+**分享部分比赛时摄像头录制的视频:**
+
+链接: https://pan.baidu.com/s/1LwxEpeYYblX3cSzb59MTVg 提取码: 84ju 复制这段内容后打开百度网盘手机App,操作更方便哦
+
+---
+
+运行效果:自瞄帧率120(摄像头最大帧率),识别距离根据环境不同大约8米左右(5mm焦距镜头)。
+
+
+
+
+## 一、代码运行环境
+
+| 硬件设备 | 操作系统 | 运行库 | ToolChain |
+| ---------------------------------------------------- | -------------------------------------------- | ------------------------------------------------------------ | ---------------------------------------------------------- |
+| IntelNUC
MindVision工业相机×1
USB转TTL×1 | Ubuntu18.04
Ubuntu16.04
Windows10 | OpenCV3.4.5
OpenCV_contrib3.4.5
Eigen3
MindVision相机驱动 | Ubuntu18/16 : cmake3+gcc7+g++7
Win10 : cmake3+VS2019 |
+
+MindVision相机型号:MV-UBS31GC
+
+**关于Windows环境下的运行支持,仅保证程序可以编译运行。对与部分辅助功能,如生成自启动脚本则不支持。**
+
+**实际装载在步兵和哨兵上的运行环境为Ubuntu18.04。**
+
+相机驱动下载地址:[相机驱动](https://www.mindvision.com.cn)
+
+OpenCV下载地址:[OpenCV](https://github.com/opencv)
+
+OpenCV安装教程 : [linux](https://docs.opencv.org/3.4.5/d7/d9f/tutorial_linux_install.html) [Windows](https://docs.opencv.org/3.4.5/d3/d52/tutorial_windows_install.html)
+
+Eigen下载方法:
+* Ubuntu16/18: ```sudo apt install libeigen3-dev```
+* Windows10 : [Eigen下载地址](http://eigen.tuxfamily.org/)
+
+## 二、程序编译运行以及调试方式
+
+### 1.编译运行
+
+* Ubuntu16/18(在项目文件夹下)
+
+```shell
+mkdir build
+cd build
+cmake ..
+make -j8
+sudo ./run
+```
+
+* Windows10
+
+ 打开cmake-gui,选择项目文件夹和build文件夹,生成VS工程。在VS中编译项目。
+
+### 2.调试方式
+
+```./run --help```可以查看所有命令行参数及其作用。所有调试选项都集成到了命令行参数中。
+
+**不使用任何参数直接运行将没有任何图像显示。**
+
+需要调参的部分:主要需要根据车辆情况而调参的参数存放在others/include/config/setconfig.h中
+
+### 3.工作条件
+
+* 对于自瞄,由于使用了数字识别,务必保证光照充足,图像上数字清晰可见。光照不足时,调整摄像头曝光或增益数值,直到数字清晰可见。
+* 务必保证摄像头焦距正确,同时镜片干净无污物。
+
+## 三、文件目录结构
+```
+.
+├── armor // 存放自瞄主要算法代码
+│ ├── include // 自瞄头文件
+│ └── src // 自瞄源码
+├── CMakeLists.txt // cmake工程文件
+├── energy // 存放能量机关主要算法代码
+│ ├── include // 能量机关头文件
+│ └── src // 能量机关源码
+├── main.cpp // 主函数
+├── others // 存放摄像头、串口、配置文件等
+│ ├── include // others头文件
+│ ├── libmvsdk.dylib // mac相机驱动链接库
+│ ├── libMVSDK.so // linux相机驱动链接库
+│ ├── MVCAMSDK_X64.dll // win10相机驱动链接库
+│ ├── MV-UB31-Group0.config // 相机配置文件
+│ └── src // others源码
+└── tools // 存放分类器训练代码及参数,自启动脚步等
+ ├── auto-pull.sh // 自动代码更新脚本
+ ├── create-startup.sh // 自启动文件创建脚本
+ ├── monitor.bat // win10进程守护脚本
+ ├── monitor.sh // linux进程守护脚本
+ ├── para // 分类器参数
+ └── TrainCNN // 分类器训练源码
+```
+## 四、关键类解析
+
+| 类名 | 主要成员 | 主要接口 | 类的作用 |
+| --------------- | ------------------------------------------------------------ | ---------------------- | ------------------------------------------------------- |
+| ArmorFinder | 过多,不做赘述 | void run(cv::Mat &src) | 将一帧图像中装甲板的detection以及数据发送封装为一个类 |
+| Energy | 过多,不做赘述 | void run(cv::Mat &src) | 将一帧图像中能量机关的detection以及数据发送封装为一个类 |
+| EnergyPartParam | 过多,不做赘述 | 无 | 能量机关所有参数的集合 |
+| LightBlob | 灯条位置
灯条颜色 | 无 | 灯条类定义 |
+| ArmorBox | 装甲板位置
装甲板的两个灯条
装甲板颜色
装甲板数字id | 无 | 装甲板类定义 |
+
+## 五、程序运行基本流程
+
+ ↗ 大能量机关 ↘
+
+各项初始化→读取当前状态 → 小能量机关 → 回到读取状态
+
+ ↓ ↘ 自瞄 ↗
+
+数据接收线程
+
+## 六、识别方式
+
+### 1.自瞄装甲板识别方式
+
+ 首先对图像进行通道拆分以及二值化操作,再进行开闭运算,通过边缘提取和条件限制得出可能为灯条的部分。再对所有可能的灯条进行两两匹配,根据形状大小特性进行筛选,得出可能为装甲板的候选区。然后把所有候选区交给分类器判断,得出真实的装甲板及其数字id。最后根据优先级选取最终击打目标以及后续处理。
+ 
+### 2.能量机关识别方式
+
+ 首先对图像进行二值化操作,然后进行一定腐蚀和膨胀,通过边缘提取和条件限制得出待击打叶片(锤子形)。在待击打叶片范围内进一步用类似方法寻找目标装甲板和流动条,在二者连线上寻找中心的“R”。根据目标装甲板坐标和中心坐标计算极坐标系下的目标角度,进而预测待击打点的坐标(小符为装甲板本身,大符需要旋转)。最后将待击打点坐标和图像中心的差值转换为yaw和pitch轴角度,增加一环PID后发送给云台主控板。
+
+## 七、通信协议
+
+### 1.通信方式
+
+使用USB转TTL进行串口通信
+
+### 2.数据接收结构体
+
+```c++
+struct McuData {
+ float curr_yaw; // 当前云台yaw角度
+ float curr_pitch; // 当前云台pitch角
+ uint8_t state; // 当前状态,自瞄-大符-小符
+ uint8_t mark; // 云台角度标记位
+ uint8_t anti_top; // 是否为反陀螺模式
+ uint8_t enemy_color; // 敌方颜色
+ int delta_x; // 能量机关x轴补偿量
+ int delta_y; // 能量机关y轴补偿量
+};
+```
+
+每个数据帧后使用字符```'\n'```作为帧尾标志
+
+### 3.数据发送结构体
+
+```c++
+struct SendData {
+ char start_flag; // 帧头标志,字符's'
+ int16_t yaw; // float类型的实际角度(以度为单位)/100*(32768-1)
+ int16_t pitch; // float类型的实际角度(以度为单位)/100*(32768-1)
+ uint16_t shoot_delay; // 反陀螺模式下的发射延迟
+ char end_flag; // 帧尾标识,字符'e'
+};
+```
+
+实际发送代码中没有使用这个结构体,而是使用uint8_t类型数组直接赋值代替
+
+## 八、代码命名规范
+
+函数名:使用首字母小写的驼峰命名法
+
+类型名:使用首字母大写的驼峰命名法
+
+变量名:使用下划线分割命名法
+
+## 九、未来的优化方向
+* 基于灯条的候选区生成由于利用了灯条在其附近区域亮度是最高的这一先验知识,当摄像头曝光和增益较高,同时灯条旁边有其他较亮的物体(如反光、光晕过大等),该灯条将无法被识别。反应在算法上即体现在二值化这一步骤之上。所以本项目采用多二值化阈值同时进行灯条提取最后进行合并的办法,有一定改善,但没有解决本质问题。**所以第一个优化方向便是环境适应力更强的灯条提取或者候选区提取**。
+* 代码中使用CNN分类器对灯条候选区进行筛选以及数字识别,但分类器总是不能做到100%的正确率,同时许多明显为背景的候选区有时也会被误判成装甲板,导致误识别。**所以第二个优化方向是更低的误识别率(可以从候选区生成入手或者从分类器入手)**。
+* 由于陀螺这样的机械设计基本上是强队的标准配置,所以我们也首次尝试了从视觉层面对陀螺有一个专门的击打方式,但实际应用场景较少(当前仅针对原地旋转的陀螺),且对操作手不太友好(要求手动对准陀螺中心),所以没有取得很大的实战价值。**所以第三个优化方向便是一个有着自动击打动态陀螺的系统**。
+
+---
+**觉得对你有帮助请点个STAR哦:)**
diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h
new file mode 100644
index 0000000..13b052f
--- /dev/null
+++ b/armor/include/armor_finder/armor_finder.h
@@ -0,0 +1,155 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#ifndef _ARMOR_FINDER_H_
+#define _ARMOR_FINDER_H_
+
+#include