RoboMaster 之 深大源码RP_Infantry_Plus阅读整理

RoboMaster 之 深大源码RP_Infantry_Plus阅读整理

闲来垂钓碧溪上。这回遇到硬茬了，任务是啃完全套RM代码，想必是一场恶战，记录一下笔记：

一、万事万物从Readme开始

1.功能介绍

本套代码完成了对RoboMaster2019赛场上大小符以及机器人装甲板的识别，通过自定的通讯协议将视觉处理后的信息发送给下位机，由单片机处理视觉信息并控制云台的运动

---

2.效果展示

① 装甲板检测：

在6米内的识别准确率几乎100%，辅以ROI操作和多线程，处理一张图片的时间在1ms以内，跑整套代码仅需4-6ms，能够做到实时检测

② 大小符识别：

深度学习和传统方法结合，可以做到实时预测旋转之后的位置，加上roi和多线程操作之后，处理一张图片的时间在2ms之内，加上读图也只需要4-6ms。

---

3.环境依赖

• ubuntu 16.04.3
• OpenCV 3.4.4
• Qt Creator 5.10.1
• CMake 3.5.1
• GCC 5.4.0

---

4.文件架构

深大开源/
├── depends（系统动态链接库）
├── main.cpp（主函数：1.引入配置文件 2.设置串口 3.设置多线程）
├── extraFile
│   ├── AimXMl
│   │   ├── calib_no_4_1280.yml（标定配置）
│   │   └── param_config.yml（参数配置）
│   ├── caffemodel（caffe框架网络结构）
│   │   ├── armornet_iter_200000.caffemodel
│   │   ├── deploy.prototxt
│   │   ├── lenet_iter_200000.caffemodel
│   │   └── lenet_train_test_deploy.prototxt
│   └── Rune
│       ├── lenet（lenet网络结构）
│       │   ├── deploy.prototxt
│       │   ├── lenet_iter_80000.caffemodel
│       │   ├── lenet_iter_80000加了负样本.caffemodel
│       │   └── lenet_iter_80000_旋转后.caffemodel
│       ├── xml
│       │   └── Mono_out.xml
│       └── yolo（yolov2训练结果）
│           ├── tiny-yolov2-trial3-noBatch_235000.weights
│           ├── tiny-yolov2-trial3-noBatch_80000.weights
│           ├── tiny-yolov2-trial3-noBatch.cfg
│           └── 单通道
│               ├── tiny-yolov2-trial3-noBatch_80000.weights
│               └── tiny-yolov2-trial3-noBatch.cfg
├── include
│   ├── Aim
│   │   ├── AngleSolver.hpp（RectPnPSolver及其子类AngleSolver的定义）
│   │   ├── ArmorDetector.hpp（struct ArmorParam/struct matched_rect/struct candidate_target/class ArmorDetector定义）
│   │   └── Settings.hpp（Settings类定义）
│   ├── Camera
│   │   └── video.h（static uint32_t gFormatTransferTbl[]、class Video、class FrameBuffer）
│   ├── header.h（opencv、c++、Qt等头文件）
│   ├── ImageConsProd.hpp（ImageConsProd类定义）
│   ├── Rune（class Detect）
│   │   ├── Detect.h
│   └── SerialPort
│       ├── CRC_Check.h
│       └── serialport.h（class SerialPort）
├── includes （相机 SDK 包含头文件）
├── RP_Infantry_Plus.pro
├── RP_Infantry_Plus.pro.user
└── src
    ├── Aim
    │   ├── AngleSolver.cpp
    │   └── ArmorDetector.cpp
    ├── Camera
    │   └── video.cpp
    ├── ImageConsProd.cpp
    ├── Rune
    │   ├── Detect.cpp
    └── SerialPort
        ├── CRC_Check.cpp
        └── serialport.cpp

---

5.文件描述

文件名	用途
AngleSolver.cpp	对目标进行角度解算（DJI开源）
ArmorDetector.cpp	进行装甲板检测
video.cpp	将工业相机 SDK 封装成类
ImageConsProd.cpp	处理多线程以及数据传输
Detect.cpp	进行大小符装甲检测
serialport.cpp	与下位机通讯的通讯协议

---

6.实现方案（重点标注为学习点）

(1) 装甲板识别

1. 自瞄流程图：

2. 基本原理：

   ① 如果 last_result 为 true，则检测区域在上一帧的附近，也就是用到了 ROI 方法，如果超过一定帧数未检测到目标，则逐渐扩大搜索范围，直到33帧仍然未检测到目标则全图搜索，此时将与 ROI 有关的变量全部清零

   ② 对检测区域内的图进行二值化，然后再用某种方法将图片中红色或蓝色的区域提取出来，之后膨胀，跟灰度二值化图片逻辑与，最后得到一张只有红/蓝灯条的二值图。提取红蓝区域的方法有以下两种：

   • 一种方法是用 RGB 的红蓝通道相减，根据设定的阈值得到一张二值图，这种方法虽好，但是在识别蓝色的时候，有时候无法排除掉日光灯干扰，除此之外，操作简洁，耗时低，国赛选用这种方法。
   • 另一种是先将图片转化成 HSV 颜色空间再用通道范围将红色/蓝色提取出来（网上可找到对应的表），这种方法可以排除很多干扰，但是近距离的时候装甲板灯条发白，如果膨胀不到位会出现灯条断裂的情况，膨胀的卷积核过大又会造成预处理耗时过久，因此要权衡一下。

   ③ 在当前二值图内找到所有的轮廓点，用最小旋转矩形将他们包围，此时得到一个个单独的旋转矩形，根据装甲板灯条的几何特征首先筛除掉一些旋转矩形

   bool if1 = (fabs(rrect.angle) < 45.0 && rrect.size.height > rrect.size.width); // 往左
   bool if2 = (fabs(rrect.angle) > 60.0 && rrect.size.width > rrect.size.height); // 往右
   bool if3 = max_rrect_len > _para.min_light_height; // 灯条的最小长度+
   bool if4 = (max_rrect_len / min_rrect_len >= 1.1) && (max_rrect_len / min_rrect_len < 15); // 灯条的长宽比
   

   ④ 将这些灯条两两再组成一个大的旋转矩形（也就是候选装甲板），根据一些限制条件筛除掉不符合条件的装甲板，将剩下的待选装甲板放入一个向量中。（其中利用灯条的角度差信息angleabs 为大连交通大学开源）

   bool condition1 = delta_h > _para.min_light_delta_h && delta_h < _para.max_light_delta_h;
   bool condition2 = MAX(leni, lenj) >= 113 ? abs(yi - yj) < 166\
       && abs(yi - yj) < 1.66 * MAX(leni, lenj) :
   	abs(yi - yj) < _para.max_light_delta_v\
       && abs(yi - yj) < 1.2 * MAX(leni, lenj); 
   bool condition3 = lr_rate < _para.max_lr_rate;
   bool condition4 = sentry_mode ? angleabs < 25 : angleabs < 15 - 5; // 给大点防止运动时掉帧
   

   ⑤ 如果向量中没有元素，则说明没有找到目标；如果只有一个，就是最终选择的装甲板；有两个以上元素的话，就要进行进一步筛选，最容易误识别的地方为装甲板灯条和它两边的两根灯条，经过分析，我们发现在这种情况下如果用 ROI 操作，包围真正装甲板的那个旋转矩形的角度是最小的，我们可以根据这个特征进行筛选，具体思路请看代码。

   ⑥ 找到装甲板之后对其进行 pnp 解算，得到云台转至其中间所需要的 pitch 和 yaw 值，角度解算请参考 DJI 开源的算法，距离信息我们实际测量得到了灯条的长度和实际距离的函数关系，直接代入即可求得距离，误差在 20cm 之内。

   ⑦ 哨兵比较特殊，我们对其进行特殊处理，如果接收到电控传来哨兵模式，我们会对其装甲板匹配条件进行宽松处理，并且对其使用多分类识别装甲板贴纸上的 ID ，在10帧之内如果未找到目标依旧给电控发送找到目标的标志位，直到超过上述帧数才发送未识别到目标，这样可以防止灯条被打灭就失去目标导致云台突然停一下的问题，打灭了之后云台依旧会向前运动，可以快速推掉哨兵。

   

(2) 大小符击打

1. 流程图：

