optimize gimbal track

samples/demo/gimbal_detection_with_clicked_tracking.cpp

@@ -2,35 +2,16 @@
 #include <string>
 // 包含SpireCV SDK头文件
 #include <sv_world.h>
-
+#include <map>
-using namespace std;
 
 // 定义窗口名称
 static const std::string RGB_WINDOW = "Image window";
-// 框选到的矩形
-cv::Rect rect_sel;
-// 框选起始点
-cv::Point pt_origin;
 
-// 是否得到一个新的框选区域
-bool b_renew_ROI = false;
-// 是否开始跟踪
-bool b_begin_TRACK = false;
 // 实现框选逻辑的回调函数
 void onMouse(int event, int x, int y, int, void *);
-
+bool isTarck = false;
-struct node
+bool isStartTarck = false;
-{
+int clickX = -1, clickY = -1;
-  double x, y;
-};
-node p1, p2, p3, p4;
-node p;
-double getCross(node p1, node p2, node p)
-{
-  return (p2.x - p1.x) * (p.y - p1.y) - (p.x - p1.x) * (p2.y - p1.y);
-}
-bool b_clicked = false;
-bool detect_tracking = true;
 
 // 定义吊舱
 sv::Gimbal *gimbal;
@@ -85,25 +66,83 @@ int main(int argc, char *argv[])
   // 实例化OpenCV的Mat类，用于内存单帧图像
   cv::Mat img;
   int frame_id = 0;
