|
|
#include "Arith_CoordModule.h"
|
|
|
|
|
|
//#include "Arith_SysVar.h"
|
|
|
#include <cmath>
|
|
|
#include "Arith_Utils.h"
|
|
|
|
|
|
|
|
|
|
|
|
// 从惯性系（建航坐标系）到 像坐标系
|
|
|
POINT32F getImagePosFromStablePole(Pole stablePole, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err)
|
|
|
{
|
|
|
POINT32F imgPos = { 0 };
|
|
|
|
|
|
// 从稳定性到设备系
|
|
|
Pole devicePole = getDevicePoleFromStablePole(stablePole, att, err);
|
|
|
|
|
|
// 从设备系到像方
|
|
|
imgPos = getImagePosFromDevicePole(devicePole, cam, servoInfo);
|
|
|
|
|
|
return imgPos;
|
|
|
|
|
|
}
|
|
|
|
|
|
// 从像坐标系 到 惯性系（建航坐标系）
|
|
|
Pole getStablePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err)
|
|
|
{
|
|
|
Pole StablePole = { 0 };
|
|
|
|
|
|
// 从像方到设备系
|
|
|
Pole devicePole = getDevicePoleFromImagePos(imagePos, cam, servoInfo);
|
|
|
|
|
|
// 从设备系到惯性系
|
|
|
StablePole = getStablePoleFromDevicePole(devicePole, att, err);
|
|
|
|
|
|
return StablePole;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
BBOOL bTargetInScanFov(ANGLE32F targetPos, Pole scanNueRange1, Pole scanNueRange2)
|
|
|
{
|
|
|
BBOOL AzInScan = TRUE;
|
|
|
BBOOL PtInScan = TRUE;
|
|
|
// 俯仰扫描区间有效
|
|
|
if (scanNueRange1.alpha != 0 && scanNueRange2.alpha != 0)
|
|
|
{
|
|
|
if (targetPos.fPt < scanNueRange1.alpha || targetPos.fPt > scanNueRange2.alpha)
|
|
|
{
|
|
|
PtInScan = FALSE;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
// 方位扫描区间有效
|
|
|
if (scanNueRange1.beta != 0 && scanNueRange2.beta != 0)
|
|
|
{
|
|
|
if (targetPos.fAz < scanNueRange1.beta || targetPos.fAz > scanNueRange2.beta)
|
|
|
{
|
|
|
AzInScan = FALSE;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
return (AzInScan && PtInScan);
|
|
|
|
|
|
}
|
|
|
|
|
|
Pole getDevicePoleFromStablePole(Pole stablePole, EulerRPY att, DeviceSetupError err)
|
|
|
{
|
|
|
Pole targetCarNUEPole = stablePole;
|
|
|
|
|
|
// 地理极坐标 转 地理直角坐标
|
|
|
PointXYZ targetCarNUEXYZ = getXYZFromPole(targetCarNUEPole);
|
|
|
|
|
|
// 地理直角坐标 转 平台直角坐标
|
|
|
PointXYZ targetCarXYZ = getCarXYZFromCarNUEXYZ(targetCarNUEXYZ, att.fYaw, att.fPitch, att.fRoll);
|
|
|
|
|
|
// 平台直角 转 光电基准系直角
|
|
|
PointXYZ targetEOBaseXYZ = getEOBaseXYZFromCarXYZ(targetCarXYZ, err.g_XBias, err.g_YBias, err.g_ZBias);
|
|
|
|
|
|
// 光电基准系直角 转 光电伺服直角
|
|
|
PointXYZ targetEOServoXYZ = getEOServoXYZFromEOBaseXYZ(targetEOBaseXYZ, err.g_YawBias, err.g_PitchBias, err.g_RollBias);
|
|
|
|
|
|
// 光电伺服直角 转 光电伺服极坐标
|
|
|
Pole targetEOServoPole = getPoleFromXYZ(targetEOServoXYZ);
|
|
|
|
|
|
return targetEOServoPole;
|
|
|
|
|
|
}
|
|
|
|
|
|
Pole getStablePoleFromDevicePole(Pole devicePole, EulerRPY att, DeviceSetupError err)
|
|
|
{
|
|
|
// 设备系转伺服直角坐标
|
|
|
PointXYZ targetServoXYZ = getXYZFromPole(devicePole);
|
|
|
|
|
|
// 光电伺服直角坐标 转 光电基准直角坐标
|
|
|
PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, err.g_YawBias, err.g_PitchBias, err.g_RollBias);
|
|
|
|
|
|
// 光电基准直角坐标 转 平台直角坐标
|
|
|
PointXYZ targetCarXYZ = getCarXYZFromEOBaseXYZ(targetEOBaseXYZ, err.g_XBias, err.g_YBias, err.g_ZBias);
|
|
|
|
|
|
// 平台直角坐标 转 平台地理直角坐标
|
|
|
PointXYZ targetCarNUEXYZ = getCarNUEXYZFromCarXYZ(targetCarXYZ, att.fYaw, att.fPitch, att.fRoll);
|
|
|
|
|
|
// 平台地理直角坐标 转 平台地理极坐标
|
|
|
Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ);
|
|
|
|
|
|
return targetCarNUEPole;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 像方转设备系
|
|
|
Pole getDevicePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo)
|
|
|
{
|
|
|
Pole targetDevicePole = { 0 };
|
|
|
|
|
|
if(cam.bImageRataSys)
|
|
|
{
|
|
|
double refmat[9] = { 0 };
|
|
|
PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat);
|
|
|
|
|
|
targetDevicePole = PMS_ProjectPixelToWorld((int)imagePos.x, (int)imagePos.y, refmat,
|
|
|
cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth / 2, cam.dy + cam.ImageHeight / 2);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 像方转相机坐标系
|
|
|
Pole targetCamPole = { 0 };
|
|
|
float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000));
|
|
|
SINT32 cx = cam.dx + cam.ImageWidth / 2;
|
|
|
SINT32 cy = cam.dy + cam.ImageHeight / 2;
|
|
|
targetCamPole.beta = (imagePos.x - cx) * fAglRes;
|
|
|
targetCamPole.alpha = (cy - imagePos.y) * fAglRes;
|
|
|
|
|
|
// 1:一般旋转矩阵方法
|
|
|
// 光电镜面极坐标 转 光电镜面直角坐标
|
|
|
PointXYZ targetCamXYZ = getXYZFromPole(targetCamPole);
|
|
|
|
|
|
// 光电镜面直角坐标 转 光电伺服直角坐标
|
|
|
PointXYZ targetServoXYZ = getEOServoXYZFromEOCamXYZ(targetCamXYZ, servoInfo.fPt, servoInfo.fAz);
|
|
|
|
|
|
// 2:正割补偿方法
|
|
|
// 光电镜面极坐标 转 光电伺服极坐标
|
|
|
// Pole targetEOServoPOLE;
|
|
|
// targetEOServoPOLE.alpha = alaph + targetCamPole.alpha;
|
|
|
// targetEOServoPOLE.beta = beta + targetCamPole.beta/cos(RADIAN(alaph));
|
|
|
// targetEOServoPOLE.distance = targetCamPole.distance;
|
|
|
// // 光电伺服极坐标 转 光电伺服直角坐标
|
|
|
// targetServoXYZ = getXYZFromPole(targetEOServoPOLE);
|
|
|
|
|
|
targetDevicePole = getPoleFromXYZ(targetServoXYZ);
|
|
|
}
|
|
|
|
|
|
|
|
|
return targetDevicePole;
|
|
|
}
|
|
|
|
|
|
|
|
|
// 设备系转像方坐标
|
|
|
POINT32F getImagePosFromDevicePole(Pole devicePole, CameraParam cam, ServoParam servoInfo)
|
|
|
{
|
|
|
POINT32F imgpos = { 0 };
|
|
|
|
|
|
// 常规光电成像模型
|
|
|
if(cam.bImageRataSys)
|
|
|
{
|
|
|
double refmat[9] = { 0 };
|
|
|
double refmat_inv[9] = { 0 };
|
|
|
PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat);
|
|
|
//invMat3(refmat, refmat_inv);
|
|
|
TransposeMat(refmat, 3, 3, refmat_inv);//等价
|
|
|
|
|
|
PointXYZ imgPosition =
|
|
|
PMS_ProjectWorldToPixel((float)devicePole.beta, (float)devicePole.alpha, refmat_inv,
|
|
|
cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth/2, cam.dy + cam.ImageHeight/2);
|
|
|
|
|
|
imgpos.x = (float)imgPosition.X;
|
|
|
imgpos.y = (float)imgPosition.Y;
|
|
|
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 设备极坐标转直角坐标
|
|
|
PointXYZ targetEOServoXYZ = getXYZFromPole(devicePole);
|
|
|
|
|
|
// 光电伺服直角 转 光电镜面直角坐标(相机系)
|
|
|
PointXYZ targetEOCamXYZ = getEOCamXYZFromEOServoXYZ(targetEOServoXYZ, servoInfo.fPt, servoInfo.fAz);
|
|
|
|
|
|
// 光电镜面极坐标
|
|
|
Pole targetEOCamPole = getPoleFromXYZ(targetEOCamXYZ);
|
|
|
|
|
|
float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000.0f));
|
|
|
SINT32 cx = cam.dx + cam.ImageWidth / 2;
|
|
|
SINT32 cy = cam.dy + cam.ImageHeight / 2;
|
|
|
imgpos.x = FLOAT32(targetEOCamPole.beta / fAglRes + cx);
|
|
|
imgpos.y = FLOAT32(cy - targetEOCamPole.alpha / fAglRes);
|
|
|
}
|
|
|
|
|
|
return imgpos;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void PMS_MultiRefMat(float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, double* mat)
|
|
|
{
|
|
|
// 反射镜1
|
|
|
// fa -- 方位角输出-外框
|
|
|
// fb -- 俯仰角输出-内框
|
|
|
PointXYZ N1_0;
|
|
|
N1_0.X = 1 / sqrt(2.0);
|
|
|
N1_0.Y = 1 / sqrt(2.0);
|
|
|
N1_0.Z = 0;
|
|
|
|
|
|
// n1绕z轴旋转fb（向上为正）
|
|
|
double Sz[9] = { cosd(fMainAPt),sind(fMainAPt),0,
|
|
|
-sind(fMainAPt),cosd(fMainAPt),0,
|
|
|
0,0,1 };
|
|
|
|
|
|
// 再绕y轴旋转fa（向右为正）
|
|
|
double Sy[9] = { cosd(fMainAz),0,-sind(fMainAz),
|
|
|
0,1,0,
|
|
|
sind(fMainAz),0,cosd(fMainAz) };
|
|
|
|
|
|
// 反射镜1法向量 %% 前两个法向量在XOY面里
|
|
|
//N1_F = Sy * Sz * N1_0;
|
|
|
PointXYZ N1_F = RtPoint(Sy, RtPoint(Sz, N1_0));
|
|
|
|
|
|
// 反射镜1反射矩阵
|
|
|
//R1 = eye(3) - (2 * N1_F * N1_F');
|
|
|
double R1[9] = { 0 };
|
|
|
RMirror(N1_F, R1);
|
|
|
|
|
|
|
|
|
// 反射镜23，没有动 %%注意：第3、4面镜子已经在XOZ面里了，注意空间关系
|
|
|
PointXYZ N2_0;
|
|
|
N2_0.X = -1 / sqrt(2.0);
|
|
|
N2_0.Y = -1 / sqrt(2.0);
|
|
|
N2_0.Z = 0;
|
|
|
double R2[9] = { 0 };
|
|
|
RMirror(N2_0, R2);
|
|
|
|
|
|
PointXYZ N3_0;
|
|
|
N3_0.X = 1 / sqrt(2.0);
|
|
|
N3_0.Y = 0;
|
|
|
N3_0.Z = 1 / sqrt(2.0);
|
|
|
double R3[9] = { 0 };
|
|
|
RMirror(N3_0, R3);
|
|
|
|
|
|
// 反射镜4是快反
|
|
|
// Qa -- 方位角输出-外框,向右偏转为正
|
|
|
// Qb -- 俯仰角输出-内框，抬头为正
|
|
|
PointXYZ N4_0;
|
|
|
N4_0.X = 1 / sqrt(2.0);
|
|
|
N4_0.Y = 0;
|
|
|
N4_0.Z = -1 / sqrt(2.0);
|
|
|
|
|
|
// 外框扫描轴为y轴，内框扫描轴L0为x=z 即(1,0,1)'
|
|
|
double Sy2[9] = { cosd(fFSMAz),0,-sind(fFSMAz),
|
|
|
0,1,0,
|
|
|
sind(fFSMAz),0,cosd(fFSMAz) };
|
|
|
|
|
|
// 内框旋转向量,注意左手系，会引入负号
|
|
|
PointXYZ L0;
|
|
|
L0.X = 1;
|
|
|
L0.Y = 0;
|
|
|
L0.Z = 1;
|
|
|
double SL2[9] = { 0 };
|
|
|
rotationVectorToMatrix(L0, -fFSMPt, SL2);
|
|
|
|
|
|
//快反镜法向量，经过内框和外框旋转
|
|
|
// N4_F = Sy2 * SL2 * N4_0;
|
|
|
PointXYZ N4_F = RtPoint(Sy2, RtPoint(SL2, N4_0));
|
|
|
|
|
|
// 快反的反射矩阵
|
|
|
double R4[9] = { 0 };
|
|
|
RMirror(N4_F, R4);
|
|
|
|
|
|
// 整个光路的反射模型,从像方到物方
|
|
|
double tmp[9] = { 0 };
|
|
|
MultiMat333(R2, R3, R4, tmp);
|
|
|
MultiMat33(R1, tmp, mat);
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
// 像方到物方的映射,转到方位俯仰角
|
|
|
Pole PMS_ProjectPixelToWorld(int x, int y, float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, float f_px, int cx, int cy)
|
|
|
{
|
|
|
// 计算反射矩阵
|
|
|
double mat[9] = { 0 };
|
|
|
PMS_MultiRefMat(fMainAz, fMainAPt, fFSMAz, fFSMPt, mat);
|
|
|
return PMS_ProjectPixelToWorld(x, y, mat, f_px, cx, cy);
|
|
|
}
|
|
|
|
|
|
Pole PMS_ProjectPixelToWorld(int x, int y, double* mat, float f_px, int cx, int cy)
|
|
|
{
|
|
|
Pole worldPole = { 0 };
|
|
|
// 内参信息
|
|
|
// const int nWidth = 860;
|
|
|
// const int nHeight = 715;
|
|
|
// const float f = 350;
|
|
|
// const int piexlSize = 7.5;
|
|
|
// float fpiexl = f/piexlSize * 1000; //像素焦距
|
|
|
|
|
|
// 从靶面出射的矢量
|
|
|
PointXYZ PO;
|
|
|
PO.X = -f_px;
|
|
|
PO.Y = -(y - cy);
|
|
|
PO.Z = -(x - cx);
|
|
|
|
|
|
//%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里
|
|
|
PO.Y = -PO.Y;
|
|
|
|
|
|
// 出射矢量
|
|
|
PointXYZ OQ;
|
|
|
OQ = RtPoint(mat, PO);
|
|
|
|
|
|
|
|
|
|
|
|
// 物方角度
|
|
|
double len = sqrt(PO.X * PO.X + PO.Y * PO.Y + PO.Z * PO.Z);
|
|
|
|
|
|
|
|
|
worldPole.alpha = ANGLE(asin(-OQ.Y / len));
|
|
|
worldPole.beta = ANGLE(atan2(OQ.Z, OQ.X));
|
|
|
|
|
|
return worldPole;
|
|
|
}
|
|
|
|
|
|
// PointXYZ PMS_ProjectPixelToWorldVec(int x, int y, double* mat, float f_px, int cx, int cy)
|
|
|
// {
|
|
|
// Pole worldPole = { 0 };
|
|
|
// // 内参信息
|
|
|
// // const int nWidth = 860;
|
|
|
// // const int nHeight = 715;
|
|
|
// // const float f = 350;
|
|
|
// // const int piexlSize = 7.5;
|
|
|
// // float fpiexl = f/piexlSize * 1000; //像素焦距
|
|
|
|
|
|
// // 从靶面出射的矢量
|
|
|
// PointXYZ PO;
|
|
|
// PO.X = -f_px;
|
|
|
// PO.Y = -(y - cy);
|
|
|
// PO.Z = -(x - cx);
|
|
|
|
|
|
// //%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里
|
|
|
// PO.Y = -PO.Y;
|
|
|
|
|
|
// // 出射矢量
|
|
|
// PointXYZ OQ;
|
|
|
// OQ = RtPoint(mat, PO);
|
|
|
|
|
|
// return OQ;
|
|
|
|
|
|
// }
|
|
|
//物方到像方的映射,转到像素位置
|
|
|
PointXYZ PMS_ProjectWorldToPixel(float fAz, float fPt, double* refmat, float f_px, int cx, int cy)
|
|
|
{
|
|
|
PointXYZ pxPos = { 0 };
|
|
|
|
|
|
// 物方直角系出射矢量
|
|
|
PointXYZ OQ = { 0 };
|
|
|
float Dep = 1000;//单位向量
|
|
|
OQ.Y = -Dep * sind(fPt);
|
|
|
OQ.X = Dep * cosd(fPt) * cosd(fAz);
|
|
|
OQ.Z = Dep * cosd(fPt) * sind(fAz);
|
|
|
|
|
|
|
|
|
// 投影,得到靶面出射矢量
|
|
|
PointXYZ PO = RtPoint(refmat, OQ);
|
|
|
|
|
|
PointXYZ OP = { 0 };//射向靶面
|
|
|
OP.X = -PO.X;
|
|
|
OP.Y = -PO.Y;
|
|
|
OP.Z = -PO.Z;
|
|
|
|
|
|
|
|
|
// 内参矩阵
|
|
|
pxPos.X = f_px * OP.Z / OP.X + cx;
|
|
|
pxPos.Y = -f_px * OP.Y / OP.X + cy;
|
|
|
|
|
|
|
|
|
return pxPos;
|
|
|
}
|
|
|
|
|
|
// PointXYZ PMS_ProjectWorldVecToPixel(PointXYZ worldVec, double* refmat, float f_px, int cx, int cy)
|
|
|
// {
|
|
|
// PointXYZ pxPos = { 0 };
|
|
|
// // 投影,得到靶面出射矢量
|
|
|
// PointXYZ PO = RtPoint(refmat, worldVec);
|
|
|
|
|
|
// PointXYZ OP = { 0 };//射向靶面
|
|
|
// OP.X = -PO.X;
|
|
|
// OP.Y = -PO.Y;
|
|
|
// OP.Z = -PO.Z;
|
|
|
|
|
|
|
|
|
// // 内参矩阵
|
|
|
// pxPos.X = f_px * OP.Z / OP.X + cx;
|
|
|
// pxPos.Y = -f_px * OP.Y / OP.X + cy;
|
|
|
|
|
|
|
|
|
// return pxPos;
|
|
|
// }
|
|
|
|
|
|
|
|
|
// // S726像方到物方转换
|
|
|
// Pole getPointMirrorServoPoleFromImageXY(int Col,int Row,int nWidth,int nHeight,
|
|
|
// float fServoA,float fServoB,float fLen,float pxSizeOfum)
|
|
|
// {
|
|
|
// // by wcw04046 @ 2021/12/04
|
|
|
// // 注意:在通常成像系统中，伺服读数基本等价于伺服系下角度（需要考虑正割补偿）
|
|
|
// // 在指向镜成像系统中，伺服读数与伺服系下角度完全不等价 fServoAz fServoPt 与目标在伺服系位置非线性
|
|
|
// // 伺服坐标系角度
|
|
|
// Pole ServoPole;
|
|
|
|
|
|
// // 像素焦距
|
|
|
// float f_0 = fLen/pxSizeOfum * 1000;
|
|
|
|
|
|
// // 等效像方入射矢量
|
|
|
// PointXYZ CamVec;
|
|
|
// CamVec.X = f_0;
|
|
|
// CamVec.Y = -nHeight/2 + Row;
|
|
|
// CamVec.Z = (-nWidth/2 + Col);
|
|
|
|
|
|
// // 反射镜能将正前方(x轴)的目标反射到成像系统光轴(y轴)上时，角编码器为零位
|
|
|
|
|
|
// // 平面镜1零位的法向量
|
|
|
// PointXYZ N_01;
|
|
|
// N_01.X = 1/sqrt(2.0);
|
|
|
// N_01.Y = 1/sqrt(2.0);
|
|
|
// N_01.Z = 0;
|
|
|
|
|
|
// double Sx[9] = {1,0,0,
|
|
|
// 0,cos(RADIAN(fServoA)),-sin(RADIAN(fServoA)),
|
|
|
// 0,sin(RADIAN(fServoA)),cos(RADIAN(fServoA))};
|
|
|
|
|
|
// double Sy[9] = {cos(RADIAN(fServoB)),0,-sin(RADIAN(fServoB)),
|
|
|
// 0,1,0,
|
|
|
// sin(RADIAN(fServoB)),0,cos(RADIAN(fServoB))};
|
|
|
|
|
|
// PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
|
|
|
// // 镜面反射矩阵
|
|
|
// // M = eye(3)-(2*N*N');
|
|
|
// double M1[9] = {0};
|
|
|
// RMirror(N_1,M1);
|
|
|
|
|
|
|
|
|
// // 平面镜2零位的法向量
|
|
|
// PointXYZ N_02;
|
|
|
// N_02.X = -1/sqrt(2.0);
|
|
|
// N_02.Y = -1/sqrt(2.0);
|
|
|
// N_02.Z = 0;
|
|
|
// PointXYZ N_2 = RtPoint(Sx,N_02);
|
|
|
// double M2[9] = {0};
|
|
|
// RMirror(N_2,M2);
|
|
|
|
|
|
// // 平面镜3零位的法向量
|
|
|
// PointXYZ N_03;
|
|
|
// N_03.X = 1/sqrt(2.0);
|
|
|
// N_03.Y = -1/sqrt(2.0);
|
|
|
// N_03.Z = 0;
|
|
|
// PointXYZ N_3 = RtPoint(Sx,N_03);
|
|
|
// double M3[9] = {0};
|
|
|
// RMirror(N_3,M3);
|
|
|
|
|
|
// // 平面镜4零位的法向量
|
|
|
// PointXYZ N_04;
|
|
|
// N_04.X = -1/sqrt(2.0);
|
|
|
// N_04.Y = 1/sqrt(2.0);
|
|
|
// N_04.Z = 0;
|
|
|
// PointXYZ N_4 = RtPoint(Sx,N_04);
|
|
|
// double M4[9] = {0};
|
|
|
// RMirror(N_4,M4);
|
|
|
|
|
|
// // 等效物方出射矢量
|
|
|
// PointXYZ worldPosVec = RtPoint(M1,RtPoint(M2,RtPoint(M3,RtPoint(M4,CamVec))));
|
|
|
// PointXYZ worldPosVec_1 = NormPoint(worldPosVec);
|
|
|
|
|
|
// ServoPole.alpha = ANGLE(asin(worldPosVec_1.Y));
|
|
|
// ServoPole.beta = ANGLE(atan2(worldPosVec_1.Z,worldPosVec_1.X));
|
|
|
// ServoPole.distance = 1000;
|
|
|
|
|
|
// return ServoPole;
|
|
|
|
|
|
// }
|
|
|
|
|
|
// // S726物方到像方转换
|
|
|
// void getPointMirrorImageXYFromServoPole(int* Col,int* Row,int nWidth,int nHeight,float WAlaph,float WBeta,
|
|
|
// float fServoA,float fServoB,float fLen,float pxSizeOfum)
|
|
|
// {
|
|
|
// //物方
|
|
|
// int dep = 1000;
|
|
|
// PointXYZ WorldVec;
|
|
|
// WorldVec.Y = -dep * sin(RADIAN(WAlaph));
|
|
|
// WorldVec.X = -dep * cos(RADIAN(WAlaph)) * cos(RADIAN(WBeta));
|
|
|
// WorldVec.Z = -dep * cos(RADIAN(WAlaph)) * sin(RADIAN(WBeta));
|
|
|
|
|
|
// // 内参矩阵
|
|
|
// float f_0 = fLen/pxSizeOfum * 1000;
|
|
|
|
|
|
// double K[9] =
|
|
|
// {f_0,0,nWidth/2,
|
|
|
// 0,f_0,nHeight/2,
|
|
|
// 0,0,1};
|
|
|
|
|
|
// // 平面镜1零位的法向量
|
|
|
// PointXYZ N_01;
|
|
|
// N_01.X = 1/sqrt(2.0);
|
|
|
// N_01.Y = 1/sqrt(2.0);
|
|
|
// N_01.Z = 0;
|
|
|
|
|
|
// double Sx[9] = {1,0,0,
|
|
|
// 0,cos(RADIAN(fServoB)),-sin(RADIAN(fServoB)),
|
|
|
// 0,sin(RADIAN(fServoB)),cos(RADIAN(fServoB))};
|
|
|
|
|
|
// double Sy[9] = {cos(RADIAN(fServoA)),0,-sin(RADIAN(fServoA)),
|
|
|
// 0,1,0,
|
|
|
// sin(RADIAN(fServoA)),0,cos(RADIAN(fServoA))};
|
|
|
|
|
|
// PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
|
|
|
// // 镜面反射矩阵
|
|
|
// // M = eye(3)-(2*N*N');
|
|
|
// double M1[9] = {0};
|
|
|
// RMirror(N_1,M1);
|
|
|
|
|
|
|
|
|
// // 平面镜2零位的法向量
|
|
|
// PointXYZ N_02;
|
|
|
// N_02.X = -1/sqrt(2.0);
|
|
|
// N_02.Y = -1/sqrt(2.0);
|
|
|
// N_02.Z = 0;
|
|
|
// PointXYZ N_2 = RtPoint(Sx,N_02);
|
|
|
// double M2[9] = {0};
|
|
|
// RMirror(N_2,M2);
|
|
|
|
|
|
// // 平面镜3零位的法向量
|
|
|
// PointXYZ N_03;
|
|
|
// N_03.X = 1/sqrt(2.0);
|
|
|
// N_03.Y = -1/sqrt(2.0);
|
|
|
// N_03.Z = 0;
|
|
|
// PointXYZ N_3 = RtPoint(Sx,N_03);
|
|
|
// double M3[9] = {0};
|
|
|
// RMirror(N_3,M3);
|
|
|
|
|
|
// // 平面镜4零位的法向量
|
|
|
// PointXYZ N_04;
|
|
|
// N_04.X = -1/sqrt(2.0);
|
|
|
// N_04.Y = 1/sqrt(2.0);
|
|
|
// N_04.Z = 0;
|
|
|
// PointXYZ N_4 = RtPoint(Sx,N_04);
|
|
|
// double M4[9] = {0};
|
|
|
// RMirror(N_4,M4);
|
|
|
|
|
|
// PointXYZ camPosVec = RtPoint(M4,RtPoint(M3,RtPoint(M2,RtPoint(M1,WorldVec))));
|
|
|
|
|
|
// double m_adjust[9] = {0,0,1,0,1,0,1,0,0};
|
|
|
|
|
|
// PointXYZ pixPos = RtPoint(K,RtPoint(m_adjust,camPosVec));
|
|
|
|
|
|
// *Col = pixPos.X / float(pixPos.Z);
|
|
|
// *Row = pixPos.Y / float(pixPos.Z);
|
|
|
|
|
|
|
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
PointXYZ getEOServoXYZFromEOCamXYZ(PointXYZ targetCamXYZ,double alaph,double beta)
|
|
|
{
|
|
|
// 先绕Z轴俯仰角旋转-alaph，后绕Y轴方位角旋转-beta
|
|
|
double oppRz[9];
|
|
|
double oppRy[9];
|
|
|
double oppRyz[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Rz(-alaph, oppRz);
|
|
|
Ry(-beta, oppRy);
|
|
|
MultiMat33(oppRy,oppRz, oppRyz);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRyz, targetCamXYZ);
|
|
|
}
|
|
|
|
|
|
PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, double eYaw, double ePitch, double eRoll)
|
|
|
{
|
|
|
// 先绕x轴横滚旋转 -eRoll，再绕z轴俯仰旋转 -ePitch，再绕y轴方位旋转-eYaw
|
|
|
|
|
|
double oppRx[9];
|
|
|
double oppRy[9];
|
|
|
double oppRz[9];
|
|
|
double oppRyzx[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Rx(-eRoll, oppRx);
|
|
|
Rz(-ePitch, oppRz);
|
|
|
Ry(-eYaw, oppRy);
|
|
|
|
|
|
MultiMat333(oppRy,oppRz, oppRx, oppRyzx);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRyzx, targetServoXYZ);
|
|
|
|
|
|
}
|
|
|
|
|
|
PointXYZ getCarXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double deltaX, double deltaY, double deltaZ)
|
|
|
{
|
|
|
//平台坐标系下目标坐标
|
|
|
PointXYZ targetCarXYZ;
|
|
|
|
|
|
|
|
|
targetCarXYZ.X = targetEOBaseXYZ.X + deltaX;
|
|
|
targetCarXYZ.Y = targetEOBaseXYZ.Y + deltaY;
|
|
|
targetCarXYZ.Z = targetEOBaseXYZ.Z + deltaZ;
|
|
|
|
|
|
return targetCarXYZ;
|
|
|
}
|
|
|
|
|
|
PointXYZ getCarNUEXYZFromCarXYZ(PointXYZ targetCarXYZ, double mYaw, double mPitch, double mRoll)
|
|
|
{
|
|
|
// 先绕x轴横滚旋转 -mRoll，再绕z轴俯仰旋转 -mPitch，再绕y轴方位旋转-mYaw
|
|
|
|
|
|
double oppRx[9];
|
|
|
double oppRy[9];
|
|
|
double oppRz[9];
|
|
|
double oppRyzx[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Rx(-mRoll, oppRx);
|
|
|
Rz(-mPitch, oppRz);
|
|
|
Ry(-mYaw, oppRy);
|
|
|
|
|
|
MultiMat333(oppRy,oppRz, oppRx, oppRyzx);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRyzx, targetCarXYZ);
|
|
|
|
|
|
}
|
|
|
|
|
|
PointXYZ getCarXYZFromCarNUEXYZ(PointXYZ targetCarNUEXYZ, double mYaw, double mPitch, double mRoll)
|
|
|
{
|
|
|
// 先绕y轴方位旋转mYaw -> 再绕z轴俯仰旋转 mPitch -> 绕x轴横滚旋转 mRoll
|
|
|
double oppRx[9];
|
|
|
double oppRy[9];
|
|
|
double oppRz[9];
|
|
|
double oppRxzy[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Ry(mYaw, oppRy);
|
|
|
Rz(mPitch, oppRz);
|
|
|
Rx(mRoll, oppRx);
|
|
|
|
|
|
MultiMat333(oppRx, oppRz, oppRy, oppRxzy);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRxzy, targetCarNUEXYZ);
|
|
|
|
|
|
}
|
|
|
|
|
|
PointXYZ getEOBaseXYZFromCarXYZ(PointXYZ targetCarXYZ, double deltaX, double deltaY, double deltaZ)
|
|
|
{
|
|
|
//平台坐标系下目标坐标
|
|
|
PointXYZ targetEOBaseXYZ;
|
|
|
|
|
|
targetEOBaseXYZ.X = targetCarXYZ.X - deltaX;
|
|
|
targetEOBaseXYZ.Y = targetCarXYZ.Y - deltaY;
|
|
|
targetEOBaseXYZ.Z = targetCarXYZ.Z - deltaZ;
|
|
|
|
|
|
return targetEOBaseXYZ;
|
|
|
}
|
|
|
|
|
|
PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double eYaw, double ePitch, double eRoll)
|
|
|
{
|
|
|
// 先绕y轴方位旋转eYaw,再绕z轴俯仰旋转 ePitch,再绕x轴横滚旋转 eRoll
|
|
|
double oppRx[9];
|
|
|
double oppRy[9];
|
|
|
double oppRz[9];
|
|
|
double oppRxzy[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Rx(eRoll, oppRx);
|
|
|
Rz(ePitch, oppRz);
|
|
|
Ry(eYaw, oppRy);
|
|
|
|
|
|
MultiMat333(oppRx, oppRz, oppRy, oppRxzy);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRxzy, targetEOBaseXYZ);
|
|
|
|
|
|
}
|
|
|
|
|
|
PointXYZ getEOCamXYZFromEOServoXYZ(PointXYZ targetEOServoXYZ, double alaph, double beta)
|
|
|
{
|
|
|
// 先绕Y轴方位角旋转beta,后绕Z轴俯仰角旋转alaph
|
|
|
double oppRz[9];
|
|
|
double oppRy[9];
|
|
|
double oppRzy[9];
|
|
|
|
|
|
// 计算旋转矩阵
|
|
|
Rz(alaph, oppRz);
|
|
|
Ry(beta, oppRy);
|
|
|
MultiMat33(oppRz, oppRy, oppRzy);
|
|
|
|
|
|
// 坐标转换
|
|
|
return RtPoint(oppRzy, targetEOServoXYZ);
|
|
|
|
|
|
}
|
|
|
|
|
|
Pole getPoleFromXYZ(PointXYZ targetXYZ)
|
|
|
{
|
|
|
double x = targetXYZ.X;
|
|
|
double y = targetXYZ.Y;
|
|
|
double z = targetXYZ.Z;
|
|
|
|
|
|
Pole result;
|
|
|
result.distance = sqrt(x*x + y*y + z*z);
|
|
|
result.alpha = ANGLE(asin(y / result.distance));
|
|
|
|
|
|
//if (x < 0)
|
|
|
//{
|
|
|
// result.alpha = -180-result.alpha;
|
|
|
//}
|
|
|
|
|
|
|
|
|
result.beta = (ANGLE(atan2(z , x)));
|
|
|
// if (x < 0 && z >= 0) // 90 - 180
|
|
|
// {
|
|
|
// result.beta = 180 - result.beta;
|
|
|
// }
|
|
|
// else if (x < 0 && z < 0) // 180 - 270
|
|
|
// {
|
|
|
// result.beta = result.beta + 180;
|
|
|
// }
|
|
|
// else if (x >= 0 && z < 0) // 270 - 360
|
|
|
// {
|
|
|
// result.beta = 360 - result.beta;
|
|
|
// }
|
|
|
|
|
|
return result;
|
|
|
|
|
|
}
|
|
|
|
|
|
PointXYZ getXYZFromPole(Pole targetPole)
|
|
|
{
|
|
|
// 距离未知时，异原点坐标系之间不能转换
|
|
|
// 但由于同车坐标系的原点差一般远小于目标距离，故距离较大时误差较小，误差量级可以用线偏差/距离获得的弧度进行估计
|
|
|
// 如线偏差1米，目标3000米，则转换误差为(1/1000-1/3000)约为0.04°
|
|
|
|
|
|
// 吊舱无源定位允许最低高度10米
|
|
|
if (targetPole.distance < 10)
|
|
|
{
|
|
|
targetPole.distance = 10000;
|
|
|
}
|
|
|
|
|
|
PointXYZ result;
|
|
|
double aa, bb, cc;
|
|
|
//double ac = sqrt(targetPole.distance*targetPole.distance-bb*bb);
|
|
|
bb = sin(RADIAN(targetPole.alpha)) * targetPole.distance;
|
|
|
double ac = sqrt(targetPole.distance*targetPole.distance - bb*bb);
|
|
|
aa = cos(RADIAN(targetPole.beta)) * ac;
|
|
|
cc = sin(RADIAN(targetPole.beta)) * ac;
|
|
|
|
|
|
result.X = aa;
|
|
|
result.Y = bb;
|
|
|
result.Z = cc;
|
|
|
|
|
|
return result;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
//大地坐标转地球空间直角坐标
|
|
|
PointXYZ getXYZFromBLH(PointBLH BLH)
|
|
|
{
|
|
|
double B = BLH.B;
|
|
|
double L = BLH.L;
|
|
|
double H = BLH.H;
|
|
|
|
|
|
double W = sqrt(1 - ee * pow(sin(RADIAN(B)), 2));
|
|
|
double N = a / W; // 椭球的卯酉圈曲率
|
|
|
|
|
|
// 将地理坐标变换到空间直角坐标
|
|
|
double X = (N + H) * cos(RADIAN(B)) * cos(RADIAN(L));
|
|
|
double Y = (N + H) * cos(RADIAN(B)) * sin(RADIAN(L));
|
|
|
double Z = (N * (1 - ee) + H) * sin(RADIAN(B));
|
|
|
|
|
|
PointXYZ pointXYZ;
|
|
|
pointXYZ.X = X;
|
|
|
pointXYZ.Y = Y;
|
|
|
pointXYZ.Z = Z;
|
|
|
|
|
|
return pointXYZ;
|
|
|
}
|
|
|
|
|
|
//地球空间直角坐标-->大地坐标
|
|
|
PointBLH getBLHFromXYZ(PointXYZ pointXYZ)
|
|
|
{
|
|
|
double X = pointXYZ.X;
|
|
|
double Y = pointXYZ.Y;
|
|
|
double Z = pointXYZ.Z;
|
|
|
|
|
|
double U = atan(Z * a / (sqrt(X * X + Y * Y) * b));
|
|
|
double B0 = atan((Z + b * epep * pow(sin(U), 3)) / (sqrt(X*X + Y*Y) - a*ee*pow(cos(U), 3)));
|
|
|
double N = a / sqrt(1 - ee * pow(sin(B0), 2));
|
|
|
//double L = atan(Y /X);
|
|
|
double L = atan2(Y, X); // 修改为全球范围计算 by wcw04046 @ 2021/02/18
|
|
|
|
|
|
double H = sqrt(X*X + Y*Y + pow(Z + N*ee*sin(B0), 2)) - N;
|
|
|
double B = atan(Z / sqrt(X*X + Y*Y) / (1 - ee*N / (N + H)));
|
|
|
|
|
|
PointBLH pointBLH;
|
|
|
pointBLH.B = ANGLE(B);
|
|
|
pointBLH.H = H;
|
|
|
// // 此处仅考虑东经
|
|
|
// pointBLH.L = ANGLE(L) > 0 ? ANGLE(L) : ANGLE(L) + 180;
|
|
|
|
|
|
// 修改为全球范围计算 by wcw04046 @ 2021/02/18
|
|
|
pointBLH.L = ANGLE(L);
|
|
|
|
|
|
return pointBLH;
|
|
|
}
|
|
|
|
|
|
//*********************************************************************************************
|
|
|
// 由NUE直角坐标变换到CGCS直角坐标
|
|
|
// targetPointXYZ：NUE下的目标坐标
|
|
|
// selfPointXYZ：测站在CGCS下的直角坐标
|
|
|
// 返回CGCS下的目标坐标
|
|
|
//*********************************************************************************************
|
|
|
PointXYZ getCGCSXYZFromNUEXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
|
|
|
{
|
|
|
// 将测站所在位置的CGCS直角坐标变换到地理坐标
|
|
|
PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);
|
|
|
|
|
|
/* 由NUE直角坐标变换到CGCS直角坐标*/
|
|
|
double A[3];
|
|
|
double G[3];
|
|
|
double OppRx[9];
|
|
|
double OppRy[9];
|
|
|
double OppRz[9];
|
|
|
double OppRzx[9];
|
|
|
double OppRzxy[9];
|
|
|
|
|
|
// 1、计算旋转矩阵
|
|
|
Rx(-selfPointBLH.B, OppRx);
|
|
|
Ry(-90, OppRy);
|
|
|
Rz(90 - selfPointBLH.L, OppRz);
|
|
|
|
|
|
MultiMat(OppRz, OppRx, OppRzx, 3, 3, 3);
|
|
|
MultiMat(OppRzx, OppRy, OppRzxy, 3, 3, 3);
|
|
|
|
|
|
// 2、将NUE直角坐标变换到CGCS直角坐标
|
|
|
A[0] = targetPointXYZ.X;
|
|
|
A[1] = targetPointXYZ.Y;
|
|
|
A[2] = targetPointXYZ.Z;
|
|
|
MultiMat(OppRzxy, A, G, 3, 3, 1);
|
|
|
|
|
|
PointXYZ result;
|
|
|
result.X = G[0] + selfPointXYZ.X;
|
|
|
result.Y = G[1] + selfPointXYZ.Y;
|
|
|
result.Z = G[2] + selfPointXYZ.Z;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
//*********************************************************************************************
|
|
|
// 由CGCS直角坐标变换到NUE直角坐标
|
|
|
// targetPointXYZ：CGCS下的目标坐标
|
|
|
// selfPointXYZ：测站在CGCS下的直角坐标
|
|
|
// 返回NUE下的目标坐标
|
|
|
//*********************************************************************************************
|
|
|
PointXYZ getNUEXYZFromCGCSXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
|
|
|
{
|
|
|
// 将测站所在位置的CGCS直角坐标变换到地理坐标
|
|
|
PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);
|
|
|
|
|
|
// 计算中间值
|
|
|
double dx = targetPointXYZ.X - selfPointXYZ.X;
|
|
|
double dy = targetPointXYZ.Y - selfPointXYZ.Y;
|
|
|
double dz = targetPointXYZ.Z - selfPointXYZ.Z;
|
|
|
|
|
|
/* 由CGCS直角坐标变换到NUE直角坐标 */
|
|
|
double A[3];
|
|
|
double G[3];
|
|
|
double OppRx[9];
|
|
|
double OppRy[9];
|
|
|
double OppRz[9];
|
|
|
double OppRyx[9];
|
|
|
double OppRyxz[9];
|
|
|
|
|
|
// 1、计算旋转矩阵
|
|
|
Rx(selfPointBLH.B, OppRx);
|
|
|
Ry(90, OppRy);
|
|
|
Rz(-90 + selfPointBLH.L, OppRz);
|
|
|
|
|
|
MultiMat(OppRy, OppRx, OppRyx, 3, 3, 3);
|
|
|
MultiMat(OppRyx, OppRz, OppRyxz, 3, 3, 3);
|
|
|
|
|
|
// 2、将CGCS直角坐标变换到NUE直角坐标
|
|
|
A[0] = dx;
|
|
|
A[1] = dy;
|
|
|
A[2] = dz;
|
|
|
MultiMat(OppRyxz, A, G, 3, 3, 1);
|
|
|
|
|
|
PointXYZ result;
|
|
|
result.X = G[0];
|
|
|
result.Y = G[1];
|
|
|
result.Z = G[2];
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
|