2. 基本原理：

   ① 在 include/Detect.h 中的 sParam.use_yolo 变为 1，则使用 yolo 来检测没有打过的扇叶，若为 0 则根据 lastData 是否有效来进行 ROI 区域的检测。

   ② 对 ROI 区域或者整图进行二值化，在 src/Detect.h 可以选择二值化的方法

   ③ 检测所有轮廓，统计所有轮廓的子轮廓的个数，排除子轮廓数目大于 1 的轮廓，以及通过面积和长宽比进行排除，得到候选的扇叶

   bool condition1 =  whrio < param.flabellum_whrio_max && whrio > param.flabellum_whrio_min;// 长宽比
   bool condition2 = area > param.flabellum_area_min;// 最小面积
   

   ④ 如果得到的扇叶个数大于 1，可以使用 lenet 或者选择面积最小的得到最终的未打过的扇叶。在 include/Detect.h 里面的 sParam.use_lenet 可以控制是否使用 lenet 进行分类

   ⑤ 在得到的最终扇叶中，再次寻找所有轮廓，寻找装甲板的中心。其中的条件为长宽比，矩匹配，面积筛选

   bool condition1 = whrio < param.armor_whrio_max && whrio > param.armor_whrio_min;// 长宽比
   bool condition2 = rev < param.armor_rev_thres;// 矩匹配阈值
   bool condition3 = area > param.armor_area_min;// 最小面积
   

   ⑥ 如果为小符，就可以不用进行预测，直接将数据发送给下位机，但如果是大符，就需要进行圆周预测，预测方法是根据旋转矩形的角度预测出切线的位置，然后再绕着装甲点旋转一定角度，近似到圆周上,由于预测角度小，所以预测点和真实打击点基本吻合

   ⑦ 对得到的装甲板数据进行保存和发送，其中装甲板的象限数据是通过箭头坐标以及旋转矩形的角度进行确定，圆心是通过象限以及给定的半径进行确定

   

---

7.通讯协议

自定的通讯协议一共有 30 个字节，除去校验和预留的数据位，还有 12 个字节的 float 型数据 和 6 个字节的标志位可供使用，能够完成大部分数据的传输。

Byte0	Byte1	Byte2	Byte3	Byte4	Byte5	Byte6
0xA5	cmdID	CRC8_Check	pitch_data	pitch_data	pitch_data	pitch_data
Byte10	Byte11	Byte12	Byte13	Byte14	Byte15	Byte16
yaw_data	dist_data	dist_data	dist_data	dist_data	flag1	flag2

• 0xA5 -帧头
• cmdID : 8 bit int - 命令模式（0 不处理，1 为红色自瞄，2 为蓝色自瞄， 3 4 5 6 7 8 为大小符）
• pitch_data : 32 bit float - 接收视觉解算出来的云台 pitch 值
• yaw_data : 32 bit float - 接收视觉解算出来的云台 yaw 值
• dist_data : 32 bit flaot - 接收视觉解算目标到相机的距离值
• flag1 : 8 bit int - 是否瞄准到中心（大小符用） / 哨兵模式 （stm32 -> PC）
• flag2 : 8 bit int - 是否找到目标（自瞄）/ 吊基地模式（stm32 -> PC）
• flag3 : 8 bit int - 是否识别到大小符
• flag4 : 8 bit int - 是否击打过大小符
• flag5 : 8 bit int - 装甲板是否贴脸（已弃用）

---

8.配置与调试

(1) 配置：

此代码除了相机可能与其他学校不同，其他只需要修改一下路径即可运行，需要修改的路径如下：

• main.cpp 中的 config_file_name
• src/Aim/ArmorDetector.hpp 中的 net
• include/header.h 中的 camera_yaml yolo_model_file yolo_txt_file lenet_model_file lenet_txt_file
• extraFile/AimXMl/param_config.yml 中的 intrinsic_file_720

刚开始的配置参数是集合在一个 yaml 文件中，在实际调车过程中，发现还是直接修改程序中的代码更快捷方便一些，因此后续几乎没有用过 yaml 进行配置，当然，为了程序规范，建议还是用外部配置文件来更改参数，以下是深大之前用的 yaml 配置文件（已弃用），有需要可以在文件中添加配置或者删除配置。

%YAML:1.0
---
# For Debug Image
show_image: 1
save_result: 0
# Parameter for Armor Detection System
min_light_height: 10
light_slope_offset: 30
max_light_delta_h: 720
min_light_delta_h: 20
max_light_delta_v: 100
max_light_delta_angle: 30
near_face_v: 100
max_lr_rate: 1.99
max_wh_ratio: 5.12
min_wh_ratio: 1.03
target_max_angle: 20
small_armor_wh_threshold: 3.33
binary_classfication_threshold: 166
# Parameter for Camera
intrinsic_file_720: "/home/nuc/kevin/RP_Infantry_Plus/extraFile/AimXMl/calib_no_4_1280.yml"

(2) 自启动：

赛场上只有三分钟准备时间，因此我们需要让程序在 PC 开机之后自启动，这就需要写一个自启动脚本，并且，为了防止程序异常中断，我们还用了看门狗来保护程序，原理就是不断检测程序是否在运行，若没有运行，则立刻运行，如果在运行，就隔一段时间再去检测。同样，只需要修改一下路径就可以运行。（我们发现PC 意外断电时有几率将二进制文件给损坏，导致程序无法运行，因此为了保险起见，在开机时重新 make 一下程序），为了防止极端情况，在程序意外中断 10 次之后我们直接重启 PC （一般不会遇到这种情况）。

#!/bin/bash

sec=1
cnt=0
name=RP_Infantry_Plus
Thread=`ps -ef | grep $name | grep -v "grep"`
cd /home/s305-nuc5/kevin/build-$name-Desktop-Debug/
make clean && make -j
while [ 1 ]
do
count=`ps -ef | grep $name | grep -v "grep" | wc -l`
echo "Thread count: $count"
echo "Expection count: $cnt"
if [ $count -gt 1 ]; then
	echo "The $name is still alive!"
	sleep $sec
else 
	echo "Starting $name..."
	cd ~ && ./ttyUSB.sh
    cd /home/s305-nuc5/kevin/build-$name-Desktop-Debug/
    gnome-terminal -x bash -c "./$name;exec bash;"
    echo "$name has started!"		
	sleep $sec
	((cnt=cnt+1))
	if [ $cnt -gt 9 ]; then
		reboot
	fi
fi
done

其中的 ttyUSB.sh 是对 TTL-USB 串口赋权限用的，内容如下，默认串口为 /dev/ttyUSB0，因为串口松动可能会变成 /dev/ttyUSB1，因此我们对两个串口都赋予权限

#!/bin/bash
echo ubuntu|sudo -S sudo chmod +777 /dev/ttyUSB0
echo ubuntu|sudo -S sudo chmod +777 /dev/ttyUSB1

只需将 watchDog.sh 添加到 ubuntu 系统的 StartUp Applications 中就可以实现开机自启动程序。

(3) 调试：

• 装甲板识别：

由于调试时需要知道所处环境对曝光的适应程度，并且要输出一些信息来帮助我们判断是否有误识别，因此在 src/Aim/ArmorDetector.cpp 和 src/ImageConsProd.cpp 中定义了多个宏定义开关方便调试，在正式比赛时要将这些开关关闭，否则会影响算法的实时性。

① 开启 src/Aim/ArmorDetector.cpp 中的 SHOW_DEBUG_IMG 宏定义，展示代码每一步的图片处理细节

② 开启 src/Aim/ArmorDetector.cpp 中的 COUT_LOG 宏定义，输出匹配灯条的信息

③ 开启 src/Aim/ArmorDetector.cpp 中的 CLASSIFICATION 宏定义，使用多分类识别装甲板（已弃用）

④ 开启 src/ImageConsProd.cpp 中的 SHOW_IMAGE 宏定义，展示程序最终选择的目标装甲板图

⑤ 开启 src/ImageConsProd.cpp 中的 COUT 宏定义，输出读一张图以及处理一张图所耗费的时间

⑥ 开启 src/ImageConsProd.cpp 中的 SERIAL_DEBUG 宏定义，强制选择自瞄或大小符模式，正式比赛务必关闭这个开关

⑦ 开启 src/ImageConsProd.cpp 中的 CAP_VIDEO 宏定义，在程序运行的同时将采取到的图片帧保存为视频存入本地

• 大小符识别：

由于在赛场的时候，灯光等场地因素对我们的图像处理有影响，因此我们也需要通过一些信息来判断程序是否成功识别成功，因此 include/Detect.h 里有个调试的开关。同时，在开启这个开关之后，对算法的实时性也有影响。

① 把 include/Detect.h 中的 sParam.debug 变成1，将会输出二值化后的图，以及程序处理过程中的细节以及得到的结果。

② 开启 src/ImageConsprod.cpp 中的 RUNE_CAP_VIDEO宏定义，在程序运行的同时将采取到的图片帧保存为视频存入本地。

二、装甲板识别 ArmorDetector

ArmorDetector::getTargetAera() 相当于 ArmorDetector 的主函数，因此作者根据该函数的逻辑来整理其中包含的函数及学习点

