// Constructor. CameraProperties remain constant during the mission
public CAMFrame(CAMProperties camera)
{
zeroCamDistance = RTDPSettings.geolocationSettings.zeroCamDistance;
zeroCamPanRoll = RTDPSettings.geolocationSettings.zeroCamPanRoll;
camPosition = camera.camPosition;
scaleFactor = camera.pixelPitch;
offsetX = (camera.imageHeight - 1) * scaleFactor / 2;
offsetY = (camera.imageWidth - 1) * scaleFactor / 2;
focalLength = camera.focalLength;
camAngles = camera.camAngles;
cosPitch = Math.Cos(camAngles.Pitch);
sinPitch = Math.Sin(camAngles.Pitch);
if (zeroCamPanRoll)
{
cosYaw = cosRoll = 1;
sinYaw = sinRoll = 0;
}
else
{
cosYaw = Math.Cos(camAngles.Yaw);
sinYaw = Math.Sin(camAngles.Yaw);
cosRoll = Math.Cos(camAngles.Roll);
sinRoll = Math.Sin(camAngles.Roll);
}
R_BODY_CAM = null;
R_CAM_BODY = null;
}
// Input parameters are the vi coordinates corresponding to the IR top-left (x1, y1),
// top-right (x2, y2), bottom-left (x3, y3) and bottom-right (x4, y4) corners.
//
// We have two systems with four equations and three incognites:
// x1vi = a11 * x1ir + a12 * y1ir + a13
// x2vi = a11 * x2ir + a12 * y2ir + a13
// x3vi = a11 * x3ir + a12 * y3ir + a13
// x4vi = a11 * x4ir + a12 * y4ir + a13
//
// y1vi = a21 * x1ir + a22 * y1ir + a23
// y2vi = a21 * x2ir + a22 * y2ir + a23
// y3vi = a21 * x3ir + a22 * y3ir + a23
// y4vi = a21 * x4ir + a22 * y4ir + a23
//
// Given a system AX=B
// The least square solution is given by: (At·A)·X = (At·B)
//
// For the first system
// [x1vi] = [x1ir y1ir 1]
// [x2vi] = [x2ir y2ir 1] [a11]
// [x3vi] = [x3ir y3ir 1]·[a12]
// [x4vi] = [x4ir y4ir 1] [a13]
//
// The least square solution is computed by solving the system:
// [x1vi] [x1ir y1ir 1]
// [x1ir x2ir x3ir x4ir] [x2vi] [x1ir x2ir x3ir x4ir] [x2ir y2ir 1] [a11]
// [y1ir y2ir y3ir y4ir]·[x3vi] = [y1ir y2ir y3ir y4ir]·[x3ir y3ir 1]·[a12]
// [ 1 1 1 1] [x4vi] [ 1 1 1 1] [x4ir y4ir 1] [a13]
private void ComputeAffineMatrix(int x1vi, int y1vi, int x2vi, int y2vi, int x3vi, int y3vi, int x4vi, int y4vi)
{
int x1ir = 0;
int y1ir = 0;
int x2ir = N_ir - 1;
int y2ir = 0;
int x3ir = 0;
int y3ir = M_ir - 1;
int x4ir = N_ir - 1;
int y4ir = M_ir - 1;
Vector3 Bx = new Vector3(x1ir * x1vi + x2ir * x2vi + x3ir * x3vi + x4ir * x4vi,
y1ir * x1vi + y2ir * x2vi + y3ir * x3vi + y4ir * x4vi,
x1vi + x2vi + x3vi + x4vi);
Vector3 By = new Vector3(x1ir * y1vi + x2ir * y2vi + x3ir * y3vi + x4ir * y4vi,
y1ir * y1vi + y2ir * y2vi + y3ir * y3vi + y4ir * y4vi,
y1vi + y2vi + y3vi + y4vi);
Matrix3 A = new Matrix3(x1ir * x1ir + x2ir * x2ir + x3ir * x3ir + x4ir * x4ir,
x1ir * y1ir + x2ir * y2ir + x3ir * y3ir + x4ir * y4ir,
x1ir + x2ir + x3ir + x4ir,
y1ir * x1ir + y2ir * x2ir + y3ir * x3ir + y4ir * x4ir,
y1ir * y1ir + y2ir * y2ir + y3ir * y3ir + y4ir * y4ir,
y1ir + y2ir + y3ir + y4ir,
x1ir + x2ir + x3ir + x4ir,
y1ir + y2ir + y3ir + y4ir,
1 + 1 + 1 + 1);
double det = A.Determinant();
if (det == 0)
{
// the system has not solution
AffineMatrixIrVi = AffineMatrixViIr = null;
return;
}
double a11 = (new Matrix3(Bx.x, A.m01, A.m02, Bx.y, A.m11, A.m12, Bx.z, A.m21, A.m22)).Determinant() / det;
double a12 = (new Matrix3(A.m00, Bx.x, A.m02, A.m10, Bx.y, A.m12, A.m20, Bx.z, A.m22)).Determinant() / det;
double a13 = (new Matrix3(A.m00, A.m01, Bx.x, A.m10, A.m11, Bx.y, A.m20, A.m21, Bx.z)).Determinant() / det;
double a21 = (new Matrix3(By.x, A.m01, A.m02, By.y, A.m11, A.m12, By.z, A.m21, A.m22)).Determinant() / det;
double a22 = (new Matrix3(A.m00, By.x, A.m02, A.m10, By.y, A.m12, A.m20, By.z, A.m22)).Determinant() / det;
double a23 = (new Matrix3(A.m00, A.m01, By.x, A.m10, A.m11, By.y, A.m20, A.m21, By.z)).Determinant() / det;
AffineMatrixIrVi = new Matrix3(a11, a12, a13, a21, a22, a23, 0, 0, 1);
AffineMatrixViIr = AffineMatrixIrVi.Inverse();
}
public void Init(Telemetry tel)
{
// Do not move N computation below, N is used by LLAtoECEF method!
gpsLLA = tel.uavPosition;
gpsLLA.Press_Altitude = gpsLLA.Altitude - localGeoidHeight;
N = a / Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(gpsLLA.Latitude), 2));
cosLat = Math.Cos(gpsLLA.Latitude);
sinLat = Math.Sin(gpsLLA.Latitude);
cosLon = Math.Cos(gpsLLA.Longitude);
sinLon = Math.Sin(gpsLLA.Longitude);
gpsECEF = LLAtoECEF(gpsLLA);
uavAngles = tel.uavAngles;
cosYaw = Math.Cos(uavAngles.Yaw);
sinYaw = Math.Sin(uavAngles.Yaw);
if (zeroUavPitchRoll)
{
cosPitch = cosRoll = 1;
sinPitch = sinRoll = 0;
}
else
{
cosPitch = Math.Cos(uavAngles.Pitch);
sinPitch = Math.Sin(uavAngles.Pitch);
cosRoll = Math.Cos(uavAngles.Roll);
sinRoll = Math.Sin(uavAngles.Roll);
}
R_ECEF_LNED = null;
R_LNED_ECEF = null;
R_LNED_BODY = null;
R_BODY_LNED = null;
}
// BODY to LNED
public Vector3 BODYtoLNED(Vector3 BODY)
{
if (zeroUavPitchRoll && zeroGpsDistance)
{
return new Vector3 (cosYaw * BODY.x - sinYaw * BODY.y,
sinYaw * BODY.x + cosYaw * BODY.y,
BODY.z);
}
else if (zeroUavPitchRoll)
{
return new Vector3 (cosYaw * (BODY.x - gpsBODY.x) - sinYaw * (BODY.y - gpsBODY.y),
sinYaw * (BODY.x - gpsBODY.x) + cosYaw * (BODY.y - gpsBODY.y),
BODY.z - gpsBODY.z);
}
else
{
if (R_BODY_LNED == null)
{
R_BODY_LNED = new Matrix3( cosYaw * cosPitch,
-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll,
sinYaw * sinRoll + cosYaw * sinPitch * cosRoll,
sinYaw * cosPitch,
cosYaw * cosRoll + sinYaw * sinPitch * sinRoll,
-cosYaw * sinRoll + sinYaw * sinPitch * cosRoll,
-sinPitch,
cosPitch * sinRoll,
cosPitch * cosRoll);
}
if (zeroGpsDistance)
return (R_BODY_LNED * BODY);
else
return (R_BODY_LNED * (BODY - gpsBODY));
}
}
// LNED to BODY
public Vector3 LNEDtoBODY(Vector3 LNED)
{
if (zeroUavPitchRoll && zeroGpsDistance)
{
return new Vector3 (cosYaw * LNED.x + sinYaw * LNED.y,
-sinYaw * LNED.x + cosYaw * LNED.y,
LNED.z);
}
else if (zeroUavPitchRoll)
{
return new Vector3 (cosYaw * LNED.x + sinYaw * LNED.y + gpsBODY.x,
-sinYaw * LNED.x + cosYaw * LNED.y + gpsBODY.y,
LNED.z + gpsBODY.z);
}
else
{
if (R_LNED_BODY == null)
{
R_LNED_BODY = new Matrix3(cosPitch * cosYaw,
cosPitch * sinYaw,
-sinPitch,
sinRoll * sinPitch * cosYaw - cosRoll * sinYaw,
sinRoll * sinPitch * sinYaw + cosRoll * cosYaw,
sinRoll * cosPitch,
cosRoll * sinPitch * cosYaw + sinRoll * sinYaw,
cosRoll * sinPitch * sinYaw - sinRoll * cosYaw,
cosRoll * cosPitch);
}
if (zeroGpsDistance)
return (R_LNED_BODY * LNED);
else
return (R_LNED_BODY * LNED + gpsBODY);
}
}
// LNED to ECEF
public Vector3 LNEDtoECEF(Vector3 LNED)
{
if (R_LNED_ECEF == null)
{
R_LNED_ECEF = new Matrix3(-sinLat * cosLon, sinLat * sinLon, -cosLat,
sinLon, cosLon, 0,
cosLat * cosLon, -cosLat * sinLon, -sinLat);
}
return (R_LNED_ECEF * LNED + gpsECEF);
}
// ECEF to LNED
public Vector3 ECEFtoLNED(Vector3 ECEF)
{
if (R_ECEF_LNED == null)
{
R_ECEF_LNED = new Matrix3(-sinLat * cosLon, -sinLat * sinLon, cosLat,
-sinLon, cosLon, 0,
-cosLat * cosLon, -cosLat * sinLon, -sinLat);
}
return (R_ECEF_LNED * (ECEF - gpsECEF));
}
// Body to Camera system transformation
public Vector3 BODYtoCAM(Vector3 BODY)
{
if (zeroCamPanRoll && zeroCamDistance)
{
return new Vector3 (cosPitch * BODY.x - sinPitch * BODY.z,
BODY.y,
sinPitch * BODY.x + cosPitch * BODY.z);
}
else if (zeroCamPanRoll)
{
return new Vector3 (cosPitch * (BODY.x - camPosition.x) - sinPitch * (BODY.z - camPosition.z),
BODY.y - camPosition.y,
sinPitch * (BODY.x - camPosition.x) + cosPitch * (BODY.z - camPosition.z));
}
else
{
if (R_BODY_CAM == null)
{
R_BODY_CAM = new Matrix3(cosPitch * cosYaw,
cosPitch * sinYaw,
-sinPitch,
sinRoll * sinPitch * cosYaw - cosRoll * sinYaw,
sinRoll * sinPitch * sinYaw + cosRoll * cosYaw,
sinRoll * cosPitch,
cosRoll * sinPitch * cosYaw + sinRoll * sinYaw,
cosRoll * sinPitch * sinYaw - sinRoll * cosYaw,
cosRoll * cosPitch);
}
if (zeroCamDistance)
return (R_BODY_CAM * BODY);
else
return (R_BODY_CAM * (BODY - camPosition));
}
}
// Camera to Body system transformation
public Vector3 CAMtoBODY(Vector3 CAM)
{
if (zeroCamPanRoll && zeroCamDistance)
{
return new Vector3 (cosPitch * CAM.x + sinPitch * CAM.z,
CAM.y,
-sinPitch * CAM.x + cosPitch * CAM.z);
}
else if (zeroCamPanRoll)
{
return new Vector3 (cosPitch * CAM.x + sinPitch * CAM.z + camPosition.x,
CAM.y + camPosition.y,
-sinPitch * CAM.x + cosPitch * CAM.z + camPosition.z);
}
else
{
if (R_CAM_BODY == null)
{
R_CAM_BODY = new Matrix3(cosYaw * cosPitch,
-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll,
sinYaw * sinRoll + cosYaw * sinPitch * cosRoll,
sinYaw * cosPitch,
cosYaw * cosRoll + sinYaw * sinPitch * sinRoll,
-cosYaw * sinRoll + sinYaw * sinPitch * cosRoll,
-sinPitch,
cosPitch * sinRoll,
cosPitch * cosRoll);
}
if (zeroCamDistance)
return (R_CAM_BODY * CAM);
else
return (R_CAM_BODY * CAM + camPosition);
}
}
public Matrix3(Matrix3 m)
{
this.m00 = m.m00; this.m01 = m.m01; this.m02 = m.m02;
this.m10 = m.m10; this.m11 = m.m11; this.m12 = m.m12;
this.m20 = m.m20; this.m21 = m.m21; this.m22 = m.m22;
}
