Fixed bugs,Added assist label.

autoaim/autoaim.cpp

@@ -4,6 +4,12 @@
 using namespace cv;
 using namespace std;
 
+#ifdef ASSIST_LABEL
+int cnt = 0;
+const string path_prefix = "/home/tup/Desktop/TUP-Vision-Infantry-2022/data/";
+ofstream file;
+#endif //ASSIST_LABEL
+
 //使用海伦公式计算三角形面积
 inline float calcTriangleArea(cv::Point2f pts[3])
 {
@@ -28,8 +34,8 @@ Autoaim::Autoaim()
     coordsolver.loadParam(camera_param_path,"Coord");
     lost_cnt = 0;
     is_last_target_exist = false;
-    // input_size = {640,384};
-    input_size = {416,416};
+    input_size = {640,384};
+    // input_size = {416,416};
 }
 
 Autoaim::~Autoaim()
@@ -99,6 +105,10 @@ bool Autoaim::run(Image &src,VisionData &data)
     auto time_crop=std::chrono::steady_clock::now();
     if (!detector.detect(input, objects))
     {
+#ifdef SHOW_AIM_CROSS
+        line(src.img, Point2f(src.img.size().width / 2, 0), Point2f(src.img.size().width / 2, src.img.size().height), Scalar(0,255,0), 1);
+        line(src.img, Point2f(0, src.img.size().height / 2), Point2f(src.img.size().width, src.img.size().height / 2), Scalar(0,255,0), 1);
+#endif //SHOW_AIM_CROSS
 #ifdef SHOW_IMG
         imshow("dst",src.img);
         waitKey(1);
@@ -107,9 +117,14 @@ bool Autoaim::run(Image &src,VisionData &data)
         is_last_target_exist = false;
         return false;
     }
+#ifdef ASSIST_LABEL
+    auto img_name = path_prefix + to_string(cnt) + ".png";
+    imwrite(img_name,input);
+#endif //ASSIST_LABEL
     auto time_infer = std::chrono::steady_clock::now();
     for (auto object : objects)
     {
+
 #ifdef DETECT_RED
         if (object.color != 1)
             continue;
@@ -118,6 +133,7 @@ bool Autoaim::run(Image &src,VisionData &data)
         if (object.color != 0)
             continue;
 #endif //DETECT_BLUE
+
         Armor armor;
         armor.id = object.cls;
         armor.color = object.color;
@@ -144,7 +160,7 @@ bool Autoaim::run(Image &src,VisionData &data)
             line(src.img, armor.apex2d[i % 4], armor.apex2d[(i + 1) % 4], Scalar(0,255,0), 1);
 #endif //SHOW_ALL_ARMOR
 
-        auto center = coordsolver.pnp(armor.apex2d, SOLVEPNP_IPPE);
+        auto center = coordsolver.pnp(armor.apex2d, SOLVEPNP_IPPE).coord;
 
 #ifdef USING_IMU
 #endif //USING_IMU
@@ -159,20 +175,18 @@ bool Autoaim::run(Image &src,VisionData &data)
         for (auto iter = predictors_map.begin(); iter != predictors_map.end();)
         {
             //删除元素后迭代器会失效，需先行获取下一元素
-            auto next = ++iter;
-            iter--;
+            auto next = iter;
             if (src.timestamp - (*iter).second.last_target.timestamp >= max_delta_t)
-            {
-                predictors_map.erase(iter);
-            }
+                next = predictors_map.erase(iter);
+            else
+                ++next;
             iter = next;
         }
     }
 
 
     std::list<std::future<void>> predict_tasks;
     //为装甲板分配或新建最佳预测器
-    // cout<<"walking"<<endl;
     for (auto armor = armors.begin(); armor != armors.end(); ++armor)
     {
         auto predictors_with_same_key = predictors_map.count((*armor).key);
@@ -264,14 +278,11 @@ bool Autoaim::run(Image &src,VisionData &data)
 
         }
     }
-    // cout<<"walked"<<endl;
     // 等待所有预测任务完成
     // cout<<"total"<<predict_tasks.size()<<endl;
-    int f = 0;
     // waitKey(20);
     for (auto task = predict_tasks.begin(); task != predict_tasks.end(); ++task)
     {
-        // cout<<"waiting"<<f++<<endl;
         (*task).wait(); 
     }
     auto target = chooseTarget(armors);
