!32 优化吊舱追踪的例程
Merge pull request !32 from AiYangSky/optimize-gimbal-track
This commit is contained in:
jario-jin
Gitee
commit
e5c00087ed
|
|
@ -2,35 +2,16 @@
|
|||
#include <string>
|
||||
// 包含SpireCV SDK头文件
|
||||
#include <sv_world.h>
|
||||
|
||||
using namespace std;
|
||||
#include <map>
|
||||
|
||||
// 定义窗口名称
|
||||
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
|
||||
isStartTarck = false;
|
||||
isTarck = false;
|
||||
}
|
||||
|
||||
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++);
|
||||
// 读取一帧图像到img
|
||||
// 读取一帧图像
|
||||
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,64 +163,20 @@ 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);
|
||||
// 控制台打印 单目标跟踪 结果
|
||||
// 向控制台输出追踪结果
|
||||
printf("Frame-[%d]\n", frame_id);
|
||||
// 打印当前检测的FPS
|
||||
printf(" FPS = %.2f\n", tgts.fps);
|
||||
|
|
@ -198,35 +193,48 @@ int main(int argc, char *argv[])
|
|||
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)
|
||||
{
|
||||
detect_tracking = true;
|
||||
pt_origin = cv::Point(x, y);
|
||||
}
|
||||
|
||||
else if (event == cv::EVENT_RBUTTONDOWN)
|
||||
// 右键点击 退出追踪模式
|
||||
else if (event == cv::MouseEventTypes::EVENT_RBUTTONDOWN)
|
||||
{
|
||||
detect_tracking = true;
|
||||
b_renew_ROI = false;
|
||||
b_begin_TRACK = false;
|
||||
b_clicked = true;
|
||||
if (isTarck)
|
||||
{
|
||||
clickX = -1;
|
||||
clickY = -1;
|
||||
isTarck = false;
|
||||
}
|
||||
}
|
||||
// 中键点击 归中（仅非追踪模式下有效）
|
||||
else if (event == cv::MouseEventTypes::EVENT_MBUTTONDOWN)
|
||||
{
|
||||
if (!isTarck)
|
||||
{
|
||||
gimbal->setHome();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue