Esempio n. 1
0
        // 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;
        }
Esempio n. 2
0
        // 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();
        }
Esempio n. 3
0
        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;
        }
Esempio n. 4
0
 // 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));
     }
 }
Esempio n. 5
0
 // 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);
     }
 } 
Esempio n. 6
0
 // 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);
 }
Esempio n. 7
0
        // 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));
        }
Esempio n. 8
0
 // 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));
     }
 }
Esempio n. 9
0
 // 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);
     }
 }
Esempio n. 10
0
 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;
 }