@@ -283,6 +294,33 @@ bool Autoaim::run(Image &src,VisionData &data)
     auto predict_info = target.center3d;
 #endif //USING_PREDICT
 
+#ifdef ASSIST_LABEL
+    auto label_name = path_prefix + to_string(cnt) + ".txt";
+    string content;
+
+    int cls = 0;
+    if (target.id == 7)
+        cls = 9 * target.color - 1;
+    if (target.id != 7)
+        cls = target.id + target.color * 9;
+
+    content.append(to_string(cls) + " ");
+    for (auto apex : target.apex2d)
+    {
+        content.append(to_string((apex.x - roi_offset.x) / input_size.width));
+        content.append(" ");
+        content.append(to_string((apex.y - roi_offset.y) / input_size.height));
+        content.append(" ");
+    }
+    content.pop_back();
+    cout<<to_string(cnt) + " "<<content<<endl;
+    file.open(label_name,std::ofstream::app);
+    file<<content;
+    file.close();
+    cnt++;
+    sleep(0.05);+
+#endif //ASSIST_LABEL
+
     Point2f apex_sum;
     //装甲板中心
     for(auto apex : target.apex2d)
@@ -302,17 +340,18 @@ bool Autoaim::run(Image &src,VisionData &data)
         line(src.img, Point2f(src.img.size().width / 2, 0), Point2f(src.img.size().width / 2, src.img.size().height), Scalar(0,255,0), 1);
         line(src.img, Point2f(0, src.img.size().height / 2), Point2f(src.img.size().width, src.img.size().height / 2), Scalar(0,255,0), 1);
 #endif //SHOW_FPS
+
     auto angle = coordsolver.getAngle(predict_info);
     auto time_predict = std::chrono::steady_clock::now();
     double dr_full_ms = std::chrono::duration<double,std::milli>(time_predict - time_start).count();
+
 #ifdef SHOW_FPS
     putText(src.img, "FPS:" + to_string(int(1000 / dr_full_ms)), Point2i(20,20), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2);
 #endif //SHOW_FPS
 
 #ifdef SHOW_IMG
     imshow("dst",src.img);
     waitKey(1);
-
 #endif //SHOW_IMG
 
 #ifdef PRINT_LATENCY

autoaim/autoaim.h

@@ -8,7 +8,7 @@
 #include "predictor/predictor.h"
 #include "../serial/serialport.h"
 
-const string network_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/model/yolox.xml";
+const string network_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/model/nano.xml";
 const string camera_param_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/params/coord_param.yaml";
 const string predict_param_path = "/home/tup/Desktop/TUP-Vision-Infantry-2022/params/filter/filter_param.yaml";
 

autoaim/debug.h

@@ -8,12 +8,14 @@
 // #define PRINT_TARGET_INFO //是否输出目标信息
 // #define SHOW_FPS    //是否显示FPS
 #define SHOW_PREDICT //是否显示预测
-// #define SHOW_AIM_CROSS//是否绘制十字瞄准线
+#define SHOW_AIM_CROSS//是否绘制十字瞄准线
 #endif // SHOW_IMG
 
 
 // #define PRINT_LATENCY //输出运行时间
 // #define PRINT_PRED_MAP_STATUS //输出目前预测器情况
 #define USING_PREDICT           //是否启用预测
 //TODO:陀螺仪
-// #define USING_IMU   //是否使用陀螺仪数据
+// #define USING_IMU   //是否使用陀螺仪数据
+
+// #define ASSIST_LABEL //辅助数据集标注

autoaim/detector/inference.cpp

@@ -4,22 +4,19 @@
 
 #include "inference.h"
 
-
-
-
 /**
  * @brief Define names based depends on Unicode path support
  */
 #define tcout                  std::cout
 #define file_name_t            std::string
 #define imread_t               cv::imread
 #define NMS_THRESH 0.1
-#define BBOX_CONF_THRESH 0.8
+#define BBOX_CONF_THRESH 0.7
 