cv::RotatedRect ArmorDetector::getTargetAera(const cv::Mat & src, const int & sb_mode, const int & jd_mode)
{
    // 传入参数为哨兵模式和吊射基地模式
    sentry_mode = sb_mode;
    base_mode = jd_mode;
    setImage(src);  // function called
    vector match_rects;
    findTargetInContours(match_rects);  // function called
    RotatedRect final_rect = chooseTarget(match_rects, src);  // function called

    if(final_rect.size.width != 0)
    {
        // 最后才加上了偏移量，前面那些坐标都是不对的，所以不能用前面那些函数解角度
        final_rect.center.x += _dect_rect.x;
        final_rect.center.y += _dect_rect.y;
        _res_last = final_rect;
        _lost_cnt = 0;
    }
    else
    {
        _find_cnt = 0;
        ++_lost_cnt;

        // 逐次加大搜索范围（根据相机帧率调整参数）
        if (_lost_cnt < 8)
            _res_last.size = Size2f(_res_last.size.width, _res_last.size.height);
        else if(_lost_cnt == 9)
            _res_last.size =Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if(_lost_cnt == 12)
            _res_last.size = Size2f(_res_last.size.width * 2, _res_last.size.height * 2);
        else if(_lost_cnt == 15)
            _res_last.size = Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if (_lost_cnt == 18)
            _res_last.size = Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if (_lost_cnt > 33 )
            _res_last = RotatedRect();
    }
    return final_rect;
}

1. 算法区分哨兵模式和吊射基地模式

sentry_mode = sb_mode;
base_mode = jd_mode;

2. void ArmorDetector::setImage(const cv::Mat & src)

(1) bool makeRectSafe(cv::Rect & rect, cv::Size size) :

保证输入矩形rect不会越界(size)

(2) 利用ROI分割方法避免多次重复检测：

如果上一次的目标消失，则删除一切与ROI有关的变量；如果上一次的目标没有消失，则用直立矩形包围上一次的旋转矩形

if(last_result.x == 0 || last_result.y == 0)
{
    _src = src;
    _dect_rect = Rect(0, 0, src.cols, src.rows);
}
else
{
    Rect rect = _res_last.boundingRect();
    ...
}

(3) 对检测区域内图像进行二值化，提取红色或蓝色区域：

法①：用 RGB 的红蓝通道相减，根据设定的阈值得到一张二值图，这种方法虽好，但是在识别蓝色的时候，有时候无法排除掉日光灯干扰，除此之外，操作简洁，耗时低，国赛选用这种方法

法②：先将图片转化成 HSV 颜色空间再用通道范围将红色/蓝色提取出来这种方法可以排除很多干扰，但是近距离的时候装甲板灯条发白，如果膨胀不到位会出现灯条断裂的情况，膨胀的卷积核过大又会造成预处理耗时过久，因此要权衡一下。

3. void ArmorDetector::findTargetInContours(vector& match_rects)：

match_rects 向量存储匹配成功的左右两个小矩形

① 倾斜方向

bool if1 = (fabs(rrect.angle) < 45.0 && rrect.size.height > rrect.size.width); // 往左
        bool if2 = (fabs(rrect.angle) > 60.0 && rrect.size.width > rrect.size.height); // 往右


if(anglei > 45.0 && anglej < 45.0){ // 八字 / \   //
    angleabs = 90.0 - anglei + anglej;
}else if(anglei <= 45.0 && anglej >= 45.0){ // 倒八字 \ /
    angleabs = 90.0 - anglej + anglei;
}else{
    if(anglei > anglej) angleabs = anglei - anglej; // 在同一边
    else angleabs = anglej - anglei;
}

② 长宽

bool if3 = max_rrect_len > _para.min_light_height; // 灯条的最小长度
if (!base_mode) // 吊射基地时条件不同
    if4 = (max_rrect_len / min_rrect_len >= 1.1) && (max_rrect_len / min_rrect_len < 15); // 灯条的长宽比
else
    if4 = (max_rrect_len / min_rrect_len >= 9.9) && (max_rrect_len / min_rrect_len < 30); // 灯条的长宽比


// if rectangle is match condition, put it in candidate vector
// lr_rate1.3有点小了，调大点可以对付破掉的3号车
bool condition1 = delta_h > _para.min_light_delta_h && delta_h < _para.max_light_delta_h;
bool condition2 = MAX(leni, lenj) >= 113 ? abs(yi - yj) < 166\
    && abs(yi - yj) < 1.66 * MAX(leni, lenj) :
abs(yi - yj) < _para.max_light_delta_v\
    && abs(yi - yj) < 1.2 * MAX(leni, lenj); // && abs(yi - yj) < MIN(leni, lenj)
bool condition3 = lr_rate < _para.max_lr_rate;
//          bool condition4 = angleabs < 15 ; // 给大点防止运动时掉帧
bool condition4;
if (!base_mode)
    condition4 = sentry_mode ? angleabs < 25 : angleabs < 15 - 5; 
else
    condition4 = angleabs > 25 && angleabs < 55; 

4. cv::RotatedRect ArmorDetector::chooseTarget(const std::vector & match_rects, const cv::Mat & src)：

(1) 用均值和方差去除中间太亮的图片（例如窗外的灯光等）

RotatedRect screen_rect = RotatedRect(rect.center, Size2f(rect.size.width * 0.88, rect.size.height), rect.angle);
Size size = Point(src.cols, src.rows);
Point p1, p2;
int x = screen_rect.center.x - screen_rect.size.width / 2 + _dect_rect.x;
int y = screen_rect.center.y - screen_rect.size.height / 2 + _dect_rect.y;
p1 = Point(x, y);
x = screen_rect.center.x + screen_rect.size.width / 2 + _dect_rect.x;
y = screen_rect.center.y + screen_rect.size.height / 2 + _dect_rect.y;
p2 = Point(x, y);
Rect roi_rect = Rect(p1, p2);
Mat roi;
if(makeRectSafe(roi_rect, size))
{
    roi = src(roi_rect).clone();
    Mat mean, stdDev;
    double avg, stddev;
    meanStdDev(roi, mean, stdDev);
    avg = mean.ptr(0)[0];
    stddev = stdDev.ptr(0)[0];
    #ifdef SHOW_DEBUG_IMG
    cout << "                                            " << avg << endl;
    cout << "                                            " << stddev << endl << endl;
    //            putText(roi, to_string(int(avg)), rects[i].center, CV_FONT_NORMAL, 1, Scalar(0, 255, 255), 2);
    imshow("2.jpg", roi);
    #endif
    // 阈值可通过实际测量修改
    if (avg > 66.66)
        continue;
}

(2) 角度

// 现在这个旋转矩形的角度
float cur_angle = match_rects[i].rect.size.width > match_rects[i].rect.size.height ? \
    abs(match_rects[i].rect.angle) : 90 - abs(match_rects[i].rect.angle);
// 现在这个旋转矩形的高度（用来判断远近）                    
int cur_height = MIN(w, h);
// 最终目标如果太倾斜的话就筛除
if (cur_angle > 33 - 5) continue; // (其实可以降到26)

