merge

armor/include/armor_finder/armor_finder.h

@@ -30,7 +30,7 @@ private:
     typedef cv::TrackerKCF TrackerToUse;                // Tracker类型定义
 
     typedef enum{
-        SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
+        WEAKSEARCHING_STATE, SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
     } State;                                            // 自瞄状态枚举定义
 
     const uint8_t &enemy_color;                         // 敌方颜色，引用外部变量，自动变化
@@ -40,9 +40,13 @@ private:
     cv::Ptr<cv::Tracker> tracker;                       // tracker对象实例
     Classifier classifier;                              // CNN分类器对象实例，用于数字识别
     int  contour_area;                                  // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
+    int tracking_cnt;                                   // 记录追踪帧数，用于定时退出追踪
+    int miss_cnt;                                       //
+    int weak_cnt;                                       //
 	Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
     const uint8_t &use_classifier;                      // 标记是否启用CNN分类器，引用外部变量，自动变化
 
+
     bool stateSearchingTarget(cv::Mat &src);            // searching state主函数
     bool stateTrackingTarget(cv::Mat &src);             // tracking state主函数
     bool stateStandBy();                                // stand by state主函数（已弃用）

armor/src/armor_finder/armor_finder.cpp

@@ -8,17 +8,17 @@
 #include <opencv2/highgui.hpp>
 #include <armor_finder/armor_finder.h>
 
-std::map<int, string> id2name = {
+std::map<int, string> id2name = {                               //装甲板id到名称的map
         {-1, "OO"},{ 0, "NO"},
         { 1, "B1"},{ 2, "B2"},{ 3, "B3"},{ 4, "B4"},{ 5, "B5"},{ 6, "B7"},{ 7, "B8"},
         { 8, "R1"},{ 9, "R2"},{10, "R3"},{11, "R4"},{12, "R5"},{13, "R7"},{14, "R8"},
-};                                                              //装甲板id到名称的map
+};
 
-std::map<string, int> name2id = {
+std::map<string, int> name2id = {                               //装甲板名称到id的map
         {"OO", -1},{"NO",  0},
         {"B1",  1},{"B2",  2},{"B3",  3},{"B4",  4},{"B5",  5},{"B7",  6},{"B8",  7},
         {"R1",  8},{"R2",  9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14},
-};                                                              //装甲板名称到id的map
+};
 
 ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &use) :
             serial(u),
@@ -27,23 +27,33 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
             classifier(paras_folder),
             contour_area(0),
             use_classifier(use),
-            boxid(-1)
+            boxid(-1),
+            tracking_cnt(0),
+            miss_cnt(0),
+            weak_cnt(0)
             {
 }
 
 void ArmorFinder::run(cv::Mat &src) {
-    static int tracking_cnt = 0;        // 记录追踪帧数，用于定时退出追踪
     cv::Mat src_use = src.clone();      // 实际参与计算的图像对象
 
     if(show_armor_box){                 // 根据条件显示当前目标装甲板
         showArmorBox("box", src, armor_box, boxid);
         cv::waitKey(1);
     }
-//    stateSearchingTarget(src_use);
+//    stateSearchingTarget(src_use);                    // for debug
 //    return;
     switch (state){
+        case WEAKSEARCHING_STATE:
+            if(stateSearchingTarget(src_use) && ++weak_cnt>5){
+                miss_cnt = 0;
+                state = SEARCHING_STATE;
+            }else{
+                weak_cnt = 0;
+            }
         case SEARCHING_STATE:
             if(stateSearchingTarget(src_use)){
+                miss_cnt = 0;
                 if((armor_box & cv::Rect2d(0, 0, 640, 480)) == armor_box) { // 判断装甲板区域是否脱离图像区域
                     if(!classifier || !use_classifier){                     /* 如果分类器不可用或者不使用分类器 */
                         cv::Mat roi = src_use.clone()(armor_box), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
@@ -57,10 +67,14 @@ void ArmorFinder::run(cv::Mat &src) {
                     tracking_cnt = 0;
                     LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
                 }
+            }else if(++miss_cnt>100){
+                weak_cnt = 0;
+                state = WEAKSEARCHING_STATE;
             }
             break;
         case TRACKING_STATE:
-            if(++tracking_cnt>100 || !stateTrackingTarget(src_use)){    // 最多追踪100帧图像
+            if(!stateTrackingTarget(src_use) || ++tracking_cnt>100){    // 最多追踪100帧图像
+                miss_cnt = 0;
                 state = SEARCHING_STATE;
                 LOGM(STR_CTR(WORD_LIGHT_YELLOW ,"into search!"));
             }
@@ -71,10 +85,6 @@ void ArmorFinder::run(cv::Mat &src) {
     }
 }
 
-#define FOCUS_PIXAL_8MM  (1488)
-#define FOCUS_PIXAL_5MM  (917)
-#define FOCUS_PIXAL      FOCUS_PIXAL_5MM
-
 bool sendTarget(Serial& serial, double x, double y, double z) {
 	static short x_tmp, y_tmp, z_tmp;
 	static time_t last_time = time(nullptr);

armor/src/armor_finder/searching_state/searching_state.cpp

@@ -291,7 +291,7 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
         showArmorBoxVector("boxes", src, armor_boxes);
         cv::waitKey(1);
     }
-    if (classifier && use_classifier) {
+    if (classifier && use_classifier && state!=WEAKSEARCHING_STATE) {
         for (auto box : armor_boxes) {
             cv::Mat roi = src(box).clone();
             cv::resize(roi, roi, cv::Size(48, 36));