diff --git a/samples/demo/gimbal_detection_with_clicked_tracking.cpp b/samples/demo/gimbal_detection_with_clicked_tracking.cpp index f220831..b252259 100644 --- a/samples/demo/gimbal_detection_with_clicked_tracking.cpp +++ b/samples/demo/gimbal_detection_with_clicked_tracking.cpp @@ -2,35 +2,16 @@ #include // 包含SpireCV SDK头文件 #include - -using namespace std; +#include // 定义窗口名称 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 *); - -struct node -{ - 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; +bool isTarck = false; +bool isStartTarck = false; +int clickX = -1, clickY = -1; // 定义吊舱 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 - sv::TargetsInFrame tgts(frame_id++); - // 读取一帧图像到img - cap.read(img); + isStartTarck = false; + isTarck = false; + } + + if (isStartTarck && !isTarck) + { + // 使用上一帧结果初始化追踪器 + std::map 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 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 " < 0) + { 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); - // 打印 跟踪目标 的中心位置，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); - } + // 打印 跟踪目标 的中心位置，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) { - // 更新框选区域坐标 - pt_origin.x = 0; - pt_origin.y = 0; + if (!isTarck && !isStartTarck) + { + clickX = x; + clickY = y; + isStartTarck = true; + } } - // 左键按下 - if (event == cv::EVENT_LBUTTONDOWN) + // 右键点击 退出追踪模式 + else if (event == cv::MouseEventTypes::EVENT_RBUTTONDOWN) { - detect_tracking = true; - pt_origin = cv::Point(x, y); + if (isTarck) + { + clickX = -1; + clickY = -1; + isTarck = false; + } } - - else if (event == cv::EVENT_RBUTTONDOWN) + // 中键点击 归中（仅非追踪模式下有效） + else if (event == cv::MouseEventTypes::EVENT_MBUTTONDOWN) { - detect_tracking = true; - b_renew_ROI = false; - b_begin_TRACK = false; - b_clicked = true; + if (!isTarck) + { + gimbal->setHome(); + } } } @@ -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 << "=============================================" << std::endl; - std::cout << "HALL_yaw:" << frame_ang_y << " " << std::endl; - std::cout << "HALL_roll:" << frame_ang_r << " " << std::endl; - std::cout << "HALL_pitch:" << frame_ang_p << " " << std::endl; - std::cout << "GYRO_yaw:" << imu_ang_y << " " << std::endl; - std::cout << "GYRO_roll:" << imu_ang_r << " " << std::endl; - std::cout << "GYRO_pitch:" << imu_ang_p << " " << std::endl; + // std::cout << "GIMBAL_CMD_RCV_POS" << std::endl; + // std::cout << "=============================================" << std::endl; + // std::cout << "HALL_yaw:" << frame_ang_y << " " << std::endl; + // std::cout << "HALL_roll:" << frame_ang_r << " " << std::endl; + // std::cout << "HALL_pitch:" << frame_ang_p << " " << std::endl; + // std::cout << "GYRO_yaw:" << imu_ang_y << " " << std::endl; + // std::cout << "GYRO_roll:" << imu_ang_r << " " << std::endl; + // std::cout << "GYRO_pitch:" << imu_ang_p << " " << std::endl; count = 0; } count++;