已注释源码展示：（其中， 和 “”"""" 引出的部分为作者注释）

/*******************************************************************************************************************
Copyright 2017 Dajiang Innovations Technology Co., Ltd (DJI)

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.
*******************************************************************************************************************/

#include "ArmorDetector.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include 
#include 
#include "opencv2/dnn/dnn.hpp"

//#define SHOW_DEBUG_IMG
//#define COUT_LOG
//#define CLASSIFICATION

using namespace cv;
using namespace std;

void ArmorDetector::setImage(const cv::Mat & src){
    _size = src.size();  // 一直是1280*720的
    // 注意这个_res_last是一个旋转矩形
    const cv::Point & last_result = _res_last.center;

    /*
        亮点1：如果 last_result为 true，则检测区域在上一帧的附近，也就是用到了 ROI 方法，如果超过一定帧数未检测到目标，
        则逐渐扩大搜索范围，直到33帧仍然未检测到目标则全图搜索，此时将与 ROI 有关的变量全部清零
    */

    // 如果上一次的目标没了，源图就是输入的图，并且搜索的ROI矩形（_dect_rect）就是整个图像
    if(last_result.x == 0 || last_result.y == 0)
    {
        _src = src;
        _dect_rect = Rect(0, 0, src.cols, src.rows);
    }
    /*
        亮点1.1：用直立矩形包围上一次的旋转矩形
        分析：利用ROI截取，可以避免每一次装甲板识别都从整张图像入手，提高了识别速率；
              另外，用直立矩形包围，可以保证将上一次的旋转矩形完全包围，避免遗漏区域
    */
    else
    {
        // 如果上一次的目标没有丢失，用直立矩形包围上一次的旋转矩形
        Rect rect = _res_last.boundingRect();
        // _para.max_light_delta_h 是左右灯柱在水平位置上的最大差值，像素单位
        疑问1：max_half_w、max_half_h变量有什么用？
        int max_half_w = _para.max_light_delta_h * 1.3;
        int max_half_h = 300;

        // 截图的区域大小，太大的话会把45度识别进去
        疑问2：“截图的区域大小，太大的话会把45度识别进去”如何理解
        double scale_w = 1.3 + 0.7;
        double scale_h = 2;

        疑问3：这一部分算法有点看不懂
        int w = int(rect.width * scale_w);
        int h = int(rect.height * scale_h);
        Point center = last_result;
        int x = std::max(center.x - w, 0);
        int y = std::max(center.y - h, 0);
        Point lu = Point(x, y);  /* point left up */
        x = std::min(center.x + w, src.cols);
        y = std::min(center.y + h, src.rows);
        Point rd = Point(x, y);  /* point right down */

        // 构造出矩形找到了搜索的ROI区域
        _dect_rect = Rect(lu, rd);
        // 为false说明矩形是空的，所以继续搜索全局像素。感觉这里会有点bug。
        // 若ROI矩形为空，则将与ROI有关的变量全部清零
        if (makeRectSafe(_dect_rect, src.size()) == false){
            _res_last = cv::RotatedRect();
            _dect_rect = Rect(0, 0, src.cols, src.rows);
            _src = src;
        }
        else
            // 如果ROI矩形合法的话就用此ROI
            src(_dect_rect).copyTo(_src);
    }

    //==========================上面已经设置好了真正处理的原图============================

    // 下面是在敌方通道上二值化进行阈值分割
    // _max_color是红蓝通道相减之后的二值图像
    / begin /
    /** 
     * 预处理其实是最重要的一步，这里有HSV和RGB两种预处理的思路，其实大致都差不多
     * 只不过在特定场合可以选择特定的预处理方式
     * 例如HSV的话可以完全过滤掉日光灯的干扰，但是耗时较大
     */
    int total_pixel = _src.cols * _src.rows;
    const uchar * ptr_src = _src.data; ptr_src指向图像数据矩阵的首地址
    const uchar * ptr_src_end = _src.data + total_pixel * 3; ptr_src_end指向图像数据矩阵的尾地址，3表示通道数
    int thres_max_color_red = 46 - 6;
    int thres_max_color_blue = 46;

    _max_color = cv::Mat(_src.size(), CV_8UC1, cv::Scalar(0));

    Mat element = getStructuringElement(MORPH_ELLIPSE, Size(7, 7));
    Mat element1 = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));
    Mat element2 = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
    // 敌方颜色为红色时
    if (enemy_color == RED)
    {
        Mat thres_whole;
        /*
            提取红蓝区域的方法有以下两种：
            一种方法是先将图片转化成 HSV 颜色空间再用通道范围将红色/蓝色提取出来（网上可找到对应的表），
            这种方法可以排除很多干扰，但是近距离的时候装甲板灯条发白，如果膨胀不到位会出现灯条断裂的情况，
            膨胀的卷积核过大又会造成预处理耗时过久，因此要权衡一下
        */
        // HSV
//        Mat temp, temp1,temp2, red;
//        vector channels;
//        cvtColor(_src, temp, COLOR_BGR2HSV);
//        split(temp, channels);
//        H = 0-10 | 156-180 表示红色
//        inRange(temp, Scalar(0,43,46),Scalar(11,255,255),temp1);
//        inRange(temp,Scalar(156,43,46),Scalar(181,255,255),temp2);
//        red = temp2 | temp1;
//        cvtColor(_src,thres_whole,CV_BGR2GRAY);
//        /*
//            亮点：哨兵比较特殊，我们对其进行特殊处理，如果接收到电控传来哨兵模式，我们会对其装甲板匹配条件进行宽松处理，
//            并且对其使用多分类识别装甲板贴纸上的 ID ，在10帧之内如果未找到目标依旧给电控发送找到目标的标志位，
//            直到超过上述帧数才发送未识别到目标，这样可以防止灯条被打灭就失去目标导致云台突然停一下的问题，
//            打灭了之后云台依旧会向前运动，可以快速推掉哨兵。
//        */
//        if (sentry_mode)
//            threshold(thres_whole,thres_whole,33,255,THRESH_BINARY);
//        else
//            threshold(thres_whole,thres_whole,60,255,THRESH_BINARY);
//
//        imshow("thresh_whole", thres_whole);
//        dilate(red, red, element1);
//        imshow("red", red);
//        _max_color = red & thres_whole;  // _max_color获得了清晰的二值图
        _max_color = rrr & thres_whole;  // _max_color获得了清晰的二值图
//        dilate(_max_color, _max_color, element2);

        /*
            一种方法是用BGR的红蓝通道相减，根据设定的阈值得到一张二值图，这种方法虽好，但是在识别蓝色的时候，
            有时候无法排除掉日光灯干扰，除此之外，操作简洁，耗时低，国赛选用这种方法。
        */
        // RGB
        vector splited;
        split(_src,splited);
        cvtColor(_src,thres_whole,CV_BGR2GRAY);
        if (sentry_mode)
            threshold(thres_whole,thres_whole,33,255,THRESH_BINARY);
        else
            threshold(thres_whole,thres_whole,60,255,THRESH_BINARY);

        imshow("thresh_whole", thres_whole);
        Mat color;
        subtract(splited[2],splited[0],color); //splited[2]-splited[0]：红减蓝
        threshold(color,color,thres_max_color_red,255,THRESH_BINARY);// red
        dilate(color, color, element);
        imshow("color", color);

        _max_color = color & thres_whole;  // _max_color获得了清晰的二值图
        dilate(_max_color, _max_color, element2);
    }

    // 敌方颜色是蓝色时（选用HSV方法，因为RGB方法识别蓝色，有时无法排除掉日光灯干扰）
    else 
    {
        Mat thres_whole;
        vector splited;
        split(_src,splited);
        cvtColor(_src,thres_whole,CV_BGR2GRAY);
        if (sentry_mode)
            threshold(thres_whole,thres_whole,60,255,THRESH_BINARY);
        else
            threshold(thres_whole,thres_whole,128,255,THRESH_BINARY);

        imshow("thres_whole", thres_whole);

        subtract(splited[0], splited[2], _max_color);
        threshold(_max_color, _max_color, thres_max_color_blue, 255, THRESH_BINARY);// blue
        dilate(_max_color, _max_color, element);
        imshow("color", _max_color);
        _max_color = _max_color & thres_whole;  // _max_color获得了清晰的二值图
        dilate(_max_color, _max_color, element2);
    }

    // end /
#ifdef SHOW_DEBUG_IMG
    cv::imshow("_max_color", _max_color);
#endif

}

void ArmorDetector::findTargetInContours(vector & match_rects) 
{
    // 在蓝色通道和红色通道图像中寻找轮廓
    vector > contours_max;
    vector hierarchy;
    // for debug use
    Mat FirstResult;
    _src.copyTo(FirstResult);

    // br是直接在_max_color上寻找的轮廓
    findContours(_max_color, contours_max, hierarchy, CV_RETR_EXTERNAL , CV_CHAIN_APPROX_SIMPLE);

    // 用直线拟合轮廓，找出符合斜率范围的轮廓
    vector RectFirstResult;
    for (size_t i = 0; i < contours_max.size(); ++i){
        // 椭圆拟合灯条轮廓
        RotatedRect rrect = minAreaRect(contours_max[i]);
        double max_rrect_len = MAX(rrect.size.width, rrect.size.height); 旋转矩形的长边
        double min_rrect_len = MIN(rrect.size.width, rrect.size.height); 旋转矩形的短边

        /// 单根灯条的条件 //
        // 角度要根据实际进行略微改动
        bool if1 = (fabs(rrect.angle) < 45.0 && rrect.size.height > rrect.size.width); // 往左
        bool if2 = (fabs(rrect.angle) > 60.0 && rrect.size.width > rrect.size.height); // 往右
        bool if3 = max_rrect_len > _para.min_light_height; // min_light_height为灯条的最小长度
        bool if4;
        if (!base_mode) //非吊射基地模式
            if4 = (max_rrect_len / min_rrect_len >= 1.1) && (max_rrect_len / min_rrect_len < 15); // 灯条的长宽比
        else //吊射基地模式
            if4 = (max_rrect_len / min_rrect_len >= 9.9) && (max_rrect_len / min_rrect_len < 30); // 灯条的长宽比
        // 筛除横着的以及太小的旋转矩形 (本来是45的，后来加成60)
        if ((if1 || if2) && if3 && if4)
        {
            RectFirstResult.push_back(rrect);
#ifdef SHOW_DEBUG_IMG
            Point2f vertice[4];
            rrect.points(vertice);
            for (int i = 0; i < 4; i++)  // 黄色
                line(FirstResult, vertice[i], vertice[(i + 1) % 4], Scalar(0, 255, 255), 2);
#endif
        }
    }

    // 少于两根灯条就认为没有匹配到
    if (RectFirstResult.size() < 2){
        match_rects.size() == 0;
        return;
    }

    // 将旋转矩形从左到右（根据x轴分量从小到大）排序，运用了lamda表达式
    sort(RectFirstResult.begin(), RectFirstResult.end(),
         [](RotatedRect & a1,RotatedRect & a2){
             return a1.center.x < a2.center.x;});

    Point2f _pt[4],pt[4];
    计算前点在后点处的圆心角大小（角度制），表达了两点连成的直线与x轴正方向的偏差
    auto ptangle = [](const Point2f &p1,const Point2f &p2){
        return fabs(atan2(p2.y-p1.y,p2.x-p1.x)*180.0/CV_PI);
    };

    / 匹配灯条的条件 //
    // 两两比较，有符合条件的就组成一个目标旋转矩形
    for (size_t i = 0; i < RectFirstResult.size() - 1; ++i) 
    {
        const RotatedRect & rect_i = RectFirstResult[i];
        const Point & center_i = rect_i.center;
        float xi = center_i.x;
        float yi = center_i.y;
        float leni = MAX(rect_i.size.width, rect_i.size.height);
        float anglei = fabs(rect_i.angle);
        rect_i.points(_pt);
            /*pt
            * 0 2
            * 1 3
            * */
        // 往右斜的长灯条（这里定义旋转矩形的定义看不懂？？？）
        // rRect.points有顺序的，y最小的点是0,顺时针1 2 3
        if(anglei > 45.0){
            pt[0] = _pt[3];
            pt[1] = _pt[0];
        }
        // 往左斜的
        else{
            pt[0] = _pt[2];
            pt[1] = _pt[3];
        }

        for (size_t j = i + 1; j < RectFirstResult.size(); j++) 
        {
            const RotatedRect & rect_j = RectFirstResult[j];
            const Point & center_j = rect_j.center;
            float xj = center_j.x;
            float yj = center_j.y;
            float lenj = MAX(rect_j.size.width, rect_j.size.height);
            float anglej=  fabs(rect_j.angle);
            float delta_h = xj - xi; //两旋转矩形的x轴分量差值
            float lr_rate = leni > lenj ? leni / lenj : lenj / leni; //两旋转矩形的长边之比
            float angleabs; //两旋转矩形旋转角的差值

            rect_j.points(_pt);
            if(anglej > 45.0){
                pt[2] = _pt[2];
                pt[3] = _pt[1];
            }else{
                pt[2] = _pt[1];
                pt[3] = _pt[0];
            }
            double maxangle = MAX(ptangle(pt[0],pt[2]),ptangle(pt[1],pt[3]));
            //std::cout<<"angle:"< 45.0 && anglej < 45.0) // 八字 / \   
            { 
                angleabs = 90.0 - anglei + anglej;
            }
            else if(anglei <= 45.0 && anglej >= 45.0) // 倒八字 \ /
            { 
                angleabs = 90.0 - anglej + anglei;
            }
            else // 在同一边
            {
                if(anglei > anglej) angleabs = anglei - anglej; 
                else angleabs = anglej - anglei;
            }

            // if rectangle is match condition, put it in candidate vector
            // lr_rate=1.3有点小了，调大点可以对付破掉的3号车
            bool condition1 = delta_h > _para.min_light_delta_h && delta_h < _para.max_light_delta_h;
            bool condition2 = MAX(leni, lenj) >= 113 ? abs(yi - yj) < 166\
                && abs(yi - yj) < 1.66 * MAX(leni, lenj) :
                abs(yi - yj) < _para.max_light_delta_v\
                && abs(yi - yj) < 1.2 * MAX(leni, lenj); // && abs(yi - yj) < MIN(leni, lenj)
            bool condition3 = lr_rate < _para.max_lr_rate;
            //                bool condition4 = angleabs < 15 ; // 给大点防止运动时掉帧
            bool condition4;
            if (!base_mode) 非吊射基地模式
                condition4 = sentry_mode ? angleabs < 25 : angleabs < 15 - 5;
            else 吊射基地模式
                condition4 = angleabs > 25 && angleabs < 55;
            //                bool condition5 = sentry_mode ? true : /*maxangle < 20*/true;
            //                bool condition4 = delta_angle < _para.max_light_delta_angle;

            Point text_center = Point((xi + xj) / 2, (yi + yj) / 2); 左右两旋转矩形中心点的中点
#ifdef COUT_LOG
            cout << "delta_h:  " << abs(yi - yj) << endl;
            cout << "lr rate:  " << lr_rate << endl;
            cout << "length:   " << MAX(leni, lenj) << endl;
            cout << condition1 << " " << condition2 << " " << condition3 << " " << condition4 << endl;
#endif

#ifdef SHOW_DEBUG_IMG
                putText(FirstResult, to_string(int(angleabs)), text_center, CV_FONT_NORMAL, 1, Scalar(0, 255, 0), 2);
                putText(FirstResult, to_string(int(maxangle)), Point(text_center.x + 20, text_center.y + 20), CV_FONT_NORMAL, 1, Scalar(255, 0, 0), 2);

#endif
            if (condition1 && condition2 && condition3 && condition4) 
            {
                将所有符合条件的左右旋转矩形拟合并存入新的旋转矩形类
                RotatedRect obj_rect = boundingRRect(rect_i, rect_j);
                double w = obj_rect.size.width;
                double h = obj_rect.size.height;
                double wh_ratio = w / h; //新旋转矩形类的长宽比
                // 长宽比不符
#ifdef COUT_LOG
            cout << "wh_ratio:  " << wh_ratio << endl;
#endif
            // 基地模式不受长宽比的限制
            if (!base_mode)
            {
                if (wh_ratio > _para.max_wh_ratio || wh_ratio < _para.min_wh_ratio)
                    continue;
            }

            // 将初步匹配到的结构体信息push进入vector向量
            match_rects.push_back(matched_rect{obj_rect, lr_rate, angleabs});
            // for debug use
            Point2f vertice[4];
            obj_rect.points(vertice);
            for (int i = 0; i < 4; i++)
                line(FirstResult, vertice[i], vertice[(i + 1) % 4], Scalar(255, 255, 255), 2); //将相邻顶点依次相连成矩形
            }
        }
    }
#ifdef SHOW_DEBUG_IMG
        imshow("showFirstResult", FirstResult);
#endif
}

cv::RotatedRect ArmorDetector::chooseTarget(const std::vector & match_rects, const cv::Mat & src) {
    // 如果没有两条矩形围成一个目标装甲板就返回一个空的旋转矩形
    if (match_rects.size() < 1){
        _is_lost = true;2222
        return RotatedRect();
    }

    // 初始化参数
    int ret_idx = -1;
    bool is_small = false;
    double weight = 0;
    vector candidate;
    // 二分类判断真假装甲板初始化
    Mat input_sample;
    vector channels;

    #define SafeRect(rect, max_size) {if (makeRectSafe(rect, max_size) == false) continue;}

    / 匹配灯条 
    //======================= 开始循环 ====================================
    for (size_t i = 0; i < match_rects.size(); ++i)
    {
        const RotatedRect & rect = match_rects[i].rect;

        // 长宽比不符,担心角度出问题，可以侧着车身验证一下（上面一个函数好像写过这个条件了）
        double w = rect.size.width;
        double h = rect.size.height;
        double wh_ratio = w / h;

        // 设置角度解法，其实不要这个也可以，只是为了根据这个算出距离来筛选太远太近的
        AngleSolver * slover = NULL;
        if(_size.height == 720)
            slover = l_solver;

        // 如果矩形的w和h之比小于阈值的话就是小装甲，否则是大装甲(初步判断)
        if (wh_ratio < _para.small_armor_wh_threshold)
            is_small = true;
        else
            is_small = false;

//        cout << "wh_ratio: " << wh_ratio << endl;

        // 用均值和方差去除中间太亮的图片（例如窗外的灯光等）        
        RotatedRect screen_rect = RotatedRect(rect.center, Size2f(rect.size.width * 0.88, rect.size.height), rect.angle);
        Size size = Point(src.cols, src.rows);
        Point p1, p2;
        int x = screen_rect.center.x - screen_rect.size.width / 2 + _dect_rect.x;
        int y = screen_rect.center.y - screen_rect.size.height / 2 + _dect_rect.y;
        p1 = Point(x, y);
        x = screen_rect.center.x + screen_rect.size.width / 2 + _dect_rect.x;
        y = screen_rect.center.y + screen_rect.size.height / 2 + _dect_rect.y;
        p2 = Point(x, y);
        Rect roi_rect = Rect(p1, p2);
        Mat roi;
        if(makeRectSafe(roi_rect, size)){
            roi = src(roi_rect).clone();
            Mat mean, stdDev;
            double avg, stddev;
            meanStdDev(roi, mean, stdDev);
            avg = mean.ptr(0)[0];
            stddev = stdDev.ptr(0)[0];
#ifdef SHOW_DEBUG_IMG
            cout << "                                            " << avg << endl;
            cout << "                                            " << stddev << endl << endl;
//            putText(roi, to_string(int(avg)), rects[i].center, CV_FONT_NORMAL, 1, Scalar(0, 255, 255), 2);
            imshow("2.jpg", roi);
#endif
            // 阈值可通过实际测量修改
            if (avg > 66.66)
                continue;
        }

        // 只匹配到一个目标的话就不送入二分类直接处理（已弃用）
        /**
         * 多分类用来识别装甲板中间的数字，对曝光有一定要求，经过大量测试，为了识别稳定不掉帧         
         * 最终决定只在击打哨兵的时候用上多分类，对于地面单位的击打则不用多分类，只用传统opencv方法 
         */
        if (match_rects.size() == 100){} else {
//#ifdef CLASSIFICATION
            if (sentry_mode){
        // 二分类
        // 对rect进行截图变成一个Mat
        RotatedRect screen_rect = RotatedRect(rect.center, Size2f(rect.size.width * 0.9, rect.size.height * 2.4), rect.angle);
        Size size = Point(src.cols, src.rows);
        Point p1, p2;
        int x = screen_rect.center.x - screen_rect.size.width / 2 + _dect_rect.x;
        int y = screen_rect.center.y - screen_rect.size.height / 2 + _dect_rect.y;
        p1 = Point(x, y);
        x = screen_rect.center.x + screen_rect.size.width / 2 + _dect_rect.x;
        y = screen_rect.center.y + screen_rect.size.height / 2 + _dect_rect.y;
        p2 = Point(x, y);
        Rect roi_rect =Rect(p1, p2);
        Mat input_sample;
        if(makeRectSafe(roi_rect, size)){
            input_sample = src(roi_rect).clone();

#ifdef SHOW_DEBUG_IMG
            imshow("1.jpg", input_sample);
#endif
        }

        /**
         * 下面这个条件虽然也已经弃用了，但是可以参考一下思路
         * 如果在哨兵的视觉算法中加入了多分类的话可能会用到这个方法
         */
        // 防止三根灯条

//        int flag = 0;
//        Mat data = roi.clone();
//        vector container;
//        vector > contours;

//        if (MIN(rect.size.height, rect.size.width) >= _para.near_face_v)
//        {
//            split(data, container);
//            threshold(container[1], container[1],  _para.bin_cls_thres, 255, THRESH_BINARY);
//            imshow("防止三根灯条", container[1]);

//            findContours(container[1], contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
//            for (int i = 0; i < contours.size(); ++ i){
//                RotatedRect r = minAreaRect(contours[i]);
//                if (MAX(r.size.height, r.size.width) >= _para.min_light_height + 30){
//                    flag = 1;
//                }
//            }
//        }

//        else
//        {
//            split(data, container);
//            threshold(container[1], container[1],  _para.bin_cls_thres, 255, THRESH_BINARY);
//            imshow("防止三根灯条", container[1]);
//            findContours(container[1], contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
//            for (int i = 0; i < contours.size(); ++ i){
//                RotatedRect r = minAreaRect(contours[i]);
//                if (MAX(r.size.height, r.size.width) >= _para.min_light_height + 4){
//                    flag = 1;
//                }
//            }
//        }


//        if (flag) continue;

        resize(input_sample, input_sample, Size(28, 28));
//        convertScaleAbs(input_sample, input_sample, 4); // lights on
        convertScaleAbs(input_sample, input_sample, 8); // lights off

        cvtColor(input_sample, input_sample, CV_BGR2GRAY);

        Mat inputBlob = dnn::blobFromImage(input_sample, 0.00390625f, Size(28, 28), Scalar(), false);
        Mat prob;
        net.setInput(inputBlob, "data");
        prob = net.forward("loss");

        int classId;
        double classProb;
        Mat probMat = prob.reshape(1, 1);
        Point classNumber;
        minMaxLoc(probMat, NULL, &classProb, NULL, &classNumber);
        classId = classNumber.x;    // 类别：0是45度，1是装甲板
        cout << "class ID: " << classId << "\n\n";
        if (classId == 0) continue;

        namedWindow("0", WINDOW_NORMAL);
        imshow("0",input_sample);
            }
//#endif
        }

        // 现在这个旋转矩形的角度
        float cur_angle = match_rects[i].rect.size.width > match_rects[i].rect.size.height ? \
                    abs(match_rects[i].rect.angle) : 90 - abs(match_rects[i].rect.angle);
        // 现在这个旋转矩形的高度（用来判断远近）                    
        int cur_height = MIN(w, h);
        // 最终目标如果太倾斜的话就筛除
        if (cur_angle > 33 - 5) continue; // (其实可以降到26)
        // 把矩形的特征信息push到一个候选vector中
        candidate.push_back(candidate_target{cur_height, cur_angle, static_cast(i), is_small, match_rects[i].lr_rate, match_rects[i].angle_abs});
        ret_idx = 1;
    }
    //================================ 到这里才结束循环 =======================================
    int final_index = 0;
    if (candidate.size() > 1){
    // 将候选矩形按照高度大小排序，选出最大的（距离最近）
    sort(candidate.begin(), candidate.end(),
            [](candidate_target & target1, candidate_target & target2){
        return target1.armor_height > target2.armor_height;
    });
    // 做比较，在最近的几个矩形中（包括45度）选出斜率最小的目标
    /**
        * 下面的几个temp值可以筛选出最终要击打的装甲板，我只用了一两个效果已经挺好的了
        * 只是偶尔还会有误识别的情况，可以将这几个temp值都组合起来进行最终的判断        
        * */

    float temp_angle = candidate[0].armor_angle;
    float temp_lr_rate = candidate[0].bar_lr_rate;
    float temp_angle_abs = candidate[0].bar_angle_abs;
    float temp_weight = temp_angle + temp_lr_rate;

    for (int i = 1; i < candidate.size(); i++)
    {
        double angle = match_rects[candidate[i].index].rect.angle;
        if ( candidate[0].armor_height / candidate[i].armor_height < 1.1)
        {
            if (candidate[i].armor_angle < temp_angle /*&& (candidate[i].bar_lr_rate */)
            {
                temp_angle = candidate[i].armor_angle;
                if (candidate[i].bar_lr_rate < 1.66) final_index = i;
            }
        }
    }
}

#ifdef SHOW_DEBUG_IMG
    Mat rect_show;
    _src.copyTo(rect_show);
    //候选区域
    Point2f vertices[4];
    match_rects[final_index].rect.points(vertices);
    putText(rect_show, to_string(int(match_rects[final_index].rect.angle)), match_rects[final_index].rect.center, CV_FONT_NORMAL, 1, Scalar(0, 255, 0), 2);
    for (int i = 0; i < 4; i++)
        line(rect_show, vertices[i], vertices[(i + 1) % 4], CV_RGB(255, 0, 0));
    imshow("final_rect", rect_show);
#endif

    // ret_idx为 -1 就说明没有目标
    if (ret_idx == -1){
        _is_lost = true;
        return RotatedRect();
    }
    // 否则就证明找到了目标
    _is_lost = false;
    _is_small_armor = candidate[final_index].is_small_armor;
    RotatedRect ret_rect = match_rects[candidate[final_index].index].rect;
    return ret_rect;
}

// 将之前的各个函数都包含在一个函数中，直接用这一个
cv::RotatedRect ArmorDetector::getTargetArea(const cv::Mat & src, const int & sb_mode, const int & jd_mode){
    // 传入参数为哨兵模式和吊射基地模式
    sentry_mode = sb_mode;
    base_mode = jd_mode;
    setImage(src);  // function called
    vector match_rects;
    findTargetInContours(match_rects);  // function called
    RotatedRect final_rect = chooseTarget(match_rects, src);  // function called

    if(final_rect.size.width != 0)
    {
        // 最后才加上了偏移量，前面那些的坐标都是不对的，所以不能用前面那些函数解角度
        final_rect.center.x += _dect_rect.x;
        final_rect.center.y += _dect_rect.y;
        _res_last = final_rect;
        _lost_cnt = 0;
    }
    else
    {
        _find_cnt = 0;
        ++_lost_cnt;
        // 逐次加大搜索范围（根据相机帧率调整参数）
        if (_lost_cnt < 8)
            _res_last.size = Size2f(_res_last.size.width, _res_last.size.height);
        else if(_lost_cnt == 9)
            _res_last.size =Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if(_lost_cnt == 12)
            _res_last.size = Size2f(_res_last.size.width * 2, _res_last.size.height * 2);
        else if(_lost_cnt == 15)
            _res_last.size = Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if (_lost_cnt == 18)
            _res_last.size = Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if (_lost_cnt > 33 )
            _res_last = RotatedRect();
    }
    return final_rect;
}

cv::RotatedRect ArmorDetector::boundingRRect(const cv::RotatedRect & left, const cv::RotatedRect & right){
    // 这个函数是用来将左右边的灯条拟合成一个目标旋转矩形，没有考虑角度
    const Point & pl = left.center, & pr = right.center;
    Point2f center = (pl + pr) / 2.0;
//    cv::Size2f wh_l = left.size;
//    cv::Size2f wh_r = right.size;
    // 这里的目标矩形的height是之前灯柱的width
    double width_l = MIN(left.size.width, left.size.height);
    double width_r = MIN(right.size.width, right.size.height);
    double height_l = MAX(left.size.width, left.size.height);
    double height_r = MAX(right.size.width, right.size.height);
    float width = POINT_DIST(pl, pr) - (width_l + width_r) / 2.0;
    float height = std::max(height_l, height_r);
    //float height = (wh_l.height + wh_r.height) / 2.0;
    float angle = std::atan2(right.center.y - left.center.y, right.center.x - left.center.x);
    return RotatedRect(center, Size2f(width, height), angle * 180 / CV_PI);
}

三、角度解算 AngleSolver

pnp算法用于求解物体与摄像头间的相对位置，使用该算法首先要标定摄像头，得到相机的内参矩阵和畸变参数，之后需要测量物体的尺寸，得到物体在世界坐标系中的坐标。（图像像素坐标系 <-> 图像物理坐标系 <-> 相机坐标系 <-> 世界坐标系)

世界坐标系：就是物体在真实世界中的坐标，比如黑白棋盘格的世界坐标系原点定在第一个棋盘格的顶点，Xw，Yw，Zw互相垂直，Zw方向就是垂直于棋盘格面板的方向。可见世界坐标系是随着物体的大小和位置变化的，单位是长度单位。只要棋盘格的大小决定了，无论板子怎么动，棋盘格角点坐标一般就不再变动(因为是相对于世界坐标系原点的位置不变)，且认为是Zw=0.

相机坐标系：以光心为相机坐标系的原点，以平行于图像的x和y方向为Xc轴和Yc轴，Zc轴和光轴平行，Xc，Yc，Zc互相垂直，单位是长度单位。

图像物理坐标系：以主光轴和图像平面交点为坐标原点，x和y方向如图所示，单位是长度单位。

图像像素坐标系：以图像的顶点为坐标原点，u和v方向平行于x和y方向，单位是以像素计。

这个坐标是自定义的，由于装甲板可以看成是一个平板，其z轴坐标可以设为0，x轴和y轴的坐标分别可以设置为正负二分之一的长和宽，这样装甲板的中心就为原点了。OpenCV中的坐标系可参看下图：

1. void RectPnPSolver::solvePnP4Points(const std::vector& points2d, cv::Mat& rot, cv::Mat& trans, double& dist)：构建pnp求解模型

rot = cv::Mat::eye(3, 3, CV_64FC1);  3*3的单位矩阵
trans = cv::Mat::zeros(3, 1, CV_64FC1);    3行列向量

std::vector point3d;   三位浮点型空间点
double half_x = width_target / 2.0;
double half_y = height_target / 2.0; 构建求解平面
point3d.push_back(Point3f(-half_x, -half_y, 0));
point3d.push_back(Point3f(half_x, -half_y, 0));
point3d.push_back(Point3f(half_x, half_y, 0));
point3d.push_back(Point3f(-half_x, half_y, 0));

cv::Mat r;   r用来保存输出的旋转向量
cv::solvePnP(point3d, points2d, cam_matrix, distortion_coeff, r, trans);
cam_matrix为相机的内参矩阵，distortion_coeff为相机的畸变参数，r为旋转向量,trans为输出的平移矩阵
相机坐标系下的点=世界坐标系下的点*r（要变为旋转矩阵）+ trans
先由图像像素坐标系到图像物理坐标系，再由图像物理坐标系到相机坐标系，最后由相机坐标系到世界坐标系
Rodrigues(r, rot);     旋转向量转为旋转矩阵

2. bool AngleSolver::getAngle(const cv::RotatedRect& rect, double& angle_x, double& angle_y, double& dist)：

void AngleSolver::adjustPTZ2Barrel(const cv::Mat& pos_in_ptz, double& angle_x, double& angle_y)：角度偏移修改

图像像素坐标与世界坐标的关系：其中X、Y、Z为物理世界中点的坐标

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rZsgV2K0-1633575877298)(E:\from C\Desktop\2021-2022 1\next term\Typora备份\备份2\51备份\image-20211007084453256.png)]

要想求得转角只需先求得X/Z和Y/Z，然后通过反三角函数就可以得到需要的角度了

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-3rhHO422-1633575877299)(E:\from C\Desktop\2021-2022 1\next term\Typora备份\备份2\51备份\image-20211007084519935.png)]

_xyz指向世界坐标，_xyz[0]表示X，_xyz[1]表示Y，_xyz[2]表示Z
const double* _xyz = (const double*)pos_in_ptz.data;
X、Y、Z三轴坐标偏移
double xyz[3] = { _xyz[0], _xyz[1] - 8, _xyz[2] + 10 };
角度转弧度，与反正切结果统一单位
double theta = 0.0, offset_x = 5.0 * 3.1415926 / 180, offset_y = -0.15 * 3.1415926 / 180;
X、Y轴角度偏移
theta = atan(xyz[1] / xyz[2]); 
angle_y = (theta + offset_y); 
angle_x = atan2(xyz[0], xyz[2]) + offset_x;
弧度转角度，发送角度偏移信息
angle_x = angle_x * 180 / 3.1415926;
angle_y = angle_y * 180 / 3.1415926;

四、串口通信

关于波特率的解释：
(1) 串口波特率需要设置合理，波特率太高则误码率会增加，波特率太低则发送速度太慢
(2) 波特率115200表示每秒发送115200位，换算成字节每秒是11520（不加校验位），若每次发送的数据均为8字节，则除8得到每秒最多可发送指令1440次，需要根据串口的发送速度选择摄像头（有人觉得串口的发送速度至少要比摄像头的帧率大10倍以上）。如果需要传较多数据，应该提高波特率

//设置波特率
void SerialPort::set_Brate()
{
    int speed_arr[] = {B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,
					   B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,
					  };
    int name_arr[] = {115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,
					  115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,
					 };
    int i;
    int status; 波特率是否设置成功的状态变量
    struct termios Opt;
    tcgetattr(fd, &Opt);

	for (i = 0;  i < sizeof(speed_arr) / sizeof(int);  i++)
	{
		if (speed == name_arr[i])
		{
			tcflush(fd, TCIOFLUSH);//清空缓冲区的内容
			cfsetispeed(&Opt, speed_arr[i]);//设置接受波特率
			cfsetospeed(&Opt, speed_arr[i]);//设置发送波特率
            status = tcsetattr(fd, TCSANOW, &Opt); //使设置立即生效

            波特率设置失败
			if (status != 0)
			{
                perror("tcsetattr fd1");
                return;
            }

			tcflush(fd, TCIOFLUSH);
        }
    }
}

串口通信为保证上下位机数据准确需要制定一个通信协议，一种方法是用字符串来对发送数据进行描述，类似于“Y010P020"来代表yaw轴转10度、pitch轴转20度，在下位机stm32上用最原始的加法和乘法把字符组合成数据。

这种方式不仅不好看、效率很低，还很容易出错，合理的方法是定义发送帧和接收帧，用帧头和帧尾来校验帧是否正确，帧头和帧尾中间放数据。

这是DJI开源代码中定义的上位机向下位机传输的帧的格式，可以看到中间的数据部分是用两个字节组合成一个16位的整数。

串口发送时一般是发送一段unsigned char数组，其中的每一个字节都可以通过强制类型转换来表示其他类型。

以上述帧为例第一个字节为帧头0xFF，中间6个字节组成三个16位整数，就可以像下面这样写。

unsigned char data[] = { 0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0xFE};
*(short*)(data+1)=(short)value1;
*(short*)(data+3)=(short)value2;
*(short*)(data+5)=(short)value3;

下位机接收时首先找0xFF，找到后再接收7个字节，对比最后一个字节是否是0xFE，若不是则丢弃，若是则本次数据有效，之后同样可以采用强制类型转换的方式来提取数据。

如果想发送其他类型的数据的话同样可以用这种方式，但要根据类型的大小分配好需要的字符数组大小以防越界。

大疆开源代码中下位机对上位机的帧的格式如下：

已注释源码展示：（其中， 和 “”"""" 引出的部分为作者注释）