-// static constexpr int INPUT_W = 640;    // Width of input
-// static constexpr int INPUT_H = 384;    // Height of input
-static constexpr int INPUT_W = 416;    // Width of input
-static constexpr int INPUT_H = 416;    // Height of input
+static constexpr int INPUT_W = 640;    // Width of input
+static constexpr int INPUT_H = 384;    // Height of input
+// static constexpr int INPUT_W = 416;    // Width of input
+// static constexpr int INPUT_H = 416;    // Height of input
 static constexpr int NUM_CLASSES = 8;  // Number of classes
 static constexpr int NUM_COLORS = 3;   // Number of color
 static constexpr int TOPK = 128;       // TopK
@@ -221,6 +218,7 @@ static void qsort_descent_inplace(std::vector<Object>& objects)
     qsort_descent_inplace(objects, 0, objects.size() - 1);
 }
 
+
 static void nms_sorted_bboxes(const std::vector<Object>& faceobjects, std::vector<int>& picked,
                             float nms_threshold)
 {

autoaim/predictor/predictor.cpp

@@ -36,7 +36,6 @@ bool Predictor::initParam(string coord_path)
     return true;
 }
 
-//TODO:融合粒子滤波与拟合
 Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp)
 {
     auto t1=std::chrono::steady_clock::now();
@@ -49,8 +48,21 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp)
         last_target = target;
         return xyz;
     }
+
+    auto d_xyz = target.xyz - last_target.xyz;
+    auto delta_t = timestamp - last_target.timestamp;
+    auto last_dist = history_info.back().dist;
+    auto delta_time_estimate = ((last_dist / 100.f) / bullet_speed) * 1000 + delay;
+    auto time_estimate = delta_time_estimate + history_info.back().timestamp - history_info.front().timestamp;
+    //如速度过大,可认为为噪声干扰,进行滑窗滤波滤除
+    if (d_xyz.norm() / (delta_t * 10.f) >= max_v)
+    {
+        auto filtered_xyz = shiftWindowFilter(history_info.size() - window_size - 2);
+        target = {filtered_xyz, (int)filtered_xyz.norm(), timestamp};
+    }
+
     //当队列长度不足时不使用拟合
-    else if (history_info.size() < history_deque_len)
+    if (history_info.size() < history_deque_len)
     {
         history_info.push_back(target);
         fitting_disabled = true;
@@ -68,24 +80,12 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp)
         history_info.push_back(target);
         fitting_disabled = false;
     }
-
-    auto d_xyz = target.xyz - last_target.xyz;
-    auto delta_t = timestamp - last_target.timestamp;
-    auto last_dist = history_info.back().dist;
-    auto delta_time_estimate = ((last_dist / 100.f) / bullet_speed) * 1000 + delay;
-    auto time_estimate = delta_time_estimate + history_info.back().timestamp - history_info.front().timestamp;
-    //如速度过大,可认为为噪声干扰,进行滑窗滤波滤除
-    if (d_xyz.norm() / (delta_t * 10.f) >= max_v && history_info.size() == history_deque_len)
-    {
-        auto filtered_xyz = shiftWindowFilter();
-        target = {filtered_xyz, (int)filtered_xyz.norm(), timestamp};
-    }
-    Eigen::Vector3d result = {0,0,0};
-    Eigen::Vector3d result_pf = {0,0,0};
-    Eigen::Vector3d result_fitting = {0,0,0};
+    Eigen::Vector3d result = {0, 0, 0};
+    Eigen::Vector3d result_pf = {0, 0, 0};
+    Eigen::Vector3d result_fitting = {0, 0, 0};
     PredictStatus is_pf_available;
     PredictStatus is_fitting_available;
-    //需注意粒子滤波使用相对时间（自上一次检测时所经过ms数），拟合使用绝对时间（自程序启动时所经过ms数）
+    //需注意粒子滤波使用相对时间（自上一次检测时所经过ms数），拟合使用自首帧所经过时间
     if (fitting_disabled)
     {
         auto is_pf_available = predict_pf_run(target, result_pf, delta_time_estimate);
@@ -99,7 +99,7 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp)
         is_fitting_available = get_fitting_available.get();
     }
 
-    // 进行融合
+    //进行融合
     if (is_fitting_available.xyz_status[0] && !fitting_disabled)
         result[0] = result_fitting[0];
     else
@@ -151,23 +151,27 @@ Eigen::Vector3d Predictor::predict(Eigen::Vector3d xyz, int timestamp)
 }
 
 