+  sv::TargetsInFrame lastTgts(frame_id);
   while (1)
   {
-    if (detect_tracking == true)
+    // 如果位标非法 则不能进行任何形式的追踪
+    if (clickX == -1 || clickY == -1)
     {
-      // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame
+      isStartTarck = false;
-      sv::TargetsInFrame tgts(frame_id++);
+      isTarck = false;
-      // 读取一帧图像到img
+    }
-      cap.read(img);
+
+    if (isStartTarck && !isTarck)
+    {
+      // 使用上一帧结果初始化追踪器
+      std::map<int, cv::Rect> inBoxList;
+
+      // 计算点击事件是否位于每个目标框内
+      for (int i = 0; i < lastTgts.targets.size(); i++)
+      {
+        int halfWidht = (lastTgts.targets[i].w * lastTgts.width) / 2;
+        int halfHeight = (lastTgts.targets[i].h * lastTgts.height) / 2;
+        int x = lastTgts.targets[i].cx * lastTgts.width;
+        int y = lastTgts.targets[i].cy * lastTgts.height;
+
+        int diffX = x - clickX;
+        int diffY = y - clickY;
+
+        if ((abs(diffX) < halfWidht) && (abs(diffY) < halfHeight))
+        {
+          std::pair<int, cv::Rect> point;
+          point.first = diffX * diffX + diffY * diffY;
+          point.second = cv::Rect(x - halfWidht, y - halfHeight, halfWidht * 2, halfHeight * 2);
+          inBoxList.insert(point);
+        }
+      }
+
+      // 取离中心点最近的目标进行跟踪
+      int min = 0X7FFFFFFF;
+      cv::Rect sel;
+      for (auto i = inBoxList.begin(); i != inBoxList.end(); i++)
+      {
+        if (i->first < min)
+        {
+          min = i->first;
+          sel = i->second;
+        }
+      }
+
+      // min被赋值则存在一个目标框与点击的点重合
+      if (min != 0X7FFFFFFF)
+      {
+        sot.init(img, sel);
+        isTarck = true;
+        printf("rect_sel Yes\n");
+      }
+      else
+      {
+        isTarck = false;
+        printf("rect_sel No\n");
+      }
+
+      isStartTarck = false;
+    }
+
+    sv::TargetsInFrame tgts(frame_id++);
+    // 读取一帧图像
+    cap.read(img);
+
+    if (!isTarck)
+    {
+      // 缩放图像尺寸用于适配检测器
       cv::resize(img, img, cv::Size(cod.image_width, cod.image_height));
 
-      // 执行通用目标检测
+      // 进行通用目标检测
       cod.detect(img, tgts);
-
       gimbalNoTrack();
 
-      // 可视化检测结果，叠加到img上
+      // 向控制台输出检测结果
-      sv::drawTargetsInFrame(img, tgts);
-
-      // 控制台打印通用目标检测结果
       printf("Frame-[%d]\n", frame_id);
       // 打印当前检测的FPS
       printf("  FPS = %.2f\n", tgts.fps);
@@ -124,109 +163,78 @@ int main(int argc, char *argv[])
         printf("  Object Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[i].los_ax, tgts.targets[i].los_ay);
         // 打印每个目标的3D位置（在相机坐标系下），跟目标实际长宽、相机参数相关
         printf("  Object Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[i].px, tgts.targets[i].py, tgts.targets[i].pz);
-        p1.x = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
-        p1.y = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
-        p2.x = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
-        p2.y = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
-        p4.x = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
-        p4.y = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
-        p3.x = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
-        p3.y = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
-        p.x = pt_origin.x;
-        p.y = pt_origin.y;
-        std::cout << "p.x " << p.x << "\t"
-                  << "p.y " << p.y << std::endl;
-        if (getCross(p1, p2, p) * getCross(p3, p4, p) >= 0 && getCross(p2, p3, p) * getCross(p4, p1, p) >= 0)
-        {
-          b_begin_TRACK = false;
-          detect_tracking = false;
-          // pt_origin = cv::Point(nor_x, nor_p_y);
-          // std::cout << "pt_origin  " <<nor_x<<"/t"<<nor_p_y<< std::endl;
-          rect_sel = cv::Rect(p1.x, p1.y, tgts.targets[i].w * tgts.width, tgts.targets[i].h * tgts.height);
-          // std::cout << rect_sel << std::endl;
-          b_renew_ROI = true;
-          frame_id = 0;
-          printf("rect_sel Yes\n");
-        }
-        else
-        {
-          printf("rect_sel No\n");
-        }
       }
     }
     else
     {
-      // 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame
+      // 缩放图像尺寸用于适配追踪器
-      sv::TargetsInFrame tgts(frame_id++);
-      // 读取一帧图像到img
-      cap.read(img);
       cv::resize(img, img, cv::Size(sot.image_width, sot.image_height));
-      // 开始 单目标跟踪 逻辑
-      // 是否有新的目标被手动框选
-      if (b_renew_ROI)
-      {
-        // 拿新的框选区域 来 初始化跟踪器
-        sot.init(img, rect_sel);
-        // std::cout << rect_sel << std::endl;
-        // 重置框选标志
-        b_renew_ROI = false;
-        // 开始跟踪
-        b_begin_TRACK = true;
-      }
-      else if (b_begin_TRACK)
-      {
-        // 以前一帧的结果继续跟踪
-        sot.track(img, tgts);
-        gimbalTrack(tgts.targets[0].cx - 0.5f, tgts.targets[0].cy - 0.5f);
 
-        // 可视化检测结果，叠加到img上
+      // 图像追踪
-        sv::drawTargetsInFrame(img, tgts);
+      sot.track(img, tgts);
-        // 控制台打印 单目标跟踪 结果
+
+      // 吊舱追踪
+      gimbalTrack(tgts.targets[0].cx - 0.5f, tgts.targets[0].cy - 0.5f);
+
+      // 向控制台输出追踪结果
+      printf("Frame-[%d]\n", frame_id);
+      // 打印当前检测的FPS
+      printf("  FPS = %.2f\n", tgts.fps);
+      // 打印当前相机的视场角（degree）
+      printf("  FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y);
+      if (tgts.targets.size() > 0)
+      {
         printf("Frame-[%d]\n", frame_id);
-        // 打印当前检测的FPS
+        // 打印 跟踪目标 的中心位置，cx，cy的值域为[0, 1]
-        printf("  FPS = %.2f\n", tgts.fps);
+        printf("  Tracking Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[0].cx, tgts.targets[0].cy);
-        // 打印当前相机的视场角（degree）
+        // 打印 跟踪目标 的外接矩形框的宽度、高度，w，h的值域为(0, 1]
-        printf("  FOV (fx, fy) = (%.2f, %.2f)\n", tgts.fov_x, tgts.fov_y);
+        printf("  Tracking Size (w, h) = (%.3f, %.3f)\n", tgts.targets[0].w, tgts.targets[0].h);
-        if (tgts.targets.size() > 0)
+        // 打印 跟踪目标 的视线角，跟相机视场相关
-        {
+        printf("  Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay);
-          printf("Frame-[%d]\n", frame_id);
-          // 打印 跟踪目标 的中心位置，cx，cy的值域为[0, 1]
-          printf("  Tracking Center (cx, cy) = (%.3f, %.3f)\n", tgts.targets[0].cx, tgts.targets[0].cy);
-          // 打印 跟踪目标 的外接矩形框的宽度、高度，w，h的值域为(0, 1]
-          printf("  Tracking Size (w, h) = (%.3f, %.3f)\n", tgts.targets[0].w, tgts.targets[0].h);
-          // 打印 跟踪目标 的视线角，跟相机视场相关
-          printf("  Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay);
-        }
       }
-    } // end of tracking
+    }
+
+    // 叠加目标框至原始图像
+    sv::drawTargetsInFrame(img, tgts);
     // 显示检测结果img
     cv::imshow(RGB_WINDOW, img);
     cv::waitKey(10);
+
+    lastTgts = tgts;
   }
+
   return 0;
 }
 
 void onMouse(int event, int x, int y, int, void *)
 {
-  if (b_clicked)
+  // 左键点击 进入追踪模式
+  if (event == cv::MouseEventTypes::EVENT_LBUTTONDOWN)
   {
-    // 更新框选区域坐标
+    if (!isTarck && !isStartTarck)
-    pt_origin.x = 0;
+    {
-    pt_origin.y = 0;
+      clickX = x;
+      clickY = y;
+      isStartTarck = true;
+    }
   }
-  // 左键按下
+  // 右键点击 退出追踪模式
-  if (event == cv::EVENT_LBUTTONDOWN)
+  else if (event == cv::MouseEventTypes::EVENT_RBUTTONDOWN)
   {
-    detect_tracking = true;
+    if (isTarck)
-    pt_origin = cv::Point(x, y);
+    {
+      clickX = -1;
+      clickY = -1;
+      isTarck = false;
+    }
   }
-
+  // 中键点击 归中（仅非追踪模式下有效）
-  else if (event == cv::EVENT_RBUTTONDOWN)
+  else if (event == cv::MouseEventTypes::EVENT_MBUTTONDOWN)
   {
-    detect_tracking = true;
+    if (!isTarck)
-    b_renew_ROI = false;
+    {
-    b_begin_TRACK = false;
+      gimbal->setHome();
-    b_clicked = true;
+    }
   }
 }
 
@@ -237,14 +245,14 @@ void GimableCallback(double &frame_ang_r, double &frame_ang_p, double &frame_ang
   static int count = 0;
   if (count == 25)
   {
-    std::cout << "GIMBAL_CMD_RCV_POS" << std::endl;
+    // std::cout << "GIMBAL_CMD_RCV_POS" << std::endl;
-    std::cout << "=============================================" << std::endl;
+    // std::cout << "=============================================" << std::endl;
-    std::cout << "HALL_yaw:" << frame_ang_y << " " << std::endl;
+    // std::cout << "HALL_yaw:" << frame_ang_y << " " << std::endl;
-    std::cout << "HALL_roll:" << frame_ang_r << " " << std::endl;
+    // std::cout << "HALL_roll:" << frame_ang_r << " " << std::endl;
-    std::cout << "HALL_pitch:" << frame_ang_p << " " << std::endl;
+    // std::cout << "HALL_pitch:" << frame_ang_p << " " << std::endl;
-    std::cout << "GYRO_yaw:" << imu_ang_y << " " << std::endl;
+    // std::cout << "GYRO_yaw:" << imu_ang_y << " " << std::endl;
-    std::cout << "GYRO_roll:" << imu_ang_r << " " << std::endl;
+    // std::cout << "GYRO_roll:" << imu_ang_r << " " << std::endl;
-    std::cout << "GYRO_pitch:" << imu_ang_p << " " << std::endl;
+    // std::cout << "GYRO_pitch:" << imu_ang_p << " " << std::endl;
     count = 0;
   }
   count++;