#include "include/SerialPort/serialport.h"

/**
 *@brief  初始化数据
 *@param  fd       类型  int  打开的串口文件句柄
 *@param  speed    类型  int  波特率

 *@param  databits 类型  int  数据位    取值为7或者8

 *@param  stopbits 类型  int  停止位    取值为1或者2
 *@param  parity   类型  int  效验类型  取值为N,E,O,S
 *@param  portchar 类型  char* 串口路径
 */

/*
    关于波特率的解释：
    1.串口波特率需要设置合理，波特率太高则误码率会增加，波特率太低则发送速度太慢
    2.波特率115200表示每秒发送115200位，换算成字节每秒是11520（不加校验位），若每次发送的数据均为8字节，则除8得到每秒最多可发送指令1440次，
      需要根据串口的发送速度选择摄像头（有人觉得串口的发送速度至少要比摄像头的帧率大10倍以上）
*/

SerialPort::SerialPort()
{
	fd = open(UART_DEVICE, O_RDWR | O_NOCTTY | O_NDELAY); 默认打开串口0
    speed = BAUDRATE; 波特率115200
    databits = 8;
    stopbits = 1;
	parity = 'N';
}

SerialPort::SerialPort(char *portpath)
{
	fd = open(portpath, O_RDWR | O_NOCTTY | O_NDELAY); 打开对应串口
    speed = BAUDRATE; 波特率115200
    databits = 8;
    stopbits = 1;
	parity = 'N';
}

