Commit c7c54e88 authored by Tyler Wilson's avatar Tyler Wilson Committed by Jesse Mapel
Browse files

A few more focal plane tests (#101) 

* Added framing test for setFocalPlane.  Also enabled the testing of protected member functions, and modification of the ISD within the test fixture.

* Added jupyter_notebooks folder to CSM-CameraModel.

* Refactored setFocalPlane test so that the ISD changes are made local to the test (and not global).

* Refactored tests for setFocalPlane, distortionJacobian to modify Isd parameters and use them in tests.

* Added two new failing tests and a printIsd() helper function in the FrameIsdTest fixture to help with debugging.

* Fixed the failing tests.  They were due to the Jacobian being unexpectedly singular.

* Added a createModel function to the FrameIsdTest fixture.  Also added 2 more tests.

* Made requested changes to clean up tests, and added a parameterized testing fixture.

* Removed a debugging statement in the JacobianTest.
parent 6a1c1e16

include/usgscsm/UsgsAstroFrameSensorModel.h

+4 −0
Original line number Diff line number Diff line
@@ -299,10 +299,14 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM { 
    static const std::string _SENSOR_MODEL_NAME;



protected:



    FRIEND_TEST(FramerParameterizedTest,JacobianTest);

    FRIEND_TEST(FrameIsdTest, setFocalPlane1);

    FRIEND_TEST(FrameIsdTest, Jacobian1);

    FRIEND_TEST(FrameIsdTest, setFocalPlane_AllCoefficientsOne);

    FRIEND_TEST(FrameIsdTest, distortMe_AllCoefficientsOne);

    FRIEND_TEST(FrameIsdTest, setFocalPlane_AlternatingOnes);

    FRIEND_TEST(FrameIsdTest, distortMe_AlternatingOnes);



    virtual bool setFocalPlane(double dx,double dy,double &undistortedX,double &undistortedY) const;

    virtual void distortionFunction(double ux, double uy, double &dx, double &dy) const;

jupyter_notebooks/UsgsAstroFrameSensorModelTesting.ipynb

+14 −29
Original line number Diff line number Diff line 
%% Cell type:markdown id: tags:



# Image to ground



## by Kaj Williams and Jesse Mapel

## Notes: includes rotation.



%% Cell type:code id: tags:



``` python

import numpy as np

from __future__ import print_function, division

```



%% Cell type:code id: tags:



``` python

Samples = 7.5

Lines = 7.5



# optical center (pixels) in x,y direction

Cx=7.5

Cy=7.5



# focal length (m)

F=50.0e-3



# size of pixels in world units (m)

Px=1.0e-4

Py=1.0e-4



# observer position:

obs_x=1000.0

obs_y=0.0

obs_z=0.0



# radius of body (m):

major_radius=10.0

minor_radius=10.0



#Rotation:

omega=0

phi=-np.pi/2.

kappa=np.pi

```



%% Cell type:markdown id: tags:



## Calculation of camera look vector:

$\begin{bmatrix}\mathbf{x} & \mathbf{y} & \mathbf{z} \end{bmatrix}=

\begin{bmatrix}\mathbf{Samples} & \mathbf{Lines} & \mathbf{1} \end{bmatrix}

\begin{bmatrix}

\mathbf{P_y} & \mathbf{0} & \mathbf{0}\\

\mathbf{0} & \mathbf{P_x} & \mathbf{0}\\

\mathbf{-C_y P_y} & \mathbf{-C_x P_x} & \mathbf{F}

\end{bmatrix}

\begin{bmatrix}

\mathbf{sin(\phi)} & \mathbf{-sin(\omega)cos(\phi)} & \mathbf{cos(\omega)cos(\phi)}\\

\mathbf{-cos(\phi)sin(\kappa)} & \mathbf{cos(\omega)cos(\kappa)-sin(\omega)sin(\phi)sin(\kappa)} & \mathbf{cos(\omega)cos(\kappa)+sin(\omega)sin(\phi)sin(\kappa)}\\

\mathbf{cos(\phi)cos(\omega)} & \mathbf{cos(\omega)sin(\kappa)+sin(\omega)sin(\phi)cos(\kappa)} & \mathbf{cos(\omega)sin(\kappa)-sin(\omega)sin(\phi)cos(\kappa)}

\end{bmatrix}

$



%% Cell type:code id: tags:



``` python

image_vector = np.array([Samples, Lines, 1], dtype=float)

```



%% Cell type:code id: tags:



``` python

camera_array = np.array([[Py, 0, 0],[0, Px,0],[-Cy*Py,-Cx*Px,F]], dtype=float)

```



%% Cell type:code id: tags:



``` python

#Compute the camera look vector and normalize it:

camera_look_vector = np.matmul(np.transpose(image_vector),camera_array)

camera_look_vector=camera_look_vector/np.linalg.norm(camera_look_vector,2)

```



%% Cell type:code id: tags:



``` python

rotation_matrix=np.ndarray(shape=(3,3), dtype=float, order='F')

```



%% Cell type:code id: tags:



``` python

rotation_matrix[0,0]=np.cos(phi)*np.cos(kappa)

rotation_matrix[1,0]=np.cos(omega)*np.sin(kappa)+np.sin(omega)*np.sin(phi)*np.cos(kappa)

rotation_matrix[2,0]=np.sin(omega)*np.sin(kappa)-np.cos(omega)*np.sin(phi)*np.cos(kappa)

rotation_matrix[0,1]=-np.cos(phi)*np.sin(kappa)

rotation_matrix[1,1]=np.cos(omega)*np.cos(kappa)-np.sin(omega)*np.sin(phi)*np.sin(kappa)

rotation_matrix[2,1]=np.sin(omega)*np.cos(kappa)+np.cos(omega)*np.sin(phi)*np.sin(kappa)

rotation_matrix[0,2]=np.sin(phi)

rotation_matrix[1,2]=-np.sin(omega)*np.cos(phi)

rotation_matrix[2,2]=np.cos(omega)*np.cos(phi)

```



%% Cell type:code id: tags:



``` python

print(rotation_matrix)

```



%% Output



    [[-6.12323400e-17 -7.49879891e-33 -1.00000000e+00]

     [ 1.22464680e-16 -1.00000000e+00 -0.00000000e+00]

     [-1.00000000e+00 -1.22464680e-16  6.12323400e-17]]



%% Cell type:code id: tags:



``` python

look_vector=np.matmul(np.transpose(camera_look_vector),rotation_matrix)

```



%% Cell type:code id: tags:



``` python

print("After accounting for rotation,  look vector x,y,z: ",look_vector)

```



%% Output



    After accounting for rotation,  look vector x,y,z:  [-1.0000000e+00 -1.2246468e-16  6.1232340e-17]



%% Cell type:code id: tags:



``` python

radius_squared_ratio =major_radius**2/minor_radius**2

a=look_vector[0]**2 + look_vector[1]**2 + radius_squared_ratio*look_vector[2]**2

b=2*(look_vector[0]*obs_x+look_vector[1]*obs_y+radius_squared_ratio*look_vector[2]*obs_z)

c=obs_x**2+obs_y**2+radius_squared_ratio*obs_z**2-major_radius**2

discriminant=b**2-4.0*a*c

print('a:{} b:{} c:{} radius_squared_ratio:{}'.format(a,b,c,radius_squared_ratio))

```



%% Output



    a:1.0 b:-2000.0 c:999900.0 radius_squared_ratio:1.0



%% Cell type:code id: tags:



``` python

if discriminant<0 :

    discriminant=0 # closest intersection

discriminant

```



%% Output



    400.0



%% Cell type:code id: tags:



``` python

distance=(-b-np.sqrt(discriminant))/(2*a)

distance

```



%% Output



    990.0



%% Cell type:code id: tags:



``` python

obs_vector=np.array([obs_x, obs_y, obs_z])

```



%% Cell type:code id: tags:



``` python

ground_point = obs_vector+distance*look_vector

```



%% Cell type:code id: tags:



``` python

print("Planet coords. x,y,z: ",ground_point)

```



%% Output



    Planet coords. x,y,z:  [ 1.00000000e+01 -1.21240033e-13  6.06200166e-14]



%% Cell type:markdown id: tags:



# Ground to Image

### Notes: very basic implementation.



%% Cell type:code id: tags:



``` python

# Look vector:

x=0

y=0

z=-1

```



%% Cell type:markdown id: tags:



$\begin{bmatrix}\mathbf{Samples} & \mathbf{Lines} & \mathbf{1} \end{bmatrix}=

\begin{bmatrix}\mathbf{x} & \mathbf{y} & \mathbf{z}& \mathbf{1} \end{bmatrix}

\begin{bmatrix}

\mathbf{0} & \mathbf{1/P_x} & \mathbf{0}\\

\mathbf{1/P_y} & \mathbf{0} & \mathbf{0}\\

\mathbf{0} & \mathbf{0} & \mathbf{0}\\

\mathbf{C_y} & \mathbf{C_x} & \mathbf{1}

\end{bmatrix}

$



%% Cell type:code id: tags:



``` python

ground_vector=np.array([x, y, z,1], dtype=float)

```



%% Cell type:code id: tags:



``` python

camera_array = np.array([[0, 1/Px, 0],[1/Py, 0,0],[0,0,0],[Cy,Cx,1]], dtype=float)

```



%% Cell type:code id: tags:



``` python

Image_coords = np.matmul(np.transpose(ground_vector),camera_array)

```



%% Cell type:code id: tags:



``` python

print("Image coords. x,y,z: ",Image_coords)

```



%% Output



    Image coords. x,y,z:  [7.5 7.5 1. ]



%% Cell type:markdown id: tags:



# Radial Distortion Model



### The following code is for the Brown-Conrady model, implemented as the "division model" approach:

### https://en.wikipedia.org/wiki/Distortion_(optics)



%% Cell type:code id: tags:



``` python

#Radial distortion coefficients:

# if K1>0 then pincushion distortion.

# If K1<0 then barrel distortion.

K1=0.1

K2=0.0



# Distortion center x,y:

Xc=7.5

Yc=7.5



# Distorted coords x,y:

Xd=8.0

Yd=8.0



```



%% Cell type:code id: tags:



``` python

#helper expression for r^2:

r2=(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)

```



%% Cell type:markdown id: tags:



$X_u=X_c+\frac{(X_d-X_c)}{(1+K_1 r^2+K_2 r^4)}$



%% Cell type:markdown id: tags:



$Y_u=Y_c+\frac{(Y_d-Y_c)}{(1+K_1 r^2+K_2 r^4)}$



%% Cell type:code id: tags:



``` python

X_undistorted = Xc+(Xd-Xc)/(1+K1*r2+K2*r2*r2)

```



%% Cell type:code id: tags:



``` python

Y_undistorted = Yc+(Yd-Yc)/(1+K1*r2+K2*r2*r2)

```



%% Cell type:code id: tags:



``` python

print("Undistorted X,Y: ",X_undistorted,Y_undistorted)

```



%% Output



    Undistorted X,Y:  7.976190476190476 7.976190476190476



%% Cell type:markdown id: tags:



# Radial Distortion Model - MDIS NAC

### The following code is from the ik kernel for the MDIS NAC instrument.  The NAC distortion

### was determined by fitting data from a simulation of the NAC's optical behavior to a 3rd order

### Taylor series expansion.





%% Cell type:code id: tags:



``` python

odtx=[1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0]

odty=[0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0]



```



%% Cell type:markdown id: tags:



### [dx,dy]=distortMe(ux,uy):

### Takes in undistorted focal plane coordinates and returns distorted coordinates [dx,dy]



%% Cell type:code id: tags:



``` python

def distortMe(ux,uy,dtx,dty):



    f=[1,ux,uy,ux**2,ux*uy,uy**2,ux**3,uy*ux**2,ux*uy**2,uy**3]

    dx=0.0

    dy=0.0

    for i in range(len(f)):

        dx = dx+f[i]*dtx[i]

        dy = dy+f[i]*dty[i]

    return [dx,dy]



```



%% Cell type:markdown id: tags:



### [ux,uy]=undistortMe(dx,dy):

### Computes undistored focal plane (ux,uy) coordinates given distorted focal plane

### coordinates using the Newton-Raphson Method for root finding a system of equations:



%% Cell type:markdown id: tags:



$$f_1(x_1,x_2,\cdots,x_n) = f_1(\textbf{x}) = \textbf{0}$$

$$f_2(x_1,x_2,\cdots,x_n) = f_2(\textbf{x}) = \textbf{0}$$

$$\vdots$$

$$f_n(x_1,x_2,\cdots,x_n) = f_n(\textbf{x}) = \textbf{0}$$



%% Cell type:markdown id: tags:



### To solve consider Taylor series expansion of f about x to first order:



$$f(\textbf{x} +\delta \textbf{x} ) = f_i(\textbf{x}) + \sum_{j=0}^n\frac{\partial f_i}{\partial x_j}\delta x_j +O(\delta x ^2),i = 1,\cdots,n$$



%% Cell type:markdown id: tags:



### Ignore the $O(\delta x^2)$ terms and let $f(x+\delta x) = 0 $ (ie, $x+\delta x$ is the root we are seeking).  Then:

### $$-f_i(\textbf{x}) = \sum_{j=0}^n\frac{\partial f_i}{\partial x_j}\delta x_j, j=0,\cdots ,n$$



%% Cell type:markdown id: tags:



### Therefore:  $\delta x_j = -f(\textbf{x})J_f^{-1}(\textbf(x)$ ($J$ = the Jacobian of $f$)



%% Cell type:markdown id: tags:



### And finally:

$$ \textbf{x} \Leftarrow \textbf{x}+ \delta(\textbf{x}) = \textbf{x} - J_f^{-1}(\textbf{x})f(\textbf{x})$$



%% Cell type:markdown id: tags:



### In code:





%% Cell type:code id: tags:



``` python

def Jacobian(x,y,odtx,odty):

    d_dx=[0.0,1.0,0.0,2*x,y,0.0,3*x*x,2*x*y,y*y,0.0]

    d_dy=[0.0,0.0,1.0,0.0,x,2*y,0.0,x**2,2*x*y,3*y**2]



    Jxx=0.0;

    Jxy = 0.0;

    Jyx= 0.0;

    Jyy = 0.0;



    for i in range(len(d_dx)):

        Jxx = Jxx+d_dx[i]*odtx[i]

        Jxy = Jxy+d_dy[i]*odtx[i]

        Jyx = Jyx + d_dx[i]*odty[i]

        Jyy = Jyy + d_dy[i]*odty[i]



    return [Jxx,Jxy,Jyx,Jyy]





```



%% Cell type:code id: tags:



``` python

def undistortMe(dx,dy,odtx,odty):

    epsilon =1.4e-5

    epsilon =1.4e-7

    maxIts = 60

    #initial guess

    x = dx

    y = dy

    [fx,fy]= distortMe(x,y,odtx,odty)

    for i in range(maxIts):

        print(i)

        [fx,fy]=distortMe(x,y,odtx,odty)

        fx=dx-fx

        fy = dy-fy

        J = Jacobian(x,y,odtx,odty)

        determinant = J[0]*J[3]-J[1]*J[2]

        [Jxx,Jxy,Jyx,Jyy] = Jacobian(x,y,odtx,odty)

        determinant = Jxx*Jyy-Jxy*Jyx



        if (abs(determinant) < 1e-7):

            print("Jacobian is singular.")

            print(determinant)

            break;

        else:

            x = x+(J[3]*fx-J[1]*fy)/determinant

            y = y+(J[0]*fy-J[2]*fx)/determinant

        if (abs(fx)+abs(fy) <= epsilon):

            x = x+(Jyy*fx-Jxy*fy)/determinant

            y = y+(Jxx*fy-Jyx*fx)/determinant

        if ( (abs(fx)+abs(fy)) <= epsilon):

            return [x,y]

    return [dx,dy]







```



%% Cell type:code id: tags:



``` python

```



%% Cell type:code id: tags:



``` python

distortion = distortMe(7.5,7.5,odtx,odty)

undistorted = undistortMe(distortion[0],distortion[1],odtx,odty)

J=Jacobian(2.5,2.5,odtx,odty)



print([Jxx,Jxy,Jyx,Jyy])

print(distortion)

print (undistorted)

```



%% Output



    0

    1

    2

    3

    4

    5

    6

    7

    8

    9

    10

    11

    12

    13

    14

    15

    16

    [232.5, 121.0, 128.5, 240.0]

    [908.5, 963.75]

    [7.499999999999999, 7.5]



%% Cell type:code id: tags:



``` python

```

src/UsgsAstroFrameSensorModel.cpp

+11 −5
Original line number Diff line number Diff line
@@ -1043,10 +1043,9 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, 
                                       double &undistortedX,

                                       double &undistortedY ) const {





  // Solve the distortion equation using the Newton-Raphson method.

  // Set the error tolerance to about one millionth of a NAC pixel.

  const double tol = 1.4E-5;

  const double tol = 1.4E-7;



  // The maximum number of iterations of the Newton-Raphson method.

  const int maxTries = 60;
@@ -1066,6 +1065,7 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, 


  distortionFunction(x, y, fx, fy);





  for (int count = 1; ((fabs(fx) +fabs(fy)) > tol) && (count < maxTries); count++) {



    this->distortionFunction(x, y, fx, fy);
@@ -1076,12 +1076,16 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, 
    distortionJacobian(x, y, Jxx, Jxy, Jyx, Jyy);



    double determinant = Jxx * Jyy - Jxy * Jyx;

    if (fabs(determinant) < 1E-6) {

    if (fabs(determinant) < 1E-7) {



      cout << "Singular determinant." << endl;

      undistortedX = x;

      undistortedY = y;

      //

      // Near-zero determinant. Add error handling here.

      //

      //-- Just break out and return with no convergence

      break;

      return false;

    }



    x = x + (Jyy * fx - Jxy * fy) / determinant;
@@ -1092,6 +1096,7 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, 
    // The method converged to a root.

    undistortedX = x;

    undistortedY = y;

    return true;



  }

  else {
@@ -1099,6 +1104,7 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, 
    // number of iterations. Return with no distortion.

    undistortedX = dx;

    undistortedY = dy;

    return false;

  }



  return true;

tests/FrameCameraTests.cpp

+297 −126

File changed.

Preview size limit exceeded, changes collapsed.