parent
632dd9026b
commit
cdc98d9952
@ -1,131 +0,0 @@
|
|||||||
/*
|
|
||||||
* @Author: turboLIU
|
|
||||||
* @Date: 2022-07-27 14:12:19
|
|
||||||
* @LastEditTime: 2024-09-27 09:18:16
|
|
||||||
* @Description: Do not edit
|
|
||||||
* @FilePath: /C/include/detection_type_api.h
|
|
||||||
*/
|
|
||||||
#ifndef DETECTION_TYPE_API
|
|
||||||
#define DETECTION_TYPE_API
|
|
||||||
|
|
||||||
|
|
||||||
#define NMS_UNION 1
|
|
||||||
#define NMS_MIN 2
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
#include "PlatformDefine.h"
|
|
||||||
|
|
||||||
namespace AIGO{
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
IR = 1,
|
|
||||||
VIS = 2
|
|
||||||
}MODE;
|
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
CHW = 1,
|
|
||||||
HWC = 2,
|
|
||||||
WHC = 3,
|
|
||||||
}DATA_LAYOUT;
|
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
RGB = 1,
|
|
||||||
BGR = 2,
|
|
||||||
RGBA = 3,
|
|
||||||
BGRA = 4,
|
|
||||||
NV12 = 5, // uv
|
|
||||||
NV21 = 6, // vu
|
|
||||||
}INPUT_LAYOUT;
|
|
||||||
|
|
||||||
|
|
||||||
typedef enum{
|
|
||||||
AI_DT_UNDEFINED = -1, //未知数据类型，默认值。
|
|
||||||
AI_FLOAT = 0,
|
|
||||||
AI_FLOAT16 = 1,
|
|
||||||
AI_INT8 = 2,
|
|
||||||
AI_INT32 = 3,
|
|
||||||
AI_UINT8 = 4,
|
|
||||||
AI_INT16 = 6,
|
|
||||||
AI_UINT16 = 7,
|
|
||||||
AI_UINT32 = 8,
|
|
||||||
AI_INT64 = 9,
|
|
||||||
AI_UINT64 = 10,
|
|
||||||
AI_DOUBLE = 11,
|
|
||||||
AI_BOOL = 12,
|
|
||||||
AI_STRING = 13,
|
|
||||||
AI_COMPLEX64 = 16,
|
|
||||||
AI_COMPLEX128 = 17,
|
|
||||||
AI_BF16 = 27
|
|
||||||
}DATATYPE;
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct tag_ID_idx
|
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
int x_loc;
|
|
||||||
int y_loc;
|
|
||||||
int z_loc;
|
|
||||||
float score;
|
|
||||||
}ID_idx;
|
|
||||||
|
|
||||||
typedef struct tag_IDX
|
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
float x;
|
|
||||||
float y;
|
|
||||||
float z;
|
|
||||||
float score;
|
|
||||||
}IDX;
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct tag_Objinfo
|
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
float x1;
|
|
||||||
float y1;
|
|
||||||
float x2;
|
|
||||||
float y2;
|
|
||||||
float score;
|
|
||||||
int classNum;
|
|
||||||
int ID;
|
|
||||||
int trackID;
|
|
||||||
float area;
|
|
||||||
|
|
||||||
}objinfo;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
typedef struct tag_ImgMat
|
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
unsigned char* data=NULL;
|
|
||||||
int width=0;
|
|
||||||
int height=0;
|
|
||||||
int depth=0;
|
|
||||||
DATA_LAYOUT layout=HWC;
|
|
||||||
INPUT_LAYOUT inlayout = RGB;
|
|
||||||
int frameID=0;
|
|
||||||
MODE mode = IR;
|
|
||||||
// bool relayout=false;
|
|
||||||
long long timestamp = 0;
|
|
||||||
long long cfg = 0; // address of GD_VIDEO_FRAME_S structure
|
|
||||||
}ImgMat;
|
|
||||||
|
|
||||||
typedef struct tag_TenMat
|
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
void * data = nullptr;
|
|
||||||
int width=0;
|
|
||||||
int height=0;
|
|
||||||
int depth=0;
|
|
||||||
size_t dataByte=0;
|
|
||||||
DATATYPE type;
|
|
||||||
DATA_LAYOUT layout;
|
|
||||||
int elementNum = 0;
|
|
||||||
long long timestamp = 0;
|
|
||||||
}TenMat;
|
|
||||||
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
@ -0,0 +1,216 @@
|
|||||||
|
#include "Arith_ExtrinsicStitcher.h"
|
||||||
|
#include "Arith_VideoStitch.h"
|
||||||
|
#include "Arith_Utils.h"
|
||||||
|
#include "Arith_CoordModule.h"
|
||||||
|
#include "Arith_SysStruct.h"
|
||||||
|
using namespace cv;
|
||||||
|
|
||||||
|
ExtrinsicStitcher::ExtrinsicStitcher()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
ExtrinsicStitcher::~ExtrinsicStitcher()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
PanInfo ExtrinsicStitcher::InitMap(FrameInfo info)
|
||||||
|
{
|
||||||
|
// 设置拼接原点
|
||||||
|
SetOriginPoint(info);
|
||||||
|
|
||||||
|
// 转换关系计算
|
||||||
|
Proj t_Proj = AnlayseTform(info);
|
||||||
|
|
||||||
|
// 帧中心的大地坐标，将其平移到全景图中心
|
||||||
|
cv::Point2f ct_geo = Trans_uv2Geo(cv::Point2f(info.nWidth / 2, info.nHeight / 2), t_Proj.tf_p2g);
|
||||||
|
|
||||||
|
// 全景图初始化
|
||||||
|
PanInfo panPara = {0};
|
||||||
|
panPara.m_pan_width = 1000;//全景宽
|
||||||
|
panPara.m_pan_height = 1000;//全景高
|
||||||
|
panPara.scale = 0.5;//比例尺,1m = ?pix
|
||||||
|
|
||||||
|
// 直接无平移解算
|
||||||
|
auto cur = Trans_Geo2pan(ct_geo, panPara);
|
||||||
|
|
||||||
|
// 计算平移到全景图固定点的平移量，从此处开始拼接
|
||||||
|
int planX = 200;
|
||||||
|
int planY = 100;
|
||||||
|
|
||||||
|
panPara.map_shiftX = planX - (cur.x);//平移X
|
||||||
|
panPara.map_shiftY = planY - (cur.y);//平移Y
|
||||||
|
|
||||||
|
return panPara;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::project(cv::Point2f pos_pan, Proj m_Proj, PanInfo pan)
|
||||||
|
{
|
||||||
|
cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, pan);
|
||||||
|
cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p);
|
||||||
|
return pos_frame;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan)
|
||||||
|
{
|
||||||
|
cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g);
|
||||||
|
|
||||||
|
cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, pan);
|
||||||
|
|
||||||
|
return pos_pan;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ExtrinsicStitcher::SetOriginPoint(FrameInfo info)
|
||||||
|
{
|
||||||
|
originPoint = info;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
|
||||||
|
{
|
||||||
|
Mat point = (Mat_<double>(3, 1) << pos_frame.x, pos_frame.y, 1);
|
||||||
|
Mat result = form.R * point;
|
||||||
|
// 转局部地理系
|
||||||
|
double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
||||||
|
double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
||||||
|
// 平移到原点地理系
|
||||||
|
warpedX += form.T.at<double>(0, 0);
|
||||||
|
warpedY += form.T.at<double>(1, 0);
|
||||||
|
|
||||||
|
return cv::Point2f(warpedX, warpedY);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv)
|
||||||
|
{
|
||||||
|
// 先平移到当前相机位置
|
||||||
|
cv::Point2f pos_cam = pos_geo;
|
||||||
|
pos_cam.x = pos_geo.x + form_inv.T.at<double>(0, 0);
|
||||||
|
pos_cam.y = pos_geo.y + form_inv.T.at<double>(1, 0);
|
||||||
|
|
||||||
|
Mat point = (Mat_<double>(3, 1) << pos_cam.x, pos_cam.y, 1);
|
||||||
|
Mat result = form_inv.R * point;
|
||||||
|
|
||||||
|
// 转像方
|
||||||
|
double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
||||||
|
double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
||||||
|
|
||||||
|
return cv::Point2f(warpedX, warpedY);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::Trans_pan2Geo(cv::Point2f pos_pan, PanInfo panPara)
|
||||||
|
{
|
||||||
|
double x = (pos_pan.x - panPara.map_shiftX) / panPara.scale;
|
||||||
|
double y = (panPara.m_pan_height - (pos_pan.y - panPara.map_shiftY)) / panPara.scale;
|
||||||
|
|
||||||
|
return cv::Point2f(x, y);
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Point2f ExtrinsicStitcher::Trans_Geo2pan(cv::Point2f pos_geo, PanInfo panPara)
|
||||||
|
{
|
||||||
|
double pan_x = pos_geo.x * panPara.scale + panPara.map_shiftX;
|
||||||
|
double pan_y = (panPara.m_pan_height - pos_geo.y * panPara.scale) + panPara.map_shiftY;
|
||||||
|
return cv::Point2f(pan_x, pan_y);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Mat ExtrinsicStitcher::Mat_TransENGMove(FrameInfo info)
|
||||||
|
{
|
||||||
|
PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos);
|
||||||
|
PointXYZ ptOrig = getXYZFromBLH(originPoint.craft.stPos);
|
||||||
|
PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, ptOrig);
|
||||||
|
|
||||||
|
Mat move = Mat::zeros(3, 1, CV_64F);
|
||||||
|
|
||||||
|
move.at<double>(0, 0) = diff.Z;
|
||||||
|
move.at<double>(1, 0) = diff.X;
|
||||||
|
move.at<double>(2, 0) = diff.Y;
|
||||||
|
return move;
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat ExtrinsicStitcher::Mat_TransENG2uv(FrameInfo info)
|
||||||
|
{
|
||||||
|
//从地理坐标系转像素坐标
|
||||||
|
|
||||||
|
//[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]'
|
||||||
|
//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH]
|
||||||
|
|
||||||
|
|
||||||
|
// 深度矩阵
|
||||||
|
Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
|
||||||
|
0, 1, 0,
|
||||||
|
0, 0, info.nEvHeight
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
float yaw = info.craft.stAtt.fYaw;
|
||||||
|
Mat M_yaw = (Mat_<double>(3, 3) << cosd(yaw), -sind(yaw), 0,
|
||||||
|
sind(yaw), cosd(yaw), 0,
|
||||||
|
0, 0, 1
|
||||||
|
);
|
||||||
|
|
||||||
|
float pit = info.craft.stAtt.fPitch;
|
||||||
|
Mat M_pitch = (Mat_<double>(3, 3) << 1, 0, 0,
|
||||||
|
0, cosd(pit), -sind(pit),
|
||||||
|
0, sind(pit), cosd(pit)
|
||||||
|
);
|
||||||
|
|
||||||
|
/* 1 0 0
|
||||||
|
0 cos sin
|
||||||
|
0 -sin cos
|
||||||
|
*/
|
||||||
|
|
||||||
|
float roll = info.craft.stAtt.fRoll;
|
||||||
|
Mat M_roll = (Mat_<double>(3, 3) << cosd(roll), 0, sind(roll),
|
||||||
|
0, 1, 0,
|
||||||
|
-sind(roll), 0, cosd(roll)
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
float beta = info.servoInfo.fServoAz;
|
||||||
|
|
||||||
|
Mat M_beta = (Mat_<double>(3, 3) << cosd(beta), -sind(beta), 0,
|
||||||
|
sind(beta), cosd(beta), 0,
|
||||||
|
0, 0, 1
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
float alaph = info.servoInfo.fServoPt;
|
||||||
|
|
||||||
|
Mat M_alaph = (Mat_<double>(3, 3) << 1, 0, 0,
|
||||||
|
0, cosd(alaph), -sind(alaph),
|
||||||
|
0, sind(alaph), cosd(alaph)
|
||||||
|
|
||||||
|
);
|
||||||
|
|
||||||
|
// 内参
|
||||||
|
FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000;
|
||||||
|
|
||||||
|
Mat M_cam = (Mat_<double>(3, 3) << fd, 0, info.nWidth / 2,
|
||||||
|
0, -fd, info.nHeight / 2,
|
||||||
|
0, 0, 1
|
||||||
|
);
|
||||||
|
|
||||||
|
Mat M = M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
|
||||||
|
//cout << M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
|
||||||
|
return M;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Proj ExtrinsicStitcher::AnlayseTform(FrameInfo info)
|
||||||
|
{
|
||||||
|
Proj projection;
|
||||||
|
// 从像方->地理
|
||||||
|
projection.tf_p2g.R = Mat_TransENG2uv(info).inv();
|
||||||
|
projection.tf_p2g.T = Mat_TransENGMove(info);
|
||||||
|
|
||||||
|
// 从地理->像方
|
||||||
|
projection.tf_g2p.R = Mat_TransENG2uv(info);
|
||||||
|
projection.tf_g2p.T = -projection.tf_p2g.T;
|
||||||
|
|
||||||
|
return projection;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in new issue