/**
 *@brief   获取模式命令
 */
bool SerialPort::get_Mode(int &mode, int &sentry_mode, int &base_mode)
{
    int bytes;
    char *name = ttyname(fd);
    if (name != NULL) printf("device:%s\n",name);
    else printf("tty is null\n");
    int result = ioctl(fd, FIONREAD, &bytes);
    if (result == -1) return false;

    if (bytes == 0)
    {
//      cout << "缓冲区为空" << endl;
        return true;
    }

    bytes = read(fd, rdata, 22);

    判断帧头和CRC校验是否正确
    if (rdata[0] == 0xA5 && Verify_CRC8_Check_Sum(rdata, 3)) 
    {
        mode  = (int)rdata[1];
        printf("接收到的指令:%d\r\n", mode);
        sentry_mode  = (int)rdata[15];
        printf("Is in sentry mode ? :%d\r\n", sentry_mode);
        base_mode  = (int)rdata[16];
        printf("Is in base mode ? :%d\r\n", base_mode);
    }
    return true;
}

/**
 *@brief   初始化串口
 */
bool SerialPort::initSerialPort()
{
	if (fd == -1)
	{
        perror(UART_DEVICE);
        return false;
    }

	std::cout << "Opening..." << std::endl;
    set_Brate();

	if (set_Bit() == FALSE)
	{
        printf("Set Parity Error\n");
		exit(0);
    }

//  set_disp_mode(0);
    printf("Open successed\n");
    return true;
}

