{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pysis import isis\n",
    "\n",
    "from plio.io import io_controlnetwork\n",
    "from knoten.csm import create_csm\n",
    "from scipy import sparse\n",
    "import ale\n",
    "import csmapi\n",
    "import numpy as np\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from knoten.bundle import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Load Network and Generate Sensors\n",
    "- Generate a set of USGSCSM sensor models from a list of ISIS cube files\n",
    "- Generate a plio dataframe from an ISIS control network\n",
    "- Compute a priori ground points for all of the free points in a control network."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "cubes = 'data/cubes.lis'\n",
    "sensors = generate_sensors(cubes)\n",
    "\n",
    "network = 'data/hand_dense.net'\n",
    "cnet = io_controlnetwork.from_isis(network)\n",
    "cnet = compute_apriori_ground_points(cnet, sensors) # autoseed did not generate ground points, calculate and repopulate the data frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Determine Which Sensor Parameters to Solve For\n",
    "Get a set of the CSM parameters for each CSM sensor in the sensors set"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Image: MRO/CTX/1085197697:073\n",
      "  IT Pos. Bias    | 0 | 0.0\n",
      "  CT Pos. Bias    | 1 | 0.0\n",
      "  Rad Pos. Bias   | 2 | 0.0\n",
      "  IT Vel. Bias    | 3 | 0.0\n",
      "  CT Vel. Bias    | 4 | 0.0\n",
      "  Rad Vel. Bias   | 5 | 0.0\n",
      "  Omega Bias      | 6 | 0.0\n",
      "  Phi Bias        | 7 | 0.0\n",
      "  Kappa Bias      | 8 | 0.0\n",
      "  Omega Rate      | 9 | 0.0\n",
      "  Phi Rate        | 10 | 0.0\n",
      "  Kappa Rate      | 11 | 0.0\n",
      "  Omega Accl      | 12 | 0.0\n",
      "  Phi Accl        | 13 | 0.0\n",
      "  Kappa Accl      | 14 | 0.0\n",
      "  Focal Bias      | 15 | 0.0\n",
      "Image: MRO/CTX/1096561308:045\n",
      "  IT Pos. Bias    | 0 | 0.0\n",
      "  CT Pos. Bias    | 1 | 0.0\n",
      "  Rad Pos. Bias   | 2 | 0.0\n",
      "  IT Vel. Bias    | 3 | 0.0\n",
      "  CT Vel. Bias    | 4 | 0.0\n",
      "  Rad Vel. Bias   | 5 | 0.0\n",
      "  Omega Bias      | 6 | 0.0\n",
      "  Phi Bias        | 7 | 0.0\n",
      "  Kappa Bias      | 8 | 0.0\n",
      "  Omega Rate      | 9 | 0.0\n",
      "  Phi Rate        | 10 | 0.0\n",
      "  Kappa Rate      | 11 | 0.0\n",
      "  Omega Accl      | 12 | 0.0\n",
      "  Phi Accl        | 13 | 0.0\n",
      "  Kappa Accl      | 14 | 0.0\n",
      "  Focal Bias      | 15 | 0.0\n",
      "Image: MRO/CTX/1136952576:186\n",
      "  IT Pos. Bias    | 0 | 0.0\n",
      "  CT Pos. Bias    | 1 | 0.0\n",
      "  Rad Pos. Bias   | 2 | 0.0\n",
      "  IT Vel. Bias    | 3 | 0.0\n",
      "  CT Vel. Bias    | 4 | 0.0\n",
      "  Rad Vel. Bias   | 5 | 0.0\n",
      "  Omega Bias      | 6 | 0.0\n",
      "  Phi Bias        | 7 | 0.0\n",
      "  Kappa Bias      | 8 | 0.0\n",
      "  Omega Rate      | 9 | 0.0\n",
      "  Phi Rate        | 10 | 0.0\n",
      "  Kappa Rate      | 11 | 0.0\n",
      "  Omega Accl      | 12 | 0.0\n",
      "  Phi Accl        | 13 | 0.0\n",
      "  Kappa Accl      | 14 | 0.0\n",
      "  Focal Bias      | 15 | 0.0\n",
      "Image: MRO/CTX/1157902986:250\n",
      "  IT Pos. Bias    | 0 | 0.0\n",
      "  CT Pos. Bias    | 1 | 0.0\n",
      "  Rad Pos. Bias   | 2 | 0.0\n",
      "  IT Vel. Bias    | 3 | 0.0\n",
      "  CT Vel. Bias    | 4 | 0.0\n",
      "  Rad Vel. Bias   | 5 | 0.0\n",
      "  Omega Bias      | 6 | 0.0\n",
      "  Phi Bias        | 7 | 0.0\n",
      "  Kappa Bias      | 8 | 0.0\n",
      "  Omega Rate      | 9 | 0.0\n",
      "  Phi Rate        | 10 | 0.0\n",
      "  Kappa Rate      | 11 | 0.0\n",
      "  Omega Accl      | 12 | 0.0\n",
      "  Phi Accl        | 13 | 0.0\n",
      "  Kappa Accl      | 14 | 0.0\n",
      "  Focal Bias      | 15 | 0.0\n"
     ]
    }
   ],
   "source": [
    "all_parameters = {sn: get_sensor_parameters(sensor) for sn, sensor in sensors.items()}\n",
    "for sn, parameters in all_parameters.items():\n",
    "    print(f\"Image: {sn}\")\n",
    "    for param in parameters:\n",
    "        print(f\"  {param.name} | {param.index} | {param.value}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Solve for angles and angular rates\n",
    "solve_parameters = {sn: params[6:12] for sn, params in all_parameters.items()}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Compute the Column Indices for Parameters\n",
    "Obtain dictionary that maps serial numbers and point IDs to the column range their parameters are in the Jacobian matrix."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "column_dict = compute_coefficient_columns(cnet, sensors, solve_parameters)\n",
    "# num_parameters = max(col_range[1] for col_range in column_dict.values())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "source": [
    "## Compute the Weight Matrix\n",
    "- According to the weighted Normal equation (J.TWJ), W needs to be a square matrix the size of (# of measures)x2. So it is the weight of the observations. In ISIS, the weight of the observations are an inverted function of the size of the pixels on the focal plane (resolution). However, in csm we do not have access to that information. \n",
    "- For the time being, since we are working exclusively with CTX images we are going to set the weight matrix equal to the identity matrix -> all observations have the same weight."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_observations = 2 * len(cnet)\n",
    "W_observations = np.eye(num_observations) # this is a place holder until Jesse adds his calculations\n",
    "W_params = compute_parameter_weights(cnet, sensors, solve_parameters, column_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculate Initial Sigma0\n",
    "Compute the resulting standard deviation of the residuals for the current state of the bundle network."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2.5961263976217976\n"
     ]
    }
   ],
   "source": [
    "V = compute_residuals(cnet, sensors)\n",
    "dX = np.zeros(W_params.shape[0])\n",
    "sigma0 = compute_sigma0(V, dX, W_params, W_observations)\n",
    "\n",
    "print((sigma0))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Populate Jacobian\n",
    "Compute the Jacobian matrix with controlnet, set of sensors, solve parameters, and coefficient columns"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "J = compute_jacobian(cnet, sensors, solve_parameters, column_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bundle Iteration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def bundle_iteration(J, V, W_parameters, W_observations):\n",
    "    \"\"\"\n",
    "    Parameters\n",
    "    ----------\n",
    "    J  :  ndarray\n",
    "          The Jacobian matrix\n",
    "    V  :  np.array\n",
    "          An array of residuals of the difference between registered measure \n",
    "          and back projected ground points in image space.\n",
    "    W_parameters  :  ndarray \n",
    "                     The parameter weight matrix (i.e.: sensor parameters and point weights)\n",
    "    W_observations  :  ndarray\n",
    "                     The observation weight matrix (i.e.: measure weights)\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "    N  :  np.ndarray\n",
    "          Normal equation matrix \n",
    "    \n",
    "    dX  :  np.ndarray\n",
    "           An array of updated parameter values\n",
    "    \"\"\"\n",
    "    \n",
    "    N = J.T.dot(W_observations).dot(J) + W_parameters\n",
    "    C = J.T.dot(W_observations).dot(V)\n",
    "    dX = np.linalg.inv(N).dot(C)\n",
    "    return N, dX"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(129,)\n"
     ]
    }
   ],
   "source": [
    "N, dX = bundle_iteration(J, V, W_params, W_observations)\n",
    "print(dX.shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculate Updated Sigma0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.3577841505153914\n"
     ]
    }
   ],
   "source": [
    "dof = W_observations.shape[0] - W_params.shape[0]\n",
    "VTPV = V.dot(W_observations).dot(V) - dX.dot(J.T).dot(W_observations).dot(V)\n",
    "if (dof > 0):\n",
    "    updated_sigma0 = np.sqrt(VTPV/dof)\n",
    "else:\n",
    "    updated_sigma0 = np.sqrt(VTPV)\n",
    "print(updated_sigma0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Redundancy Number"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Minimum redundancy: 0.7502088509294791\n",
      "Maximum redundancy: 0.9653389403009147\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "{'whiskers': [<matplotlib.lines.Line2D at 0x1ad6cea60>,\n",
       "  <matplotlib.lines.Line2D at 0x1ad6ced00>],\n",
       " 'caps': [<matplotlib.lines.Line2D at 0x1ad6cefa0>,\n",
       "  <matplotlib.lines.Line2D at 0x1ad6ec280>],\n",
       " 'boxes': [<matplotlib.lines.Line2D at 0x1ad6ce7c0>],\n",
       " 'medians': [<matplotlib.lines.Line2D at 0x1ad6ec520>],\n",
       " 'fliers': [<matplotlib.lines.Line2D at 0x1ad6ec7c0>],\n",
       " 'means': []}"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 13132 characters]",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# redundancy for every measure\n",
    "# vector will hold same order as the measures in the cnet df\n",
    "# def compute_measure_redundancy\n",
    "def compute_redundancy(N, W_observations, J):\n",
    "    Qxx = np.linalg.inv(N)\n",
    "    Qvv = np.linalg.inv(W_observations) - J.dot(Qxx).dot(J.T)\n",
    "    r = np.diagonal(Qvv.dot(W_observations))\n",
    "    \n",
    "    return r\n",
    "\n",
    "r = compute_redundancy(N, W_observations, J)\n",
    "print(f'Minimum redundancy: {min(r)}')\n",
    "print(f'Maximum redundancy: {max(r)}')\n",
    "plt.boxplot(r)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Whole bundle process in a loop"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "iteration 0: sigma0 = 4.0255575933213645\n",
      "\n",
      "corrections: mean = 0.0607625027080373 min = -15.894645144585613 max = 43.173076576185686\n",
      "iteration 1: sigma0 = 1.3372395601830835\n",
      "\n",
      "corrections: mean = -1.9487862295781398e-05 min = -0.002348815201592963 max = 0.0011802360614414334\n",
      "iteration 2: sigma0 = 1.116733991093145\n",
      "\n",
      "corrections: mean = -6.40338937723717e-10 min = -5.349525439150892e-08 max = 4.348174825756319e-08\n",
      "iteration 3: sigma0 = 1.116733988965997\n",
      "\n",
      "corrections: mean = -8.137793063550186e-11 min = -6.4754516024339534e-09 max = 9.929915597684175e-09\n",
      "iteration 4: sigma0 = 1.1167339889935555\n",
      "\n",
      "change in sigma0 of 2.7558400006455486e-11 converged!\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/astamile/opt/anaconda3/envs/knoten/lib/python3.9/site-packages/plio/io/io_controlnetwork.py:356: UserWarning: The pointLog field is currently unsupported. Any pointLog data will not be saved.\n",
      "  warnings.warn('The pointLog field is currently unsupported. Any pointLog data will not be saved.')\n"
     ]
    }
   ],
   "source": [
    "sensors = generate_sensors(cubes) # generate sensors\n",
    "cnet = io_controlnetwork.from_isis(network) # load in network\n",
    "cnet = compute_apriori_ground_points(cnet, sensors) # calculate ground points\n",
    "\n",
    "### INPUTS ###\n",
    "all_parameters = {sn: get_sensor_parameters(sensor) for sn, sensor in sensors.items()} #all parameters\n",
    "parameters = {sn: parameter[:3] for sn, parameter in all_parameters.items()} #just solving for camera angles and angle velocity\n",
    "##############\n",
    "\n",
    "column_dict = compute_coefficient_columns(cnet, sensors, parameters)\n",
    "num_parameters = max(col_range[1] for col_range in column_dict.values())\n",
    "num_observations = 2 * len(cnet)\n",
    "W_observations = np.eye(num_observations)\n",
    "W_params = compute_parameter_weights(cnet, sensors, parameters, column_dict)\n",
    "\n",
    "iteration = 0\n",
    "V = compute_residuals(cnet, sensors)\n",
    "dX = np.zeros(W_params.shape[0]) #initialize for sigma calculatioN\n",
    "sigma0 = compute_sigma0(V, dX, W_params, W_observations)\n",
    "print(f'iteration {iteration}: sigma0 = {sigma0}\\n')\n",
    "\n",
    "max_iterations = 50\n",
    "tol = 1e-10\n",
    "total_correction = np.zeros(num_parameters)\n",
    "for i in range(max_iterations): \n",
    "    iteration += 1\n",
    "    old_sigma0 = sigma0\n",
    "\n",
    "    J = compute_jacobian(cnet, sensors, parameters, column_dict)    \n",
    "    N = J.T.dot(W_observations).dot(J) + W_params # calculate the normal equation\n",
    "    C = J.T.dot(W_observations).dot(V) - W_params.dot(total_correction)\n",
    "    dX = np.linalg.inv(N).dot(C) #calculate change in camera parameters and ground points\n",
    "    total_correction += dX\n",
    "    print(f'corrections: mean = {dX.mean()} min = {dX.min()} max = {dX.max()}')\n",
    "    \n",
    "    update_parameters(sensors, parameters, cnet, dX, column_dict)\n",
    "    V = compute_residuals(cnet, sensors)\n",
    "    sigma0 = compute_sigma0(V, dX, W_params, W_observations)\n",
    "    print(f'iteration {iteration}: sigma0 = {sigma0}\\n')\n",
    "    if (abs(sigma0 - old_sigma0) < tol):\n",
    "        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} converged!')\n",
    "        onet = io_controlnetwork.to_isis(cnet, 'data/py-adjusted.net', targetname=\"Mars\")\n",
    "        break\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.9.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