- inline Eigen::Vector3d Predictor::shiftWindowFilter()
+inline Eigen::Vector3d Predictor::shiftWindowFilter(int start_idx=0)
 {
-    auto max_iter = history_deque_len - window_size + 1;
-    Eigen::Vector3d total_sum = {0,0,0};
-    for(int i = 0; i < max_iter; i++)
+    //计算最大迭代次数
+    auto max_iter = int(history_info.size() - start_idx) - window_size + 1;
+    Eigen::Vector3d total_sum = {0, 0, 0};
+    // cout<<history_info.size()<<endl;
+    // cout<<max_iter<<endl;
+    // cout<<start_idx<<endl;
+    if (max_iter == 0 || start_idx < 0)
+        return history_info.back().xyz;
+
+    for (int i = 0; i < max_iter; i++)
     {
-        auto window_sum = [&]()
-        {
-            Vector3d sum = {0,0,0};
-            for(int j = 0; j < window_size; j++)
-                sum = sum + history_info.at(i + j).xyz;
-            return sum / window_size;
-        };
-        total_sum = total_sum + window_sum();
+        Eigen::Vector3d sum = {0,0,0};
+        for (int j = 0; j < window_size; j++)
+            sum += history_info.at(start_idx + i + j).xyz;
+        total_sum += sum / window_size;
     }
     return total_sum / max_iter;
 }
+
 /**
  * @brief 进行一次粒子滤波预测
  * @param target 本次预测目标信息

autoaim/predictor/predictor.h

@@ -52,6 +52,7 @@ class Predictor
     bool initParam(string coord_path);
     Predictor generate();
     Eigen::Vector3d predict(Eigen::Vector3d xyz,  int timestamp);
+    Eigen::Vector3d shiftWindowFilter(int start_idx);
     PredictStatus predict_pf_run(TargetInfo target, Vector3d &result, int time_estimated);
     PredictStatus predict_fitting_run(Vector3d &result, int time_estimated);
 private:
@@ -63,14 +64,13 @@ class Predictor
     std::deque<TargetInfo> history_info;                                //目标队列
 
     const int max_timespan = 1000;                                       //最大时间跨度，大于该时间重置预测器(ms)
-    const int max_cost = 1e1;                                            //回归函数最大Cost
+    const int max_cost = 5e1;                                            //回归函数最大Cost
     const int max_v = 15;                                                //设置最大速度,单位m/s
     const int history_deque_len = 10;                                   //队列长度   
     const int bullet_speed = 30;                                        //TODO:弹速可变
     const int delay = 50;                                              //发弹延迟
-    const int window_size = 5;                                         //滑动窗口大小
+    const int window_size = 3;                                         //滑动窗口大小
 
-    Eigen::Vector3d shiftWindowFilter();
 };
 
 struct CURVE_FITTING_COST

coordsolver/coordsolver.cpp

@@ -48,7 +48,7 @@ bool CoordSolver::loadParam(string coord_path,string param_name)
     return true;
 }
 
-Eigen::Vector3d CoordSolver::pnp(Point2f apex[4], int method)
+PnPInfo CoordSolver::pnp(Point2f apex[4], int method)
 {
     std::vector<Point3d> points_world;
     std::vector<Point2f> points_pic(apex,apex + 4);
@@ -72,12 +72,25 @@ Eigen::Vector3d CoordSolver::pnp(Point2f apex[4], int method)
             {6.6,2.7,0}};
 
     Mat rvec;
+    Mat rmat;
     Mat tvec;
+    Eigen::Matrix3d rmat_eigen;
+    Eigen::Vector3d coord_world = {0, 0, 0};
+    Eigen::Vector3d tvec_eigen;
+    Eigen::Vector3d coord_camera;
 
     solvePnP(points_world, points_pic, intrinsic, dis_coeff, rvec, tvec, false, method);
 
-    Eigen::Vector3d result;
-    cv2eigen(tvec, result);
+    PnPInfo result;
+    //Pc = R * Pw + T
+    Rodrigues(rvec,rmat);
+    cv2eigen(rmat, rmat_eigen);
+    cv2eigen(tvec, tvec_eigen);
+    //转换至相机坐标系(左手坐标系)
+    result.coord = (rmat_eigen * coord_world) + tvec_eigen;
+    result.euler = rotationMatrixToEulerAngles(rmat_eigen) * 180 / CV_PI;
+    if (result.euler[0] <= 0)
+        result.euler[0] += 360;
     return result;
 }