/**
 *@brief   设置波特率
 */
void SerialPort::set_Brate()
{
    int speed_arr[] = {B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,
					   B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,
					  };
    int name_arr[] = {115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,
					  115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,
					 };
    int i;
    int status; 波特率是否设置成功的状态变量
    struct termios Opt;
    tcgetattr(fd, &Opt);

	for (i = 0;  i < sizeof(speed_arr) / sizeof(int);  i++)
	{
		if (speed == name_arr[i])
		{
			tcflush(fd, TCIOFLUSH);//清空缓冲区的内容
			cfsetispeed(&Opt, speed_arr[i]);设置接受波特率
			cfsetospeed(&Opt, speed_arr[i]);设置发送波特率
            status = tcsetattr(fd, TCSANOW, &Opt); //使设置立即生效

            波特率设置失败
			if (status != 0)
			{
                perror("tcsetattr fd1");
                return;
            }

			tcflush(fd, TCIOFLUSH);
        }
    }
}

/**
 *@brief   设置串口数据位，停止位和效验位
 */
int SerialPort::set_Bit()
{
    struct termios termios_p;

	if (tcgetattr(fd, &termios_p) != 0)
	{
        perror("SetupSerial 1");
		return (FALSE);
    }

    termios_p.c_cflag |= (CLOCAL | CREAD);  //接收数据
	termios_p.c_cflag &= ~CSIZE;//设置数据位数

    串口数据位
    switch (databits)
    {
	case 7:
		termios_p.c_cflag |= CS7;
		break;

	case 8:
		termios_p.c_cflag |= CS8;
		break;

	default:
		fprintf(stderr, "Unsupported data size\n");
		return (FALSE);
    }

	//奇偶校验位double
    switch (parity)
    {
	case 'n':
	case 'N':
		termios_p.c_cflag &= ~PARENB;   /* Clear parity enable */
		termios_p.c_iflag &= ~INPCK;     /* Enable parity checking */
		break;

	case 'o': odd
	case 'O':
		termios_p.c_cflag |= (PARODD | PARENB); /* 设置为奇效验*/
		termios_p.c_iflag |= INPCK;             /* Disable parity checking */
		break;

	case 'e': even
	case 'E':
		termios_p.c_cflag |= PARENB;     /* Enable parity */
		termios_p.c_cflag &= ~PARODD;   /* 转换为偶效验*/
		termios_p.c_iflag |= INPCK;       /* Disable parity checking */
		break;

	case 'S':
	case 's':  /*as no parity*/
		termios_p.c_cflag &= ~PARENB;
		termios_p.c_cflag &= ~CSTOPB;
		break;

	default:
		fprintf(stderr, "Unsupported parity\n");
		return (FALSE);
    }

    //停止位
    switch (stopbits)
    {
	case 1:
		termios_p.c_cflag &= ~CSTOPB;
		break;

	case 2:
		termios_p.c_cflag |= CSTOPB;
		break;

	default:
		fprintf(stderr, "Unsupported stop bits\n");
		return (FALSE);
    }

    /* Set input parity option */
    if (parity != 'n')
        termios_p.c_iflag |= INPCK;

	tcflush(fd, TCIFLUSH); //清除输入缓存区
    termios_p.c_cc[VTIME] = 150; //设置超时15 seconds
    termios_p.c_cc[VMIN] = 0;  //最小接收字符
    termios_p.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  /*Input原始输入*/
	termios_p.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
    termios_p.c_iflag &= ~(ICRNL | IGNCR);
    termios_p.c_oflag &= ~OPOST;   /*Output禁用输出处理*/

	if (tcsetattr(fd, TCSANOW, &termios_p) != 0) /* Update the options and do it NOW */
    {
        perror("SetupSerial 3");
        return (FALSE);
    }
    return (TRUE);
}



