diff --git a/include/usgscsm/UsgsAstroFrameSensorModel.h b/include/usgscsm/UsgsAstroFrameSensorModel.h
index 087206e367dd6cb00f6c51ba1b1b8bc0c18b19c4..fb9052a1531b7eaef9c61819f0587944b3f88121 100644
--- a/include/usgscsm/UsgsAstroFrameSensorModel.h
+++ b/include/usgscsm/UsgsAstroFrameSensorModel.h
@@ -5,7 +5,7 @@
#include <cmath>
#include <iostream>
#include <vector>
-
+#include <gtest/gtest.h>
#include "RasterGM.h"
#include "CorrelationModel.h"
@@ -298,8 +298,11 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM {
static const std::string _SENSOR_MODEL_NAME;
-
- protected:
+protected:
+ FRIEND_TEST(FrameIsdTest, setFocalPlane1);
+ FRIEND_TEST(FrameIsdTest, Jacobian1);
+ FRIEND_TEST(FrameIsdTest, setFocalPlane_AllCoefficientsOne);
+ FRIEND_TEST(FrameIsdTest, distortMe_AllCoefficientsOne);
virtual bool setFocalPlane(double dx,double dy,double &undistortedX,double &undistortedY) const;
virtual void distortionFunction(double ux, double uy, double &dx, double &dy) const;
@@ -307,6 +310,7 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM {
double &Jxy, double &Jyx, double &Jyy) const;
+
private:
// Input parameters
diff --git a/jupyter_notebooks/UsgsAstroFrameSensorModelTesting.ipynb b/jupyter_notebooks/UsgsAstroFrameSensorModelTesting.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..1a87cb486359a878c70ee9ceb1cdd77c4c32ff7f
--- /dev/null
+++ b/jupyter_notebooks/UsgsAstroFrameSensorModelTesting.ipynb
@@ -0,0 +1,693 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {
+ "collapsed": true
+ },
+ "source": [
+ "# Image to ground\n",
+ "\n",
+ "## by Kaj Williams and Jesse Mapel\n",
+ "## Notes: includes rotation."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 284,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import numpy as np\n",
+ "from __future__ import print_function, division"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 285,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "Samples = 7.5\n",
+ "Lines = 7.5\n",
+ "\n",
+ "# optical center (pixels) in x,y direction\n",
+ "Cx=7.5 \n",
+ "Cy=7.5 \n",
+ "\n",
+ "# focal length (m)\n",
+ "F=50.0e-3 \n",
+ "\n",
+ "# size of pixels in world units (m)\n",
+ "Px=1.0e-4 \n",
+ "Py=1.0e-4\n",
+ "\n",
+ "# observer position:\n",
+ "obs_x=1000.0\n",
+ "obs_y=0.0\n",
+ "obs_z=0.0\n",
+ "\n",
+ "# radius of body (m):\n",
+ "major_radius=10.0\n",
+ "minor_radius=10.0\n",
+ "\n",
+ "#Rotation:\n",
+ "omega=0\n",
+ "phi=-np.pi/2.\n",
+ "kappa=np.pi"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Calculation of camera look vector:\n",
+ "$\\begin{bmatrix}\\mathbf{x} & \\mathbf{y} & \\mathbf{z} \\end{bmatrix}= \n",
+ "\\begin{bmatrix}\\mathbf{Samples} & \\mathbf{Lines} & \\mathbf{1} \\end{bmatrix}\n",
+ "\\begin{bmatrix}\n",
+ "\\mathbf{P_y} & \\mathbf{0} & \\mathbf{0}\\\\\n",
+ "\\mathbf{0} & \\mathbf{P_x} & \\mathbf{0}\\\\\n",
+ "\\mathbf{-C_y P_y} & \\mathbf{-C_x P_x} & \\mathbf{F} \n",
+ "\\end{bmatrix}\n",
+ "\\begin{bmatrix}\n",
+ "\\mathbf{sin(\\phi)} & \\mathbf{-sin(\\omega)cos(\\phi)} & \\mathbf{cos(\\omega)cos(\\phi)}\\\\\n",
+ "\\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)}\\\\\n",
+ "\\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)} \n",
+ "\\end{bmatrix}\n",
+ "$"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 286,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "image_vector = np.array([Samples, Lines, 1], dtype=float)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 287,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "camera_array = np.array([[Py, 0, 0],[0, Px,0],[-Cy*Py,-Cx*Px,F]], dtype=float)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 288,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "#Compute the camera look vector and normalize it:\n",
+ "camera_look_vector = np.matmul(np.transpose(image_vector),camera_array)\n",
+ "camera_look_vector=camera_look_vector/np.linalg.norm(camera_look_vector,2)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 289,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "rotation_matrix=np.ndarray(shape=(3,3), dtype=float, order='F')"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 290,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "rotation_matrix[0,0]=np.cos(phi)*np.cos(kappa)\n",
+ "rotation_matrix[1,0]=np.cos(omega)*np.sin(kappa)+np.sin(omega)*np.sin(phi)*np.cos(kappa)\n",
+ "rotation_matrix[2,0]=np.sin(omega)*np.sin(kappa)-np.cos(omega)*np.sin(phi)*np.cos(kappa)\n",
+ "rotation_matrix[0,1]=-np.cos(phi)*np.sin(kappa)\n",
+ "rotation_matrix[1,1]=np.cos(omega)*np.cos(kappa)-np.sin(omega)*np.sin(phi)*np.sin(kappa)\n",
+ "rotation_matrix[2,1]=np.sin(omega)*np.cos(kappa)+np.cos(omega)*np.sin(phi)*np.sin(kappa)\n",
+ "rotation_matrix[0,2]=np.sin(phi)\n",
+ "rotation_matrix[1,2]=-np.sin(omega)*np.cos(phi)\n",
+ "rotation_matrix[2,2]=np.cos(omega)*np.cos(phi)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 291,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "[[-6.12323400e-17 -7.49879891e-33 -1.00000000e+00]\n",
+ " [ 1.22464680e-16 -1.00000000e+00 -0.00000000e+00]\n",
+ " [-1.00000000e+00 -1.22464680e-16 6.12323400e-17]]\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(rotation_matrix)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 292,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "look_vector=np.matmul(np.transpose(camera_look_vector),rotation_matrix)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 293,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "After accounting for rotation, look vector x,y,z: [-1.0000000e+00 -1.2246468e-16 6.1232340e-17]\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(\"After accounting for rotation, look vector x,y,z: \",look_vector)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 294,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "a:1.0 b:-2000.0 c:999900.0 radius_squared_ratio:1.0\n"
+ ]
+ }
+ ],
+ "source": [
+ "radius_squared_ratio =major_radius**2/minor_radius**2\n",
+ "a=look_vector[0]**2 + look_vector[1]**2 + radius_squared_ratio*look_vector[2]**2\n",
+ "b=2*(look_vector[0]*obs_x+look_vector[1]*obs_y+radius_squared_ratio*look_vector[2]*obs_z)\n",
+ "c=obs_x**2+obs_y**2+radius_squared_ratio*obs_z**2-major_radius**2\n",
+ "discriminant=b**2-4.0*a*c\n",
+ "print('a:{} b:{} c:{} radius_squared_ratio:{}'.format(a,b,c,radius_squared_ratio))"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 295,
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "400.0"
+ ]
+ },
+ "execution_count": 295,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "if discriminant<0 :\n",
+ " discriminant=0 # closest intersection\n",
+ "discriminant"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 296,
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "990.0"
+ ]
+ },
+ "execution_count": 296,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "distance=(-b-np.sqrt(discriminant))/(2*a)\n",
+ "distance"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 297,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "obs_vector=np.array([obs_x, obs_y, obs_z])"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 298,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "ground_point = obs_vector+distance*look_vector"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 299,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "Planet coords. x,y,z: [ 1.00000000e+01 -1.21240033e-13 6.06200166e-14]\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(\"Planet coords. x,y,z: \",ground_point)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {
+ "collapsed": true
+ },
+ "source": [
+ "# Ground to Image\n",
+ "### Notes: very basic implementation."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 300,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# Look vector:\n",
+ "x=0\n",
+ "y=0\n",
+ "z=-1"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "$\\begin{bmatrix}\\mathbf{Samples} & \\mathbf{Lines} & \\mathbf{1} \\end{bmatrix}=\n",
+ "\\begin{bmatrix}\\mathbf{x} & \\mathbf{y} & \\mathbf{z}& \\mathbf{1} \\end{bmatrix} \n",
+ "\\begin{bmatrix}\n",
+ "\\mathbf{0} & \\mathbf{1/P_x} & \\mathbf{0}\\\\\n",
+ "\\mathbf{1/P_y} & \\mathbf{0} & \\mathbf{0}\\\\\n",
+ "\\mathbf{0} & \\mathbf{0} & \\mathbf{0}\\\\\n",
+ "\\mathbf{C_y} & \\mathbf{C_x} & \\mathbf{1} \n",
+ "\\end{bmatrix}\n",
+ "$"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 301,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "ground_vector=np.array([x, y, z,1], dtype=float)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 302,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "camera_array = np.array([[0, 1/Px, 0],[1/Py, 0,0],[0,0,0],[Cy,Cx,1]], dtype=float)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 303,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "Image_coords = np.matmul(np.transpose(ground_vector),camera_array)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 304,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "Image coords. x,y,z: [7.5 7.5 1. ]\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(\"Image coords. x,y,z: \",Image_coords)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {
+ "collapsed": true
+ },
+ "source": [
+ "# Radial Distortion Model\n",
+ "\n",
+ "### The following code is for the Brown-Conrady model, implemented as the \"division model\" approach:\n",
+ "### https://en.wikipedia.org/wiki/Distortion_(optics)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 305,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "#Radial distortion coefficients:\n",
+ "# if K1>0 then pincushion distortion.\n",
+ "# If K1<0 then barrel distortion.\n",
+ "K1=0.1\n",
+ "K2=0.0\n",
+ "\n",
+ "# Distortion center x,y:\n",
+ "Xc=7.5\n",
+ "Yc=7.5\n",
+ "\n",
+ "# Distorted coords x,y:\n",
+ "Xd=8.0\n",
+ "Yd=8.0\n",
+ "\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 306,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "#helper expression for r^2:\n",
+ "r2=(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "$X_u=X_c+\\frac{(X_d-X_c)}{(1+K_1 r^2+K_2 r^4)}$"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "$Y_u=Y_c+\\frac{(Y_d-Y_c)}{(1+K_1 r^2+K_2 r^4)}$"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 307,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "X_undistorted = Xc+(Xd-Xc)/(1+K1*r2+K2*r2*r2)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 308,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "Y_undistorted = Yc+(Yd-Yc)/(1+K1*r2+K2*r2*r2)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 309,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "Undistorted X,Y: 7.976190476190476 7.976190476190476\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(\"Undistorted X,Y: \",X_undistorted,Y_undistorted)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {
+ "collapsed": true
+ },
+ "source": [
+ "# Radial Distortion Model - MDIS NAC\n",
+ "### The following code is from the ik kernel for the MDIS NAC instrument. The NAC distortion\n",
+ "### was determined by fitting data from a simulation of the NAC's optical behavior to a 3rd order\n",
+ "### Taylor series expansion.\n",
+ "\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 310,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "odtx=[1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0]\n",
+ "odty=[0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0]\n",
+ "\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### [dx,dy]=distortMe(ux,uy):\n",
+ "### Takes in undistorted focal plane coordinates and returns distorted coordinates [dx,dy]"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 311,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "def distortMe(ux,uy,dtx,dty):\n",
+ " \n",
+ " f=[1,ux,uy,ux**2,ux*uy,uy**2,ux**3,uy*ux**2,ux*uy**2,uy**3]\n",
+ " dx=0.0\n",
+ " dy=0.0\n",
+ " for i in range(len(f)):\n",
+ " dx = dx+f[i]*dtx[i]\n",
+ " dy = dy+f[i]*dty[i] \n",
+ " return [dx,dy]\n",
+ " \n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### [ux,uy]=undistortMe(dx,dy):\n",
+ "### Computes undistored focal plane (ux,uy) coordinates given distorted focal plane \n",
+ "### coordinates using the Newton-Raphson Method for root finding a system of equations:\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "$$f_1(x_1,x_2,\\cdots,x_n) = f_1(\\textbf{x}) = \\textbf{0}$$\n",
+ "$$f_2(x_1,x_2,\\cdots,x_n) = f_2(\\textbf{x}) = \\textbf{0}$$\n",
+ "$$\\vdots$$\n",
+ "$$f_n(x_1,x_2,\\cdots,x_n) = f_n(\\textbf{x}) = \\textbf{0}$$\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### To solve consider Taylor series expansion of f about x to first order:\n",
+ "\n",
+ "$$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",
+ "metadata": {},
+ "source": [
+ "### 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:\n",
+ "### $$-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",
+ "metadata": {},
+ "source": [
+ "### Therefore: $\\delta x_j = -f(\\textbf{x})J_f^{-1}(\\textbf(x)$ ($J$ = the Jacobian of $f$)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### And finally:\n",
+ "$$ \\textbf{x} \\Leftarrow \\textbf{x}+ \\delta(\\textbf{x}) = \\textbf{x} - J_f^{-1}(\\textbf{x})f(\\textbf{x})$$"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### In code:\n",
+ "\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 312,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "def Jacobian(x,y,odtx,odty):\n",
+ " d_dx=[0.0,1.0,0.0,2*x,y,0.0,3*x*x,2*x*y,y*y,0.0]\n",
+ " d_dy=[0.0,0.0,1.0,0.0,x,2*y,0.0,x**2,2*x*y,3*y**2]\n",
+ " \n",
+ " Jxx=0.0;\n",
+ " Jxy = 0.0;\n",
+ " Jyx= 0.0;\n",
+ " Jyy = 0.0;\n",
+ " \n",
+ " for i in range(len(d_dx)):\n",
+ " Jxx = Jxx+d_dx[i]*odtx[i]\n",
+ " Jxy = Jxy+d_dy[i]*odtx[i]\n",
+ " Jyx = Jyx + d_dx[i]*odty[i]\n",
+ " Jyy = Jyy + d_dy[i]*odty[i] \n",
+ " \n",
+ " return [Jxx,Jxy,Jyx,Jyy]\n",
+ "\n",
+ " "
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 313,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "def undistortMe(dx,dy,odtx,odty):\n",
+ " epsilon =1.4e-5\n",
+ " maxIts = 60\n",
+ " #initial guess\n",
+ " x = dx\n",
+ " y = dy\n",
+ " [fx,fy]= distortMe(x,y,odtx,odty)\n",
+ " for i in range(maxIts):\n",
+ " print(i)\n",
+ " [fx,fy]=distortMe(x,y,odtx,odty)\n",
+ " fx=dx-fx\n",
+ " fy = dy-fy\n",
+ " J = Jacobian(x,y,odtx,odty)\n",
+ " determinant = J[0]*J[3]-J[1]*J[2]\n",
+ " \n",
+ " if (abs(determinant) < 1e-7):\n",
+ " print(\"Jacobian is singular.\")\n",
+ " print(determinant)\n",
+ " break;\n",
+ " else:\n",
+ " x = x+(J[3]*fx-J[1]*fy)/determinant\n",
+ " y = y+(J[0]*fy-J[2]*fx)/determinant\n",
+ " if (abs(fx)+abs(fy) <= epsilon):\n",
+ " return [x,y]\n",
+ " return [dx,dy] \n",
+ " \n",
+ " \n",
+ " "
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": []
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 314,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "0\n",
+ "1\n",
+ "2\n",
+ "3\n",
+ "4\n",
+ "5\n",
+ "6\n",
+ "7\n",
+ "8\n",
+ "9\n",
+ "10\n",
+ "11\n",
+ "12\n",
+ "13\n",
+ "14\n",
+ "15\n",
+ "16\n",
+ "[908.5, 963.75]\n",
+ "[7.499999999999999, 7.5]\n"
+ ]
+ }
+ ],
+ "source": [
+ "distortion = distortMe(7.5,7.5,odtx,odty)\n",
+ "undistorted = undistortMe(distortion[0],distortion[1],odtx,odty)\n",
+ "\n",
+ "print(distortion)\n",
+ "print (undistorted)"
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 3",
+ "language": "python",
+ "name": "python3"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.6.5"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index 6116a18dbb58bb9e28483c43bba7dfa2228210da..8ff40263616876531ca645665b3ce607265d1e6c 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -1076,7 +1076,7 @@ bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy,
distortionJacobian(x, y, Jxx, Jxy, Jyx, Jyy);
double determinant = Jxx * Jyy - Jxy * Jyx;
- if (determinant < 1E-6) {
+ if (fabs(determinant) < 1E-6) {
//
// Near-zero determinant. Add error handling here.
//
diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp
index 0d23f2878f0c8f4121373e3de65e65a006f0e2b8..cdf235d77330c548c97c16193532c8d6d3431e1b 100644
--- a/tests/FrameCameraTests.cpp
+++ b/tests/FrameCameraTests.cpp
@@ -2,12 +2,15 @@
#include "UsgsAstroFrameSensorModel.h"
#include <json.hpp>
-
+#include <iostream>
+#include <iomanip>
#include <sstream>
#include <fstream>
-
+#include <map>
+#include <vector>
#include <gtest/gtest.h>
+using namespace std;
using json = nlohmann::json;
class FrameIsdTest : public ::testing::Test {
@@ -15,6 +18,16 @@ class FrameIsdTest : public ::testing::Test {
csm::Isd isd;
+ void printIsd(csm::Isd &localIsd) {
+ multimap<string,string> isdmap= localIsd.parameters();
+ for (auto it = isdmap.begin(); it != isdmap.end();++it){
+
+ cout << it->first << " : " << it->second << endl;
+ }
+
+
+ }
+
virtual void SetUp() {
std::ifstream isdFile("data/simpleFramerISD.json");
json jsonIsd = json::parse(isdFile);
@@ -36,8 +49,12 @@ class FrameIsdTest : public ::testing::Test {
class FrameSensorModel : public ::testing::Test {
protected:
+
+ protected :
UsgsAstroFrameSensorModel *sensorModel;
-
+
+
+
void SetUp() override {
sensorModel = NULL;
std::ifstream isdFile("data/simpleFramerISD.json");
@@ -46,24 +63,25 @@ class FrameSensorModel : public ::testing::Test {
for (json::iterator it = jsonIsd.begin(); it != jsonIsd.end(); ++it) {
json jsonValue = it.value();
if (jsonValue.size() > 1) {
- for (int i = 0; i < jsonValue.size(); i++) {
- isd.addParam(it.key(), jsonValue[i].dump());
+ for (int i = 0; i < jsonValue.size(); i++){
+ isd.addParam(it.key(), jsonValue[i].dump());
}
}
else {
isd.addParam(it.key(), jsonValue.dump());
}
}
+
isdFile.close();
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
}
@@ -72,6 +90,7 @@ class FrameSensorModel : public ::testing::Test {
delete sensorModel;
sensorModel = NULL;
}
+
}
};
@@ -124,6 +143,187 @@ TEST_F(FrameSensorModel, OffBody4) {
}
+
+TEST_F(FrameIsdTest, setFocalPlane1) {
+ int precision = 20;
+ UsgsAstroFramePlugin frameCameraPlugin;
+ std:string odtx_key= "odt_x";
+ std::string odty_key="odt_y";
+
+ isd.clearParams(odty_key);
+ isd.clearParams(odtx_key);
+
+ csm::ImageCoord imagePt(7.5, 7.5);
+ double ux,uy;
+
+ vector<double> odtx{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ vector<double> odty{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+
+
+ for (auto & val: odtx){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_x", strval.str());
+ }
+ for (auto & val: odty){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_y", strval.str());
+ }
+
+ csm::Model *model = frameCameraPlugin.constructModelFromISD(
+ isd,
+ "USGS_ASTRO_FRAME_SENSOR_MODEL");
+
+
+
+ UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
+
+ sensorModel->setFocalPlane(imagePt.samp, imagePt.line, ux, uy);
+ EXPECT_NEAR(imagePt.samp,7.5,1e-8 );
+ EXPECT_NEAR(imagePt.line,7.5,1e-8);
+
+}
+
+
+
+TEST_F(FrameIsdTest, Jacobian1) {
+
+ int precision = 20;
+ UsgsAstroFramePlugin frameCameraPlugin;
+ std:string odtx_key= "odt_x";
+ std::string odty_key="odt_y";
+
+ isd.clearParams(odty_key);
+ isd.clearParams(odtx_key);
+
+ csm::ImageCoord imagePt(7.5, 7.5);
+
+ vector<double> odtx{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0};
+ vector<double> odty{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0};
+
+ for (auto & val: odtx){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_x", strval.str());
+ }
+ for (auto & val: odty){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_y", strval.str());
+ }
+
+ csm::Model *model = frameCameraPlugin.constructModelFromISD(
+ isd,
+ "USGS_ASTRO_FRAME_SENSOR_MODEL");
+
+ UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
+
+
+ double Jxx,Jxy,Jyx,Jyy;
+ sensorModel->distortionJacobian(imagePt.samp, imagePt.line, Jxx, Jxy,Jyx,Jyy);
+
+
+
+ EXPECT_NEAR(Jxx,56.25,1e-8 );
+ EXPECT_NEAR(Jxy,112.5,1e-8);
+ EXPECT_NEAR(Jyx,56.25,1e-8);
+ EXPECT_NEAR(Jyy,281.25,1e-8);
+
+}
+
+
+TEST_F(FrameIsdTest, distortMe_AllCoefficientsOne) {
+
+ int precision = 20;
+ UsgsAstroFramePlugin frameCameraPlugin;
+ std:string odtx_key= "odt_x";
+ std::string odty_key="odt_y";
+
+ isd.clearParams(odty_key);
+ isd.clearParams(odtx_key);
+
+ csm::ImageCoord imagePt(7.5, 7.5);
+
+ vector<double> odtx{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+ vector<double> odty{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+
+ for (auto & val: odtx){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_x", strval.str());
+ }
+ for (auto & val: odty){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_y", strval.str());
+ }
+
+ csm::Model *model = frameCameraPlugin.constructModelFromISD(
+ isd,
+ "USGS_ASTRO_FRAME_SENSOR_MODEL");
+
+ UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
+
+
+ printIsd(isd);
+ double dx,dy;
+ sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy );
+
+ cout << "dx: " << dx << endl;
+ cout << "dy: " << dy << endl;
+
+ EXPECT_NEAR(dx,1872.25,1e-8 );
+ EXPECT_NEAR(dy,1872.25,1e-8);
+
+}
+
+TEST_F(FrameIsdTest, setFocalPlane_AllCoefficientsOne) {
+
+ int precision = 20;
+ UsgsAstroFramePlugin frameCameraPlugin;
+ std:string odtx_key= "odt_x";
+ std::string odty_key="odt_y";
+
+ isd.clearParams(odty_key);
+ isd.clearParams(odtx_key);
+
+ csm::ImageCoord imagePt(1872.25, 1872.25);
+
+ vector<double> odtx{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+ vector<double> odty{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+
+ for (auto & val: odtx){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_x", strval.str());
+ }
+ for (auto & val: odty){
+ ostringstream strval;
+ strval << setprecision(precision) << val;
+ isd.addParam("odt_y", strval.str());
+ }
+
+ csm::Model *model = frameCameraPlugin.constructModelFromISD(
+ isd,
+ "USGS_ASTRO_FRAME_SENSOR_MODEL");
+
+ UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
+
+
+ double ux,uy;
+ sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy );
+
+
+ //The Jacobian is singular, so the setFocalPlane should break out of it's iteration and
+ //returns the same distorted coordinates that were passed in.
+ EXPECT_NEAR(ux,imagePt.samp,1e-8 );
+ EXPECT_NEAR(uy,imagePt.line,1e-8);
+
+}
+
+
+
// Focal Length Tests:
TEST_F(FrameIsdTest, FL500_OffBody4) {
std::string key = "focal_length";
@@ -131,13 +331,13 @@ TEST_F(FrameIsdTest, FL500_OffBody4) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(15.0, 15.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
@@ -151,13 +351,13 @@ TEST_F(FrameIsdTest, FL500_OffBody3) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(0.0, 0.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
@@ -171,13 +371,13 @@ TEST_F(FrameIsdTest, FL500_Center) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(7.5, 7.5);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
@@ -191,20 +391,20 @@ TEST_F(FrameIsdTest, FL500_SlightlyOffCenter) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(7.5, 6.5);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
EXPECT_NEAR(groundPt.x, 9.99803960, 1e-8);
EXPECT_NEAR(groundPt.y, 0.0, 1e-8);
EXPECT_NEAR(groundPt.z, 1.98000392e-01, 1e-8);
-
+
}
//Observer x position:
@@ -214,20 +414,20 @@ TEST_F(FrameIsdTest, X10_SlightlyOffCenter) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(7.5, 6.5);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
EXPECT_NEAR(groundPt.x, 10.0, 1e-8);
EXPECT_NEAR(groundPt.y, 0.0, 1e-8);
EXPECT_NEAR(groundPt.z, 0.0, 1e-8);
-
+
}
TEST_F(FrameIsdTest, X1e9_SlightlyOffCenter) {
std::string key = "x_sensor_origin";
@@ -235,13 +435,13 @@ TEST_F(FrameIsdTest, X1e9_SlightlyOffCenter) {
isd.clearParams(key);
isd.addParam(key,newValue);
UsgsAstroFramePlugin frameCameraPlugin;
-
+
csm::Model *model = frameCameraPlugin.constructModelFromISD(
isd,
"USGS_ASTRO_FRAME_SENSOR_MODEL");
-
+
UsgsAstroFrameSensorModel* sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model);
-
+
ASSERT_NE(sensorModel, nullptr);
csm::ImageCoord imagePt(7.5, 6.5);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
diff --git a/tests/data/simpleFramerISD.json b/tests/data/simpleFramerISD.json
index e44f64797b293025dff5828e7bcc11f578d323c0..f1c3733b925fb0c0ec20b82362c3a5e3835aa3b9 100644
--- a/tests/data/simpleFramerISD.json
+++ b/tests/data/simpleFramerISD.json
@@ -47,17 +47,7 @@
"phi": -1.5707963267948966,
"kappa": 3.141592653589793,
"semi_major_axis":0.01,
- "semi_minor_axis":0.01,
- "transx": [
- 0.0,
- 0.1,
- 0.0
- ],
- "transy": [
- 0.0,
- 0.0,
- 0.1
- ],
+ "semi_minor_axis":0.01,
"x_sensor_origin": 1000,
"y_sensor_origin": 0,
"z_sensor_origin": 0,