single_photo_resection.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# photogrammetry-resection\n",
    "\n",
    "A simple single-photo resection program written in python.\n",
    "\n",
    "Solution technique uses a performance function based on the collinearity equation that is minimized with scipy.optimize.minimize() or scipy.optimize.leastsq().\n",
    "\n",
    "Two input files are needed: camera calibration and control points. Sample data is from: \n",
    "    Introduction to Modern Photogrammetry by Mikhail, Bethel, McGlone\n",
    "    John Wiley & Sons, Inc. 2001\n",
    "\n",
    "Command line usage: python resection.py cam.inp resect.inp\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Camera Calibration input file, cam.inp\n",
    "sample data:"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "0\n",
    "0\n",
    "-1.57\n",
    "914250.0\n",
    "575400.0\n",
    "800.0\n",
    "0\n",
    "0\n",
    "152.222\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Control point input file, resect.inp\n",
    "sample data:\n"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "ph12   56.515  -78.969  913928.64  575198.44  189.64\n",
    "t19     1.242    1.134  914270.77  575432.35  191.26\n",
    "ph11   95.576   97.171  914684.64  575022.09  186.72\n",
    "ph21  -70.988   92.733  914662.47  575738.30  191.94\n",
    "s311    0.651  -30.068  914137.97  575435.45  190.69\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### These packages are required:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "import numpy as np\n",
    "from scipy.optimize import minimize\n",
    "from scipy.optimize import leastsq\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## resection_minimize.py\n",
    "using scipy minimize"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization terminated successfully.\n",
      "         Current function value: 0.000751\n",
      "         Iterations: 28\n",
      "         Function evaluations: 520\n",
      "         Gradient evaluations: 65\n",
      "Solution:\n",
      "omega,  -0.006507623272946931\n",
      "phi,  -0.008522026409835095\n",
      "kappa,  -1.5753221241359454\n",
      "XL,  914260.4216951398\n",
      "YL,  575441.8356485438\n",
      "ZL,  839.1303716499455\n"
     ]
    }
   ],
   "source": [
    "# Author:  Jeffrey T. Walton, Paul Smith's College, New York\n",
    "#\n",
    "#   Single-photo resection - calculates the camera orientation and location\n",
    "#       given camera calibration parameters, control point photo and world\n",
    "#        coordinates and initial guesses for camera exterior orientation.\n",
    "#\n",
    "#   based on MATLAB code from:\n",
    "#   Introduction to Modern Photogrammetry by Mikhail, Bethel, McGlone\n",
    "#   John Wiley & Sons, Inc. 2001\n",
    "\n",
    "import sys\n",
    "import numpy as np\n",
    "from scipy.optimize import minimize\n",
    "\n",
    "\n",
    "def collinearity_eqn_residual(iop,eop,x,y,X,Y,Z):\n",
    "    \"\"\"\n",
    "    Usage:\n",
    "        collinearity_eqn_residual(iop,eop,x,y,X,Y,Z)\n",
    "\n",
    "    Inputs:\n",
    "        iop = dict of interior orientation parameters: x0, y0, f\n",
    "        eop = dict of exterior orientation parameters: omega, phi, kappa, XL, YL, ZL\n",
    "        x = array of x photo coordinates of control points\n",
    "        y = array of y photo coordinates of control points\n",
    "        X = array of X world coordinates of control points\n",
    "        Y = array of Y world coordinates of control points\n",
    "        Z = array of Z world coordinates of control points\n",
    "\n",
    "    Returns:\n",
    "        residuals in x and y collinearity equations for a single point as a tuple\n",
    "    \"\"\"\n",
    "    from math import sin, cos\n",
    "    x0 = iop['x0']\n",
    "    y0 = iop['y0']\n",
    "    focallength = iop['f']\n",
    "\n",
    "    om = eop['omega']\n",
    "    ph = eop['phi']\n",
    "    kp = eop['kappa']\n",
    "\n",
    "    XL = eop['XL']\n",
    "    YL = eop['YL']\n",
    "    ZL = eop['ZL']\n",
    "\n",
    "    Mom = np.matrix([[1, 0, 0], [0, cos(om), sin(om)], [0, -sin(om), cos(om)]])\n",
    "    Mph = np.matrix([[cos(ph), 0, -sin(ph)], [0, 1, 0], [sin(ph), 0, cos(ph)]])\n",
    "    Mkp = np.matrix([[cos(kp), sin(kp), 0], [-sin(kp), cos(kp), 0], [0, 0, 1]])\n",
    "\n",
    "    M = Mkp * Mph * Mom\n",
    "\n",
    "    uvw = M * np.matrix([[X-XL], [Y-YL], [Z-ZL]])\n",
    "\n",
    "    resx = x - x0 + focallength * uvw[0,0] / uvw[2,0]\n",
    "    resy = y - y0 + focallength * uvw[1,0] / uvw[2,0]\n",
    "\n",
    "    return resx, resy\n",
    "\n",
    "\n",
    "class CollinearityData:\n",
    "    \"\"\"\n",
    "    class to store data for the collinearity equations\n",
    "    \"\"\"\n",
    "    def __init__(self, camera_file, point_file):\n",
    "        \"\"\"\n",
    "        initilizes data for collinearity equations\n",
    "\n",
    "        reads camera parameters from camera_file\n",
    "        reads control point data from point_file\n",
    "        \"\"\"\n",
    "\n",
    "        f = open(camera_file,'r')\n",
    "        dat = np.loadtxt(f,float)\n",
    "        f.close\n",
    "\n",
    "        self.eop = {}\n",
    "\n",
    "        # data from lines 1-3 of the camera_file\n",
    "        self.eop['omega'] = dat[0]\n",
    "        self.eop['phi'] = dat[1]\n",
    "        self.eop['kappa'] = dat[2]\n",
    "\n",
    "        # data from lines 4-6 of the camera_file\n",
    "        self.eop['XL'] = dat[3]\n",
    "        self.eop['YL'] = dat[4]\n",
    "        self.eop['ZL'] = dat[5]\n",
    "\n",
    "        self.iop = {}\n",
    "\n",
    "        # data from lines 7-9 of the camera_file\n",
    "        self.iop['x0'] = dat[6]\n",
    "        self.iop['y0'] = dat[7]\n",
    "        self.iop['f'] = dat[8]\n",
    "\n",
    "        self.label = []\n",
    "        x = []\n",
    "        y = []\n",
    "        X = []\n",
    "        Y = []\n",
    "        Z = []\n",
    "\n",
    "        f = open(point_file,'r')\n",
    "        for line in f:\n",
    "            l = line.split()\n",
    "            # each line has 6 values: label, x, y, X, Y, Z (whitespace delimited)\n",
    "            self.label.append(l[0])\n",
    "            x.append(float(l[1]))\n",
    "            y.append(float(l[2]))\n",
    "            X.append(float(l[3]))\n",
    "            Y.append(float(l[4]))\n",
    "            Z.append(float(l[5]))\n",
    "        f.close\n",
    "\n",
    "        self.x = np.array(x)\n",
    "        self.y = np.array(y)\n",
    "        self.X = np.array(X)\n",
    "        self.Y = np.array(Y)\n",
    "        self.Z = np.array(Z)\n",
    "\n",
    "\n",
    "\n",
    "def coll_func(indep_vars):\n",
    "    \"\"\"\n",
    "    collinearity function calculates a sum of the squared residuals of the\n",
    "        collinearity equations for all of the control points\n",
    "    This function is passed to scipy.optimize.minimize()\n",
    "\n",
    "    Inputs:\n",
    "        indep_vars (passed) are the exterior orientation parameters of the camera\n",
    "        data (global) camera interior calibration data, photo points, control points\n",
    "\n",
    "    Returns:\n",
    "        sum of squared residuals of collinearity eqns\n",
    "    \"\"\"\n",
    "    global data\n",
    "    iop = data.iop\n",
    "    #eop = data.eop\n",
    "    label = data.label\n",
    "    x = data.x\n",
    "    y = data.y\n",
    "    X = data.X\n",
    "    Y = data.Y\n",
    "    Z = data.Z\n",
    "\n",
    "    eop = {}\n",
    "    eop['omega'] = indep_vars[0]\n",
    "    eop['phi'] = indep_vars[1]\n",
    "    eop['kappa'] = indep_vars[2]\n",
    "    eop['XL'] = indep_vars[3]\n",
    "    eop['YL'] = indep_vars[4]\n",
    "    eop['ZL'] = indep_vars[5]\n",
    "\n",
    "    i = 0\n",
    "    F = 0.0\n",
    "    for l in label:\n",
    "\n",
    "        F1, F2 = collinearity_eqn_residual(iop,eop,x[i],y[i],X[i],Y[i],Z[i])\n",
    "        F += F1**2 + F2**2\n",
    "        i += 1\n",
    "\n",
    "    return F\n",
    "\n",
    "camera_file = 'cam.inp'\n",
    "point_file = 'resect.inp'\n",
    "\n",
    "data = CollinearityData(camera_file, point_file)\n",
    "\n",
    "x0 = np.zeros(6)\n",
    "# initilaize guesses for eop as read from file\n",
    "eop = data.eop\n",
    "x0[0] = eop['omega']\n",
    "x0[1] = eop['phi']\n",
    "x0[2] = eop['kappa']\n",
    "x0[3] = eop['XL']\n",
    "x0[4] = eop['YL']\n",
    "x0[5] = eop['ZL']\n",
    "\n",
    "res = minimize(coll_func, x0, method='BFGS', options={'disp': True})\n",
    "\n",
    "#print res.x\n",
    "print ('Solution:')\n",
    "print ('omega, ', res.x[0])\n",
    "print ('phi, ', res.x[1])\n",
    "print ('kappa, ', res.x[2])\n",
    "print ('XL, ', res.x[3])\n",
    "print ('YL, ', res.x[4])\n",
    "print ('ZL, ', res.x[5])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## resection_leastsq.py\n",
    "using scipy leastsq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Solution:\n",
      "omega,  -0.006507481143624354\n",
      "phi,  -0.008521803654879007\n",
      "kappa,  -1.5753221236896509\n",
      "XL,  914260.4218627542\n",
      "YL,  575441.8355519563\n",
      "ZL,  839.1304372341308\n",
      "number of function evaluations:  29\n",
      "sum squared residuals:  0.000751104878973308\n"
     ]
    }
   ],
   "source": [
    "# Author:  Jeffrey T. Walton, Paul Smith's College, New York\n",
    "#\n",
    "#   Single-photo resection - calculates the camera orientation and location\n",
    "#       given camera calibration parameters, control point photo and world\n",
    "#        coordinates and initial guesses for camera exterior orientation.\n",
    "#\n",
    "#   based on MATLAB code from:\n",
    "#   Introduction to Modern Photogrammetry by Mikhail, Bethel, McGlone\n",
    "#   John Wiley & Sons, Inc. 2001\n",
    "\n",
    "import sys\n",
    "import numpy as np\n",
    "from scipy.optimize import leastsq\n",
    "\n",
    "\n",
    "def collinearity_eqn_residual(iop,eop,x,y,X,Y,Z):\n",
    "    \"\"\"\n",
    "    Usage:\n",
    "        collinearity_eqn_residual(iop,eop,x,y,X,Y,Z)\n",
    "\n",
    "    Inputs:\n",
    "        iop = dict of interior orientation parameters: x0, y0, f\n",
    "        eop = dict of exterior orientation parameters: omega, phi, kappa, XL, YL, ZL\n",
    "        x = array of x photo coordinates of control points\n",
    "        y = array of y photo coordinates of control points\n",
    "        X = array of X world coordinates of control points\n",
    "        Y = array of Y world coordinates of control points\n",
    "        Z = array of Z world coordinates of control points\n",
    "\n",
    "    Returns:\n",
    "        residuals in x and y collinearity equations for a single point as a tuple\n",
    "    \"\"\"\n",
    "    from math import sin, cos\n",
    "    x0 = iop['x0']\n",
    "    y0 = iop['y0']\n",
    "    focallength = iop['f']\n",
    "\n",
    "    om = eop['omega']\n",
    "    ph = eop['phi']\n",
    "    kp = eop['kappa']\n",
    "\n",
    "    XL = eop['XL']\n",
    "    YL = eop['YL']\n",
    "    ZL = eop['ZL']\n",
    "\n",
    "    Mom = np.matrix([[1, 0, 0], [0, cos(om), sin(om)], [0, -sin(om), cos(om)]])\n",
    "    Mph = np.matrix([[cos(ph), 0, -sin(ph)], [0, 1, 0], [sin(ph), 0, cos(ph)]])\n",
    "    Mkp = np.matrix([[cos(kp), sin(kp), 0], [-sin(kp), cos(kp), 0], [0, 0, 1]])\n",
    "\n",
    "    M = Mkp * Mph * Mom\n",
    "\n",
    "    uvw = M * np.matrix([[X-XL], [Y-YL], [Z-ZL]])\n",
    "\n",
    "    resx = x - x0 + focallength * uvw[0,0] / uvw[2,0]\n",
    "    resy = y - y0 + focallength * uvw[1,0] / uvw[2,0]\n",
    "\n",
    "    return resx, resy\n",
    "\n",
    "\n",
    "class CollinearityData:\n",
    "    \"\"\"\n",
    "    class to store data for the collinearity equations\n",
    "    \"\"\"\n",
    "    def __init__(self, camera_file, point_file):\n",
    "        \"\"\"\n",
    "        initilizes data for collinearity equations\n",
    "\n",
    "        reads camera parameters from camera_file\n",
    "        reads control point data from point_file\n",
    "        \"\"\"\n",
    "\n",
    "        f = open(camera_file,'r')\n",
    "        dat = np.loadtxt(f,float)\n",
    "        f.close\n",
    "\n",
    "        self.eop = {}\n",
    "\n",
    "        # data from lines 1-3 of the camera_file\n",
    "        self.eop['omega'] = dat[0]\n",
    "        self.eop['phi'] = dat[1]\n",
    "        self.eop['kappa'] = dat[2]\n",
    "\n",
    "        # data from lines 4-6 of the camera_file\n",
    "        self.eop['XL'] = dat[3]\n",
    "        self.eop['YL'] = dat[4]\n",
    "        self.eop['ZL'] = dat[5]\n",
    "\n",
    "        self.iop = {}\n",
    "\n",
    "        # data from lines 7-9 of the camera_file\n",
    "        self.iop['x0'] = dat[6]\n",
    "        self.iop['y0'] = dat[7]\n",
    "        self.iop['f'] = dat[8]\n",
    "\n",
    "        self.label = []\n",
    "        x = []\n",
    "        y = []\n",
    "        X = []\n",
    "        Y = []\n",
    "        Z = []\n",
    "\n",
    "        f = open(point_file,'r')\n",
    "        for line in f:\n",
    "            l = line.split()\n",
    "            # each line has 6 values: label, x, y, X, Y, Z (whitespace delimited)\n",
    "            self.label.append(l[0])\n",
    "            x.append(float(l[1]))\n",
    "            y.append(float(l[2]))\n",
    "            X.append(float(l[3]))\n",
    "            Y.append(float(l[4]))\n",
    "            Z.append(float(l[5]))\n",
    "        f.close\n",
    "\n",
    "        self.x = np.array(x)\n",
    "        self.y = np.array(y)\n",
    "        self.X = np.array(X)\n",
    "        self.Y = np.array(Y)\n",
    "        self.Z = np.array(Z)\n",
    "\n",
    "\n",
    "\n",
    "def coll_func(indep_vars):\n",
    "    \"\"\"\n",
    "    collinearity function calculates a sum of the squared residuals of the\n",
    "        collinearity equations for all of the control points\n",
    "    This function is passed to scipy.optimize.minimize()\n",
    "\n",
    "    Inputs:\n",
    "        indep_vars (passed) are the exterior orientation parameters of the camera\n",
    "        data (global) camera interior calibration data, photo points, control points\n",
    "\n",
    "    Returns:\n",
    "        sum of squared residuals of collinearity eqns\n",
    "    \"\"\"\n",
    "    global data\n",
    "    iop = data.iop\n",
    "    #eop = data.eop\n",
    "    label = data.label\n",
    "    x = data.x\n",
    "    y = data.y\n",
    "    X = data.X\n",
    "    Y = data.Y\n",
    "    Z = data.Z\n",
    "\n",
    "    eop = {}\n",
    "    eop['omega'] = indep_vars[0]\n",
    "    eop['phi'] = indep_vars[1]\n",
    "    eop['kappa'] = indep_vars[2]\n",
    "    eop['XL'] = indep_vars[3]\n",
    "    eop['YL'] = indep_vars[4]\n",
    "    eop['ZL'] = indep_vars[5]\n",
    "\n",
    "    i = 0\n",
    "    F = np.zeros(2*len(label))\n",
    "    for l in label:\n",
    "\n",
    "        F[2*i], F[2*i+1] = collinearity_eqn_residual(iop,eop,x[i],y[i],X[i],Y[i],Z[i])\n",
    "        i += 1\n",
    "\n",
    "    return F\n",
    "\n",
    "\n",
    "camera_file = 'cam.inp'\n",
    "point_file = 'resect.inp'\n",
    "\n",
    "data = CollinearityData(camera_file, point_file)\n",
    "\n",
    "x0 = np.zeros(6)\n",
    "# initilaize guesses for eop as read from file\n",
    "eop = data.eop\n",
    "x0[0] = eop['omega']\n",
    "x0[1] = eop['phi']\n",
    "x0[2] = eop['kappa']\n",
    "x0[3] = eop['XL']\n",
    "x0[4] = eop['YL']\n",
    "x0[5] = eop['ZL']\n",
    "\n",
    "x, cov_x, info, msg, ier = leastsq(coll_func, x0, full_output=True)\n",
    "\n",
    "print ('Solution:')\n",
    "print ('omega, ', x[0])\n",
    "print ('phi, ', x[1])\n",
    "print ('kappa, ', x[2])\n",
    "print ('XL, ', x[3])\n",
    "print ('YL, ', x[4])\n",
    "print ('ZL, ', x[5])\n",
    "print ('number of function evaluations: ', info['nfev'])\n",
    "print ('sum squared residuals: ', np.sum(info['fvec']**2))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## resection_leastsq_Dfun.py\n",
    "using scipy leastsq\n",
    "incorporating derivative functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Solution:\n",
      "omega,  -0.006507481065393404\n",
      "phi,  -0.008521803480548321\n",
      "kappa,  -1.5753221236972152\n",
      "XL,  914260.4218628866\n",
      "YL,  575441.8355519054\n",
      "ZL,  839.1304372813759\n",
      "number of function evaluations:  5\n",
      "sum squared residuals:  0.0007511048789732501\n"
     ]
    }
   ],
   "source": [
    "# Author:  Jeffrey T. Walton, Paul Smith's College, New York\n",
    "#\n",
    "#   Single-photo resection - calculates the camera orientation and location\n",
    "#       given camera calibration parameters, control point photo and world\n",
    "#        coordinates and initial guesses for camera exterior orientation.\n",
    "#\n",
    "#   based on MATLAB code from:\n",
    "#   Introduction to Modern Photogrammetry by Mikhail, Bethel, McGlone\n",
    "#   John Wiley & Sons, Inc. 2001\n",
    "\n",
    "import sys\n",
    "import numpy as np\n",
    "from scipy.optimize import leastsq\n",
    "\n",
    "\n",
    "def collinearity_eqn_residual(iop,eop,x,y,X,Y,Z):\n",
    "    \"\"\"\n",
    "    Usage:\n",
    "        collinearity_eqn_residual(iop,eop,x,y,X,Y,Z)\n",
    "\n",
    "    Inputs:\n",
    "        iop = dict of interior orientation parameters: x0, y0, f\n",
    "        eop = dict of exterior orientation parameters: omega, phi, kappa, XL, YL, ZL\n",
    "        x = array of x photo coordinates of control points\n",
    "        y = array of y photo coordinates of control points\n",
    "        X = array of X world coordinates of control points\n",
    "        Y = array of Y world coordinates of control points\n",
    "        Z = array of Z world coordinates of control points\n",
    "\n",
    "    Returns:\n",
    "        residuals in x and y collinearity equations for a single point as a tuple\n",
    "    \"\"\"\n",
    "    from math import sin, cos\n",
    "    x0 = iop['x0']\n",
    "    y0 = iop['y0']\n",
    "    focallength = iop['f']\n",
    "\n",
    "    om = eop['omega']\n",
    "    ph = eop['phi']\n",
    "    kp = eop['kappa']\n",
    "\n",
    "    XL = eop['XL']\n",
    "    YL = eop['YL']\n",
    "    ZL = eop['ZL']\n",
    "\n",
    "    Mom = np.matrix([[1, 0, 0], [0, cos(om), sin(om)], [0, -sin(om), cos(om)]])\n",
    "    Mph = np.matrix([[cos(ph), 0, -sin(ph)], [0, 1, 0], [sin(ph), 0, cos(ph)]])\n",
    "    Mkp = np.matrix([[cos(kp), sin(kp), 0], [-sin(kp), cos(kp), 0], [0, 0, 1]])\n",
    "\n",
    "    M = Mkp * Mph * Mom\n",
    "\n",
    "    uvw = M * np.matrix([[X-XL], [Y-YL], [Z-ZL]])\n",
    "\n",
    "    resx = x - x0 + focallength * uvw[0,0] / uvw[2,0]\n",
    "    resy = y - y0 + focallength * uvw[1,0] / uvw[2,0]\n",
    "\n",
    "    return resx, resy\n",
    "\n",
    "def collinearity_eqn_residual_Jacobian(iop,eop,x,y,X,Y,Z):\n",
    "    \"\"\"\n",
    "    Usage:\n",
    "        collinearity_eqn_residual_Jacobian(iop,eop,x,y,X,Y,Z)\n",
    "\n",
    "    Inputs:\n",
    "        iop = dict of interior orientation parameters: x0, y0, f\n",
    "        eop = dict of exterior orientation parameters: omega, phi, kappa, XL, YL, ZL\n",
    "        x = array of x photo coordinates of control points\n",
    "        y = array of y photo coordinates of control points\n",
    "        X = array of X world coordinates of control points\n",
    "        Y = array of Y world coordinates of control points\n",
    "        Z = array of Z world coordinates of control points\n",
    "\n",
    "    Returns:\n",
    "        the Jacobian of the collinearity equations\n",
    "        a (2, 6) matrix of partial derivatives of x and y wrt om, ph, kp\n",
    "    \"\"\"\n",
    "    from math import sin, cos\n",
    "    #x0 = iop['x0']\n",
    "    #y0 = iop['y0']\n",
    "    focallength = iop['f']\n",
    "\n",
    "    om = eop['omega']\n",
    "    ph = eop['phi']\n",
    "    kp = eop['kappa']\n",
    "\n",
    "    XL = eop['XL']\n",
    "    YL = eop['YL']\n",
    "    ZL = eop['ZL']\n",
    "\n",
    "    # Appendix C, Mikhail et al.\n",
    "\n",
    "    Mom = np.matrix([[1, 0, 0], [0, cos(om), sin(om)], [0, -sin(om), cos(om)]])\n",
    "    Mph = np.matrix([[cos(ph), 0, -sin(ph)], [0, 1, 0], [sin(ph), 0, cos(ph)]])\n",
    "    Mkp = np.matrix([[cos(kp), sin(kp), 0], [-sin(kp), cos(kp), 0], [0, 0, 1]])\n",
    "\n",
    "    M = Mkp * Mph * Mom\n",
    "\n",
    "    UVW = M * np.matrix([[X-XL], [Y-YL], [Z-ZL]])\n",
    "    \n",
    "    U = UVW[0,0]\n",
    "    V = UVW[1,0]\n",
    "    W = UVW[2,0]\n",
    "\n",
    "    jacobian = np.zeros((2,6))\n",
    "\n",
    "    dUVW_dom = M * np.matrix([[0.0], [Z-ZL], [YL-Y]])\n",
    "    dUVW_dph = np.matrix([[0, 0, -cos(kp)], [0, 0, sin(kp)], [cos(kp), -sin(kp), 0]]) * UVW\n",
    "    dUVW_dkp = np.matrix([[V], [-U], [0.0]])\n",
    "\n",
    "    dUVW_dXL = M * np.matrix([[-1.0],  [0.0],  [0.0]])\n",
    "    dUVW_dYL = M * np.matrix([[ 0.0], [-1.0],  [0.0]])\n",
    "    dUVW_dZL = M * np.matrix([[ 0.0], [ 0.0], [-1.0]])\n",
    "    \n",
    "    f_W = focallength / W\n",
    "    \n",
    "    jacobian[0,0] = f_W *(dUVW_dom[0,0]-U/W*dUVW_dom[2,0])\n",
    "    jacobian[0,1] = f_W *(dUVW_dph[0,0]-U/W*dUVW_dph[2,0])\n",
    "    jacobian[0,2] = f_W *(dUVW_dkp[0,0]-U/W*dUVW_dkp[2,0])\n",
    "    jacobian[0,3] = f_W *(dUVW_dXL[0,0]-U/W*dUVW_dXL[2,0])\n",
    "    jacobian[0,4] = f_W *(dUVW_dYL[0,0]-U/W*dUVW_dYL[2,0])\n",
    "    jacobian[0,5] = f_W *(dUVW_dZL[0,0]-U/W*dUVW_dZL[2,0])\n",
    "    jacobian[1,0] = f_W *(dUVW_dom[1,0]-V/W*dUVW_dom[2,0])\n",
    "    jacobian[1,1] = f_W *(dUVW_dph[1,0]-V/W*dUVW_dph[2,0])\n",
    "    jacobian[1,2] = f_W *(dUVW_dkp[1,0]-V/W*dUVW_dkp[2,0])\n",
    "    jacobian[1,3] = f_W *(dUVW_dXL[1,0]-V/W*dUVW_dXL[2,0])\n",
    "    jacobian[1,4] = f_W *(dUVW_dYL[1,0]-V/W*dUVW_dYL[2,0])\n",
    "    jacobian[1,5] = f_W *(dUVW_dZL[1,0]-V/W*dUVW_dZL[2,0])\n",
    "    \n",
    "    return jacobian\n",
    "\n",
    "\n",
    "class CollinearityData:\n",
    "    \"\"\"\n",
    "    class to store data for the collinearity equations\n",
    "    \"\"\"\n",
    "    def __init__(self, camera_file, point_file):\n",
    "        \"\"\"\n",
    "        initilizes data for collinearity equations\n",
    "\n",
    "        reads camera parameters from camera_file\n",
    "        reads control point data from point_file\n",
    "        \"\"\"\n",
    "\n",
    "        f = open(camera_file,'r')\n",
    "        dat = np.loadtxt(f,float)\n",
    "        f.close\n",
    "\n",
    "        self.eop = {}\n",
    "\n",
    "        # data from lines 1-3 of the camera_file\n",
    "        self.eop['omega'] = dat[0]\n",
    "        self.eop['phi'] = dat[1]\n",
    "        self.eop['kappa'] = dat[2]\n",
    "\n",
    "        # data from lines 4-6 of the camera_file\n",
    "        self.eop['XL'] = dat[3]\n",
    "        self.eop['YL'] = dat[4]\n",
    "        self.eop['ZL'] = dat[5]\n",
    "\n",
    "        self.iop = {}\n",
    "\n",
    "        # data from lines 7-9 of the camera_file\n",
    "        self.iop['x0'] = dat[6]\n",
    "        self.iop['y0'] = dat[7]\n",
    "        self.iop['f'] = dat[8]\n",
    "\n",
    "        self.label = []\n",
    "        x = []\n",
    "        y = []\n",
    "        X = []\n",
    "        Y = []\n",
    "        Z = []\n",
    "\n",
    "        f = open(point_file,'r')\n",
    "        for line in f:\n",
    "            l = line.split()\n",
    "            # each line has 6 values: label, x, y, X, Y, Z (whitespace delimited)\n",
    "            self.label.append(l[0])\n",
    "            x.append(float(l[1]))\n",
    "            y.append(float(l[2]))\n",
    "            X.append(float(l[3]))\n",
    "            Y.append(float(l[4]))\n",
    "            Z.append(float(l[5]))\n",
    "        f.close\n",
    "\n",
    "        self.x = np.array(x)\n",
    "        self.y = np.array(y)\n",
    "        self.X = np.array(X)\n",
    "        self.Y = np.array(Y)\n",
    "        self.Z = np.array(Z)\n",
    "\n",
    "\n",
    "\n",
    "def coll_func(indep_vars):\n",
    "    \"\"\"\n",
    "    collinearity function calculates a sum of the squared residuals of the\n",
    "        collinearity equations for all of the control points\n",
    "    This function is passed to scipy.optimize.minimize()\n",
    "\n",
    "    Inputs:\n",
    "        indep_vars (passed) are the exterior orientation parameters of the camera\n",
    "        data (global) camera interior calibration data, photo points, control points\n",
    "\n",
    "    Returns:\n",
    "        sum of squared residuals of collinearity eqns\n",
    "    \"\"\"\n",
    "    global data\n",
    "    iop = data.iop\n",
    "    #eop = data.eop\n",
    "    label = data.label\n",
    "    x = data.x\n",
    "    y = data.y\n",
    "    X = data.X\n",
    "    Y = data.Y\n",
    "    Z = data.Z\n",
    "\n",
    "    eop = {}\n",
    "    eop['omega'] = indep_vars[0]\n",
    "    eop['phi'] = indep_vars[1]\n",
    "    eop['kappa'] = indep_vars[2]\n",
    "    eop['XL'] = indep_vars[3]\n",
    "    eop['YL'] = indep_vars[4]\n",
    "    eop['ZL'] = indep_vars[5]\n",
    "\n",
    "    i = 0\n",
    "    F = np.zeros(2*len(label))\n",
    "    for l in label:\n",
    "\n",
    "        F[2*i], F[2*i+1] = collinearity_eqn_residual(iop,eop,x[i],y[i],X[i],Y[i],Z[i])\n",
    "        i += 1\n",
    "\n",
    "    return F\n",
    "\n",
    "\n",
    "def coll_Dfunc(indep_vars):\n",
    "    \"\"\"\n",
    "    The Jacobian of the collinearity function calculates rate of change of the \n",
    "        residuals of the collinearity equations wrt the indep_vars (eop)\n",
    "    This function is passed to scipy.optimize.minimize()\n",
    "\n",
    "    Inputs:\n",
    "        indep_vars (passed) are the exterior orientation parameters of the camera\n",
    "        data (global) camera interior calibration data, photo points, control points\n",
    "\n",
    "    Returns:\n",
    "        Jacobian (first derivative) matrix\n",
    "    \"\"\"\n",
    "    global data\n",
    "    iop = data.iop\n",
    "    #eop = data.eop\n",
    "    label = data.label\n",
    "    x = data.x\n",
    "    y = data.y\n",
    "    X = data.X\n",
    "    Y = data.Y\n",
    "    Z = data.Z\n",
    "\n",
    "    eop = {}\n",
    "    eop['omega'] = indep_vars[0]\n",
    "    eop['phi'] = indep_vars[1]\n",
    "    eop['kappa'] = indep_vars[2]\n",
    "    eop['XL'] = indep_vars[3]\n",
    "    eop['YL'] = indep_vars[4]\n",
    "    eop['ZL'] = indep_vars[5]\n",
    "\n",
    "    i = 0\n",
    "    dF = np.zeros((2*len(label), len(indep_vars)))\n",
    "    for l in label:\n",
    "        dF[2*i:2*i+2,0:] = collinearity_eqn_residual_Jacobian(iop,eop,x[i],y[i],X[i],Y[i],Z[i])\n",
    "        i += 1\n",
    "\n",
    "    return dF\n",
    "\n",
    "\n",
    "camera_file = 'cam.inp'\n",
    "point_file = 'resect.inp'\n",
    "\n",
    "data = CollinearityData(camera_file, point_file)\n",
    "\n",
    "x0 = np.zeros(6)\n",
    "# initilaize guesses for eop as read from file\n",
    "eop = data.eop\n",
    "x0[0] = eop['omega']\n",
    "x0[1] = eop['phi']\n",
    "x0[2] = eop['kappa']\n",
    "x0[3] = eop['XL']\n",
    "x0[4] = eop['YL']\n",
    "x0[5] = eop['ZL']\n",
    "\n",
    "#x, cov_x, info, msg, ier = leastsq(coll_func, x0, full_output=True)\n",
    "x, cov_x, info, msg, ier = leastsq(coll_func, x0, Dfun=coll_Dfunc, full_output=True)\n",
    "\n",
    "print ('Solution:')\n",
    "print ('omega, ', x[0])\n",
    "print ('phi, ', x[1])\n",
    "print ('kappa, ', x[2])\n",
    "print ('XL, ', x[3])\n",
    "print ('YL, ', x[4])\n",
    "print ('ZL, ', x[5])\n",
    "print ('number of function evaluations: ', info['nfev'])\n",
    "print ('sum squared residuals: ', np.sum(info['fvec']**2))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}