{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "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 in Network"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cubes = 'control_network_metrics/registration_quality/cubes.lis'\n",
    "sensors = generate_sensors(cubes, directory='data_lak/tmp/', clean=True)\n",
    "\n",
    "network_path = 'control_network_metrics/registration_quality/measure_error_50px_1pts_5mean_2std.net'\n",
    "network = io_controlnetwork.from_isis(network_path)\n",
    "network = compute_apriori_ground_points(network, sensors) # autoseed did not generate ground points, calculate and repopulate the data frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Determine Solve Parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "all_parameters = {sn: get_sensor_parameters(sensor) for sn, sensor in sensors.items()} #all parameters\n",
    "parameters = {sn: parameter[6:12] for sn, parameter in all_parameters.items()} #just solving for camera angles and angle velocity"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def compute_sigma(V, W_parameters, W_observations):\n",
    "    \"\"\"\n",
    "    Computes the resulting standard deviation of the residuals for the current state of the bundle network.\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    V  :  np.array\n",
    "          The control network dataframe with updated ground points\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.: point weights)\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "       : float64\n",
    "         Standard deviation of the residuals\n",
    "    \n",
    "    \"\"\"\n",
    "    num_parameters = W_parameters.shape[0]\n",
    "    num_observations = W_observations.shape[0]\n",
    "    dof = num_observations - num_parameters\n",
    "    VTPV = (V.dot(W_observations).dot(V))\n",
    "    sigma0 = np.sqrt(VTPV/dof)\n",
    "    return sigma0\n",
    "\n",
    "def bundle_iteration(J, V, W_parameters, W_observations):\n",
    "    \"\"\"\n",
    "    Parameters\n",
    "    ----------\n",
    "    J  :  ndarray\n",
    "          The control network as a dataframe generated by plio.\n",
    "    V  :  np.array\n",
    "          The control network dataframe with updated ground points\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  :  \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\n",
    "\n",
    "# For data snooping we need to calculate updated residuals\n",
    "def compute_normalized_residual(J, V, N, W_parameters, W_observations):\n",
    "    \"\"\"\n",
    "    Computes the normalized residual statistic for the data snooping method. Method derived from \n",
    "    Forstner 1985 \"The Reliability of Block Triangulation\"\n",
    "    \n",
    "    Parameters\n",
    "    ----------\n",
    "    V  :  np.array\n",
    "          The control network dataframe with updated ground points\n",
    "    N  :  \n",
    "        \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.: point weights)\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "       : np.array\n",
    "         Normalized residual statistic for the data snooping\n",
    "    \n",
    "    \"\"\"\n",
    "    sigma0 = compute_sigma(V, W_parameters, W_observations)\n",
    "    Qxx = np.linalg.inv(N)\n",
    "    Qvv = np.linalg.inv(W_observations) - J.dot(Qxx).dot(J.T)\n",
    "    qvv = np.diagonal(Qvv)\n",
    "    sigma_vi = sigma0*np.sqrt(qvv)\n",
    "    wi = -V/sigma_vi\n",
    "    \n",
    "    return wi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Data Snooping Function"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "k = 3.29 #critical values from Forstner\n",
    "# k = 4.1 #cricital value from Baarda"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def data_snooping(network, sensors, parameters, k):\n",
    "    \"\"\"\n",
    "    Parameters\n",
    "    ----------\n",
    "    network  :  DataFrame\n",
    "                The control network as a dataframe generated by plio\n",
    "    sensors  :  dict\n",
    "                A dictionary that maps ISIS serial numbers to CSM sensors\n",
    "    parameters  : list\n",
    "                 The list of  CsmParameter to compute the partials W.R.T.\n",
    "    k  :  float64\n",
    "          Critical value used for rejection criteria; defaults to Forstner's 3.29 \n",
    "          (or Baarda's 4.1??)\n",
    "    \n",
    "    Returns\n",
    "    -------\n",
    "      :  list\n",
    "      Indices of the network DataFrame that were rejected during data snooping\n",
    "    \"\"\"\n",
    "    net = network \n",
    "    net['mask'] = \"True\"\n",
    "\n",
    "    rejected_indices = []\n",
    "    awi = np.array([5, 5, 5, 5]) #initialize larger than k so you get into first iteration\n",
    "    while (awi > k).any():\n",
    "        print(len(net[net['mask']=='True']))\n",
    "\n",
    "        # weight matrices\n",
    "        coefficient_columns = compute_coefficient_columns(network, sensors, parameters)\n",
    "        num_parameters = max(col_range[1] for col_range in coefficient_columns.values())\n",
    "        W_parameters = compute_parameter_weights(network, sensors, parameters, coefficient_columns)\n",
    "        num_observations = 2 * len(net[net['mask']==\"True\"])\n",
    "        W_observations = np.eye(num_observations)\n",
    "\n",
    "        # bundle iteration (and set up)\n",
    "        V = compute_residuals(net[net['mask']==\"True\"], sensors)\n",
    "        J = compute_jacobian(net[net['mask']==\"True\"], sensors, parameters, coefficient_columns)\n",
    "        sigma0 = compute_sigma(V, W_parameters, W_observations)\n",
    "        N, dX = bundle_iteration(J, V, W_parameters, W_observations)\n",
    "\n",
    "        # calculate test statistic\n",
    "        wi = compute_normalized_residual(J, V, N, W_parameters, W_observations)\n",
    "        awi = abs(wi)\n",
    "\n",
    "        #find maximum\n",
    "        imax = np.argmax(awi)\n",
    "        print(f'max wi = {awi[imax]}') # display\n",
    "\n",
    "        if awi[imax] <= k:\n",
    "            print('Data Snooping Outlier Rejection Complete')\n",
    "            break\n",
    "\n",
    "        else:\n",
    "            reject = net.index[net['mask']==\"True\"][imax]\n",
    "            net.loc[net.index == reject, ['mask']] = \"False\"\n",
    "            rejected_indices.append(reject)\n",
    "            \n",
    "    return rejected_indices\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rejected_indices = data_snooping(network, sensors, parameters, k)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# plt.figure()\n",
    "# plt.boxplot(wi)\n",
    "plt.figure()\n",
    "plt.hist(wi[abs(wi) < k], bins=np.linspace(-4,5,50));\n",
    "plt.hist(wi[abs(wi) > k], bins=np.linspace(-4,5,50));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "csm_local",
   "language": "python",
   "name": "csm_local"
  },
  "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.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
