{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/jmapel/miniconda3/envs/knoten/lib/python3.7/site-packages/ale/__init__.py:22: UserWarning: ALESPICEROOT environment variable not set, Spice Drivers will not function correctly\n",
      "  warnings.warn('ALESPICEROOT environment variable not set, Spice Drivers will not function correctly')\n"
     ]
    }
   ],
   "source": [
    "import os\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",
    "from collections import OrderedDict\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from knoten.bundle import *"
   ]
  },
  {
   "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",
    "sensors = {sn: sensors[sn] for sn in cnet[\"serialnumber\"].unique()}\n",
    "cnet = compute_apriori_ground_points(cnet, sensors) # autoseed did not generate ground points, calculate and repopulate the data frame"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "scrolled": false
   },
   "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/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",
      "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"
     ]
    }
   ],
   "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": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Image: MRO/CTX/1085197697:073\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",
      "Image: MRO/CTX/1157902986:250\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",
      "Image: MRO/CTX/1096561308:045\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",
      "Image: MRO/CTX/1136952576:186\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"
     ]
    }
   ],
   "source": [
    "# Solve for angles and angular rates\n",
    "solve_parameters = {sn: params[6:12] for sn, params in all_parameters.items()}\n",
    "for sn, parameters in solve_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": 5,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "column_dict = compute_coefficient_columns(cnet, sensors, solve_parameters)"
   ]
  },
  {
   "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)\n",
    "dof = W_observations.shape[0] - W_params.shape[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "max_iterations = 10\n",
    "tol = 1e-10"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_sensor_params = sum([len(parameters) for parameters in solve_parameters.values()])\n",
    "num_point_params = 3 * len(cnet[\"id\"].unique())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "iteration 0: sigma0 = 4.204919720360203\n",
      "\n",
      "corrections: mean = 0.05349160077573805 min = -14.251221291713048 max = 40.834990964537575\n",
      "iteration 1: sigma0 = 1.3539063538494558\n",
      "\n",
      "corrections: mean = 2.7124445740181957e-06 min = -0.011426521506164248 max = 0.011842165594603658\n",
      "iteration 2: sigma0 = 1.127336617096085\n",
      "\n",
      "corrections: mean = 4.600122255126198e-08 min = -1.8475863314383526e-06 max = 4.573929399245028e-06\n",
      "iteration 3: sigma0 = 1.1273365324778626\n",
      "\n",
      "corrections: mean = 1.879394505450981e-11 min = -1.283151025419733e-09 max = 1.8514518745707175e-09\n",
      "iteration 4: sigma0 = 1.127336532470605\n",
      "\n",
      "change in sigma0 of 7.257527911974648e-12 converged!\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",
    "column_dict = compute_coefficient_columns(cnet, sensors, solve_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, solve_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",
    "total_correction_dense = 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, solve_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_dense)\n",
    "    dX = np.linalg.inv(N).dot(C) #calculate change in camera parameters and ground points\n",
    "    total_correction_dense += dX\n",
    "    print(f'corrections: mean = {dX.mean()} min = {dX.min()} max = {dX.max()}')\n",
    "    \n",
    "    update_parameters(sensors, solve_parameters, cnet, dX, column_dict)\n",
    "    \n",
    "    V = compute_residuals(cnet, sensors)\n",
    "    sigma0 = compute_sigma0(V, dX, W_params, W_observations)\n",
    "    sigma0 = np.sqrt((V.dot(W_observations).dot(V) + dX.dot(W_params).dot(dX))/dof)\n",
    "    print(f'iteration {iteration}: sigma0 = {sigma0}\\n')\n",
    "    \n",
    "    if (abs(sigma0 - old_sigma0) < tol):\n",
    "        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} converged!')\n",
    "        break\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "corrections: mean = 0.05349160077573873 min = -14.251221291713076 max = 40.834990964537546\n",
      "iteration 1: sigma0 = 1.353906353849451\n",
      "\n",
      "corrections: mean = 2.712443886869334e-06 min = -0.011426521492657233 max = 0.011842165579696888\n",
      "iteration 2: sigma0 = 1.1273366170980164\n",
      "\n",
      "corrections: mean = 4.6016402655676494e-08 min = -1.8475512374681445e-06 max = 4.5737885812869825e-06\n",
      "iteration 3: sigma0 = 1.1273365324789022\n",
      "\n",
      "corrections: mean = 3.7306738076770124e-12 min = -1.429027500961002e-09 max = 1.6954805056745274e-09\n",
      "iteration 4: sigma0 = 1.1273365324578994\n",
      "\n",
      "change in sigma0 of 2.100275509064886e-11 converged!\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",
    "# This is setup once per bundle and then accumulates\n",
    "total_corrections = np.zeros(num_sensor_params + num_point_params)\n",
    "\n",
    "# These are computed once per bundle\n",
    "W_CC_sparse = sparse.csc_matrix((num_sensor_params, num_sensor_params))\n",
    "W_PP = {}\n",
    "\n",
    "# Compute image param weight matrices\n",
    "for sn, params in solve_parameters.items():\n",
    "    coeff_indices = column_dict[sn]\n",
    "    coeff_range = np.arange(coeff_indices[0], coeff_indices[1])\n",
    "    num_image_coeffs = coeff_indices[1] - coeff_indices[0]\n",
    "    W_CC_data = compute_image_weight(sensors[sn], params).ravel()\n",
    "    W_CC_row = np.repeat(coeff_range, num_image_coeffs)\n",
    "    W_CC_column = np.tile(coeff_range, num_image_coeffs)\n",
    "    W_CC_sparse += sparse.coo_matrix((W_CC_data, (W_CC_row, W_CC_column)), (num_sensor_params, num_sensor_params)).tocsc()\n",
    "    \n",
    "# Compute point param weight matrices\n",
    "for point_id in cnet['id'].unique():\n",
    "    W_PP[point_id] = compute_point_weight(cnet, point_id)\n",
    "    \n",
    "V = compute_residuals(cnet, sensors)\n",
    "sigma0 = compute_sigma0_sparse(V, np.zeros(total_corrections.shape), W_CC_sparse, W_PP, W_observations, column_dict)\n",
    "    \n",
    "# Start iteration logic\n",
    "for i in range(max_iterations):\n",
    "    old_sigma0 = sigma0\n",
    "    \n",
    "    H_CC_sparse = sparse.csc_matrix((num_sensor_params, num_sensor_params))\n",
    "    H_CC_sparse += W_CC_sparse\n",
    "\n",
    "    g_C_sparse = np.zeros(num_sensor_params)\n",
    "    g_C_sparse -= W_CC_sparse.dot(total_corrections[:num_sensor_params])\n",
    "    # g_C_sparse += W_CC_sparse.dot(sensor_corrections)\n",
    "\n",
    "    # Q = H_PP^-1 * H_PC \n",
    "    Q_mats = {}\n",
    "    # NIC = H_PP^-1 * g_P\n",
    "    NIC_vecs = {}\n",
    "\n",
    "    updates = np.zeros(num_sensor_params + num_point_params)\n",
    "\n",
    "    for point_id, group in cnet.groupby('id'):\n",
    "        ground_pt = group.iloc[0][[\"adjustedX\", \"adjustedY\", \"adjustedZ\"]]\n",
    "        H_CP = sparse.csc_matrix((num_sensor_params, 3))\n",
    "        H_PP = np.zeros((3, 3))\n",
    "        g_P = np.zeros(3)\n",
    "        for measure_idx, row in group.iterrows():\n",
    "            serial = row[\"serialnumber\"]\n",
    "            sensor = sensors[serial]\n",
    "            point_partials = compute_ground_partials(sensor, ground_pt)\n",
    "            sensor_partials = compute_sensor_partials(sensor, solve_parameters[serial], ground_pt)\n",
    "            coeff_indices = column_dict[serial]\n",
    "            coeff_range = np.arange(coeff_indices[0], coeff_indices[1])\n",
    "            num_image_coeffs = coeff_indices[1] - coeff_indices[0]\n",
    "\n",
    "            H_CC_point_data = np.dot(sensor_partials.T, sensor_partials).ravel()\n",
    "            H_CC_point_row = np.repeat(coeff_range, num_image_coeffs)\n",
    "            H_CC_point_column = np.tile(coeff_range, num_image_coeffs)\n",
    "            H_CC_sparse += sparse.coo_matrix((H_CC_point_data, (H_CC_point_row, H_CC_point_column)), (num_sensor_params, num_sensor_params)).tocsc()\n",
    "\n",
    "            H_CP_point_data = np.dot(sensor_partials.T, point_partials).ravel()\n",
    "            H_CP_point_row = np.repeat(coeff_range, 3)\n",
    "            H_CP_point_column = np.tile(np.arange(0, 3), num_image_coeffs)\n",
    "            H_CP += sparse.coo_matrix((H_CP_point_data, (H_CP_point_row, H_CP_point_column)), (num_sensor_params, 3)).tocsc()\n",
    "\n",
    "            H_PP += np.dot(point_partials.T, point_partials)\n",
    "\n",
    "            g_C_sparse[coeff_indices[0]:coeff_indices[1]] += np.dot(sensor_partials.T, V[2*measure_idx:2*measure_idx+2])\n",
    "            g_P += np.dot(point_partials.T, V[2*measure_idx:2*measure_idx+2])\n",
    "        point_param_range = column_dict[point_id]\n",
    "        g_P -= W_PP[point_id].dot(total_corrections[point_param_range[0]:point_param_range[1]])\n",
    "        H_PP += W_PP[point_id]\n",
    "        H_PP_inv = sparse.csc_matrix(np.linalg.inv(H_PP))\n",
    "        Q_mats[point_id] = H_PP_inv.dot(H_CP.transpose())\n",
    "        NIC_vecs[point_id] = H_PP_inv.dot(g_P)\n",
    "        H_CC_sparse -= H_CP.dot(Q_mats[point_id])\n",
    "        g_C_sparse -= H_CP.dot(NIC_vecs[point_id])\n",
    "\n",
    "    updates[:num_sensor_params] = sparse.linalg.spsolve(H_CC_sparse, g_C_sparse)\n",
    "\n",
    "    for point_id in Q_mats:\n",
    "        point_param_indices = column_dict[point_id]\n",
    "        updates[point_param_indices[0]:point_param_indices[1]] = NIC_vecs[point_id] - Q_mats[point_id].dot(updates[:num_sensor_params])\n",
    "        \n",
    "    print(f'corrections: mean = {updates.mean()} min = {updates.min()} max = {updates.max()}')\n",
    "\n",
    "    total_corrections += updates\n",
    "\n",
    "    update_parameters(sensors, solve_parameters, cnet, updates, column_dict)\n",
    "    V = compute_residuals(cnet, sensors)\n",
    "    sigma0 = compute_sigma0_sparse(V, updates, W_CC_sparse, W_PP, W_observations, column_dict)\n",
    "    print(f'iteration {i+1}: sigma0 = {sigma0}\\n')\n",
    "    \n",
    "    if (abs(sigma0 - old_sigma0) < tol):\n",
    "        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} converged!')\n",
    "        break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Sensor diff\n",
      "-1.450808806424675e-09\n",
      "1.2130136894938914e-09\n",
      "Point diff\n",
      "-2.297802850770303e-10\n",
      "4.665956510052638e-10\n"
     ]
    }
   ],
   "source": [
    "print(\"Sensor diff\")\n",
    "print(np.min(total_correction_dense[:num_sensor_params].flatten() - total_corrections[:num_sensor_params]))\n",
    "print(np.max(total_correction_dense[:num_sensor_params].flatten() - total_corrections[:num_sensor_params]))\n",
    "print(\"Point diff\")\n",
    "print(np.min(total_correction_dense[num_sensor_params:].flatten() - total_corrections[num_sensor_params:]))\n",
    "print(np.max(total_correction_dense[num_sensor_params:].flatten() - total_corrections[num_sensor_params:]))"
   ]
  },
  {
   "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.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