//int SerialPort::set_disp_mode(int option)
//{
//   int err;
//   struct termios term;
//   if(tcgetattr(fd,&term)==-1){
//     perror("Cannot get the attribution of the terminal");
//     return 1;
//   }
//   if(option)
//        term.c_lflag|=ECHOFLAGS;   //(ECHO | ECHOE | ECHOK | ECHONL)
//   else
//        term.c_lflag &=~ECHOFLAGS;
//   err=tcsetattr(fd,TCSAFLUSH,&term);
//   if(err==-1 && err==EINTR){
//        perror("Cannot set the attribution of the terminal");
//        return 1;
//   }
//   return 0;
//}


/**
 *@brief   转换数据并发送
 *@param  p        类型  int  x坐标
 *@param  yaw      类型  int  y坐标
 *@param  dis      类型  int  dis  距离
 *@param  unsign   类型  int  空白（可添加数据）
 */
void SerialPort::TransformDataFirst(int Xpos, int Ypos, int dis)
{
    Tdata[0] = 0xA5; 帧头
    Tdata[1] = CmdID0;   //command：“关闭视觉”模式

	Append_CRC8_Check_Sum(Tdata, 3); //CRC8校验

	for (int i = 0; i < 4; i++)
	{
		Tdata[3 + i] = Xpos % 10;
		Xpos = (Xpos - Xpos % 10) / 10;
    }
	for (int i = 0; i < 4; i++)
	{
		Tdata[7 + i] = Ypos % 10;
		Ypos = (Ypos - Ypos % 10) / 10;
    }
	for (int i = 0; i < 4; i++)
	{
		Tdata[11 + i] = dis % 10;
		dis = (dis - dis % 10) / 10;
    }

	Tdata[15] = 0;
    Tdata[16] = 0;
    Tdata[17] = 0;
    Tdata[18] = 0;
    Tdata[19] = 0;

	Append_CRC16_Check_Sum(Tdata, 22); //CRC16校验
}


/**
 *@brief   转换数据并发送
 *@param   data   类型  VisionData(union)  包含pitch,yaw,distance
 *@param   flag   类型  char   用于判断是否瞄准目标，0代表没有，1代表已经瞄准
 */
void SerialPort::TransformData(const VisionData &data)
{
    Tdata[0] = 0xA5; 帧头
    Tdata[1] = CmdID1; “识别红色”模式
	Append_CRC8_Check_Sum(Tdata, 3);

    Tdata[3] = data.pitch_angle.c[0];
    Tdata[4] = data.pitch_angle.c[1];
    Tdata[5] = data.pitch_angle.c[2];
    Tdata[6] = data.pitch_angle.c[3];

    Tdata[7] = data.yaw_angle.c[0];
    Tdata[8] = data.yaw_angle.c[1];
    Tdata[9] = data.yaw_angle.c[2];
    Tdata[10] = data.yaw_angle.c[3];

    Tdata[11] = data.dis.c[0];
    Tdata[12] = data.dis.c[1];
    Tdata[13] = data.dis.c[2];
    Tdata[14] = data.dis.c[3];

    Tdata[15] = data.ismiddle;
	Tdata[16] = data.isFindTarget;

    Tdata[17] = data.isfindDafu;
    Tdata[18] = 0x00;
    Tdata[19] = data.nearFace;

	Append_CRC16_Check_Sum(Tdata, 22); CRC16校验
}

//发送数据函数
void SerialPort::send()
{
	write(fd, Tdata, 22);
}

//关闭通讯协议接口
void SerialPort::closePort()
{
    close(fd);
}

五、参考资料

本文主要参考了以下资料，若有侵权，请告知删除

RoboMaster视觉教程（5）目标位置解算（通过像素点获取转角）_江达小记-CSDN博客

RoboMaster视觉教程（6）目标位置解算（PnP求解目标与摄像头间的相对位置）_江达小记-CSDN博客_pnp解算

相机针孔模型----从世界坐标系，到相机坐标系，再到图像物理坐标系，最后到图像像素坐标系的转换过程解析_wangxiaokun671903的专栏-CSDN博客_图像像素坐标系到图像物理坐标系

双目相机的畸变校正以及平行校正（极线校正）的入门问题总结_wangxiaokun671903的专栏-CSDN博客_极线校正

RoboMaster视觉教程（8）串口通讯_江达小记-CSDN博客