{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from knoten.bundle import *\n",
    "from knoten.csm import create_csm\n",
    "from plio.io import io_controlnetwork\n",
    "import csmapi\n",
    "\n",
    "import threading\n",
    "import concurrent.futures\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Distributed Bundle Adjustment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def update_single_sensor(sensor, updates, parameters):\n",
    "    if len(updates) != len(parameters):\n",
    "        print(\"Updates and parameters are of different lengths!\")\n",
    "        return\n",
    "    for param_idx in range(len(parameters)):\n",
    "        current_value = sensor.getParameterValue(parameters[param_idx].index)\n",
    "        sensor.setParameterValue(parameters[param_idx].index, current_value + updates[param_idx])\n",
    "        \n",
    "def compute_single_residual(sensor, ground_pt, measure):\n",
    "    ground_pt = csmapi.EcefCoord(ground_pt[0], ground_pt[1], ground_pt[2])\n",
    "    img_coord = sensor.groundToImage(ground_pt)\n",
    "    return measure - np.array([img_coord.line, img_coord.samp])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def admm_iteration(point_id, serial, sensor, \n",
    "                    parameters, measure,\n",
    "                    r, s, rho_x, rho_y, \n",
    "                    image_weights, point_weights, \n",
    "                    x_old, y_old,\n",
    "                    threshold=1e-7, max_its=20):\n",
    "    \"\"\"\n",
    "    Parameters\n",
    "    ----------\n",
    "    point_id:  str\n",
    "               network id for point\n",
    "               \n",
    "    serial:    str\n",
    "               serial number associated with the measure's images\n",
    "                  \n",
    "    sensor:    csmapi.csmapi.RasterGM\n",
    "               sensor model\n",
    "    \n",
    "    parameters: list\n",
    "                list of CsmParameter objects representing sensor solve parameters\n",
    "                \n",
    "    ground_pt:  numpy.ndarray\n",
    "                Adjusted X, Y, Z coordinates of ground point\n",
    "                \n",
    "    measure:    numpy.ndarray\n",
    "                line and sample of measure\n",
    "            \n",
    "    r:  numpy.ndarray\n",
    "    \n",
    "    s:  numpy.ndarray\n",
    "    \n",
    "    rho_x:  float\n",
    "    \n",
    "    rho_y:  float\n",
    "    \n",
    "    image_weights: numpy.ndarray\n",
    "    \n",
    "    point_weights: numpy.ndarray\n",
    "    \n",
    "    x_old:  numpy.ndarray\n",
    "    \n",
    "    y_old:  numpy.ndarray\n",
    "    \n",
    "    thresdhold:  float\n",
    "                 convergence threshold for G-N \n",
    "    \n",
    "    max_its:  int\n",
    "              max number of iterations to try for convergence\n",
    "              \n",
    "    Returns\n",
    "    -------\n",
    "    point_id: str\n",
    "    \n",
    "    serial:   str\n",
    "    \n",
    "    x_corrections:  np.ndarray\n",
    "                 point updates\n",
    "    \n",
    "    y_corrections:  np.ndarray\n",
    "                 sensor parameter updates\n",
    "                  \n",
    "    \"\"\"\n",
    "    # Function\n",
    "    x_corrections = np.zeros(3)\n",
    "    y_corrections = np.zeros(len(parameters))\n",
    "\n",
    "    for i in range(max_its):\n",
    "        measure_v = compute_single_residual(sensor, x_old, measure)\n",
    "        point_partials = compute_ground_partials(sensor, x_old)\n",
    "        sensor_partials = compute_sensor_partials(sensor, parameters, x_old)\n",
    "        measure_partials = np.concatenate([point_partials, sensor_partials], axis=-1)\n",
    "        N = measure_partials.T.dot(measure_partials)\n",
    "        N[:3,:3] += rho_x * np.eye(point_partials.shape[-1]) + point_weights\n",
    "        N[3:,3:] += rho_y * np.eye(sensor_partials.shape[-1]) + image_weights\n",
    "        RHS = measure_partials.T.dot(measure_v)\n",
    "        RHS[:3] -= rho_x * x_old + point_weights.dot(x_corrections) + r\n",
    "        RHS[3:] -= rho_y * y_old + image_weights.dot(y_corrections) + s\n",
    "\n",
    "        update = np.linalg.inv(N).dot(RHS)\n",
    "\n",
    "        x_corrections += update[:3]\n",
    "        y_corrections += update[3:]\n",
    "\n",
    "        x_old += update[:3]\n",
    "        update_single_sensor(sensor, update[3:], parameters)\n",
    "\n",
    "        if np.linalg.norm(update) < threshold:\n",
    "            break\n",
    "    \n",
    "    return point_id, serial, x_corrections, y_corrections"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "4.204919720360203\n"
     ]
    }
   ],
   "source": [
    "cubes = 'data/cubes.lis'\n",
    "sensors_dict = 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_dict) # autoseed did not generate ground points, calculate and repopulate the data frame\n",
    "\n",
    "all_parameters = {sn: get_sensor_parameters(sensor) for sn, sensor in sensors_dict.items()}\n",
    "solve_parameters = {sn: params[6:12] for sn, params in all_parameters.items()}\n",
    "\n",
    "# initial estimate of sigma0\n",
    "column_dict = compute_coefficient_columns(cnet, sensors_dict, solve_parameters)\n",
    "num_observations = 2 * len(cnet)\n",
    "num_parameters = max(col_range[1] for col_range in column_dict.values())\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_dict, solve_parameters, column_dict)\n",
    "\n",
    "V = compute_residuals(cnet, sensors_dict)\n",
    "dX = np.zeros(W_params.shape[0])\n",
    "sigma0 = compute_sigma0(V, dX, W_params, W_observations)\n",
    "print(sigma0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# z_i,j             -> apriori line/sample - constant\n",
    "# f(xij(k), yji(k)) -> updated_sensor.ground_to_image(updated_point)\n",
    "\n",
    "# xij(k)            -> dX of previous iteration (point parameters)\n",
    "# yji(k)            -> dX of precious iteration (sensor parameters)\n",
    "\n",
    "# xi(k)             -> consensus variable of point\n",
    "# yj(k)             -> consensus variable of sensor\n",
    "\n",
    "# rij(k)            -> Lagrangian multiplier of previous iteration (point parameters)\n",
    "# sji(k)            -> Lagrangian multiplier of previous iteration (sensor parameters)\n",
    "\n",
    "# rho_x(k)          -> penalty parameter of the previous iteration (point parameters)\n",
    "# rho_y(k)          -> penalty parameter of the previous iteration (sensor parameters)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initial Bundle Conditions\n",
    "ptids = cnet['id']\n",
    "serialnumbers = cnet['serialnumber']\n",
    "\n",
    "zij = [np.array([l, s]) for l, s in zip(cnet['line'], cnet['sample'])]\n",
    "\n",
    "rhox = 0.001*np.ones(len(cnet)) # initial estimates of the penatly parameters (point)\n",
    "rhoy = 0.001*np.ones(len(cnet)) # (camera)\n",
    "\n",
    "# initial estimates of the lagrangian multipliers\n",
    "rij_initial = {ptid: np.random.rand(1,3)[0] for ptid in ptids.unique()}\n",
    "rij = [rij_initial[ptid] for ptid in ptids] # points\n",
    "sji_initial = {sn: np.round(10*np.random.rand(1,len(solve_parameters[sn]))[0]) for sn in serialnumbers.unique()}\n",
    "sji = [sji_initial[sn] for sn in serialnumbers] # cameras \n",
    "\n",
    "xi = [np.array(np.zeros(3)) for _ in range(0, len(cnet))] # consenseous variable (point)\n",
    "yj = [np.array(np.zeros(len(solve_parameters[sn]))) for sn in serialnumbers] # consenseous variable (camera)\n",
    "\n",
    "xij = [np.array(row[[\"adjustedX\", \"adjustedY\", \"adjustedZ\"]].values.astype('float64')) for _, row in cnet.iterrows()]\n",
    "yji = list()\n",
    "for sn in serialnumbers:\n",
    "    arr  = np.array([])\n",
    "    for parameter in solve_parameters[sn]:\n",
    "        arr = np.append(arr, parameter.value)\n",
    "    yji.append(arr)\n",
    "\n",
    "sensors_list = [sensors_dict[sn] for sn in serialnumbers]\n",
    "parameters_list = [solve_parameters[sn] for sn in serialnumbers]\n",
    "vtpv_list = [0 for _ in range(0, len(cnet))]\n",
    "\n",
    "current = pd.DataFrame(columns=['id', 'serialnumber', 'sensor', 'parameters', 'zij', 'rhox', 'rhoy', 'rij', 'sji', 'xij', 'yji', 'xi', 'yj', 'vtpv'], \\\n",
    "                       data=zip(ptids, serialnumbers, sensors_list, parameters_list, zij, rhox, rhoy, rij, sji, xij, yji, xi, yj, vtpv_list))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "130\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>id</th>\n",
       "      <th>serialnumber</th>\n",
       "      <th>sensor</th>\n",
       "      <th>parameters</th>\n",
       "      <th>zij</th>\n",
       "      <th>rhox</th>\n",
       "      <th>rhoy</th>\n",
       "      <th>rij</th>\n",
       "      <th>sji</th>\n",
       "      <th>xij</th>\n",
       "      <th>yji</th>\n",
       "      <th>xi</th>\n",
       "      <th>yj</th>\n",
       "      <th>vtpv</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>autoseed_001</td>\n",
       "      <td>MRO/CTX/1085197697:073</td>\n",
       "      <td>&lt;csmapi.csmapi.RasterGM; proxy of &lt;Swig Object...</td>\n",
       "      <td>[6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...</td>\n",
       "      <td>[302.06913127502867, 200.85677699800772]</td>\n",
       "      <td>0.001</td>\n",
       "      <td>0.001</td>\n",
       "      <td>[0.7545983067809806, 0.30226941011733066, 0.10...</td>\n",
       "      <td>[7.0, 1.0, 5.0, 5.0, 8.0, 1.0]</td>\n",
       "      <td>[2919361.3500983184, -1194116.8210146269, 1238...</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>autoseed_001</td>\n",
       "      <td>MRO/CTX/1157902986:250</td>\n",
       "      <td>&lt;csmapi.csmapi.RasterGM; proxy of &lt;Swig Object...</td>\n",
       "      <td>[6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...</td>\n",
       "      <td>[15166.790917003123, 744.1819182946748]</td>\n",
       "      <td>0.001</td>\n",
       "      <td>0.001</td>\n",
       "      <td>[0.7545983067809806, 0.30226941011733066, 0.10...</td>\n",
       "      <td>[9.0, 7.0, 2.0, 9.0, 2.0, 3.0]</td>\n",
       "      <td>[2919361.3500983184, -1194116.8210146269, 1238...</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>autoseed_001</td>\n",
       "      <td>MRO/CTX/1096561308:045</td>\n",
       "      <td>&lt;csmapi.csmapi.RasterGM; proxy of &lt;Swig Object...</td>\n",
       "      <td>[6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...</td>\n",
       "      <td>[13311.618299705062, 406.4429423017374]</td>\n",
       "      <td>0.001</td>\n",
       "      <td>0.001</td>\n",
       "      <td>[0.7545983067809806, 0.30226941011733066, 0.10...</td>\n",
       "      <td>[1.0, 2.0, 9.0, 4.0, 2.0, 8.0]</td>\n",
       "      <td>[2919361.3500983184, -1194116.8210146269, 1238...</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>autoseed_002</td>\n",
       "      <td>MRO/CTX/1085197697:073</td>\n",
       "      <td>&lt;csmapi.csmapi.RasterGM; proxy of &lt;Swig Object...</td>\n",
       "      <td>[6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...</td>\n",
       "      <td>[95.88393113304467, 1925.9642063681222]</td>\n",
       "      <td>0.001</td>\n",
       "      <td>0.001</td>\n",
       "      <td>[0.18048150856645673, 0.7133177098398927, 0.63...</td>\n",
       "      <td>[7.0, 1.0, 5.0, 5.0, 8.0, 1.0]</td>\n",
       "      <td>[2923282.5660719196, -1184949.09107713, 123813...</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>autoseed_002</td>\n",
       "      <td>MRO/CTX/1157902986:250</td>\n",
       "      <td>&lt;csmapi.csmapi.RasterGM; proxy of &lt;Swig Object...</td>\n",
       "      <td>[6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...</td>\n",
       "      <td>[14937.449494029423, 2092.5731615034165]</td>\n",
       "      <td>0.001</td>\n",
       "      <td>0.001</td>\n",
       "      <td>[0.18048150856645673, 0.7133177098398927, 0.63...</td>\n",
       "      <td>[9.0, 7.0, 2.0, 9.0, 2.0, 3.0]</td>\n",
       "      <td>[2923282.5660719196, -1184949.09107713, 123813...</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0]</td>\n",
       "      <td>[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]</td>\n",
       "      <td>0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "             id            serialnumber  \\\n",
       "0  autoseed_001  MRO/CTX/1085197697:073   \n",
       "1  autoseed_001  MRO/CTX/1157902986:250   \n",
       "2  autoseed_001  MRO/CTX/1096561308:045   \n",
       "3  autoseed_002  MRO/CTX/1085197697:073   \n",
       "4  autoseed_002  MRO/CTX/1157902986:250   \n",
       "\n",
       "                                              sensor  \\\n",
       "0  <csmapi.csmapi.RasterGM; proxy of <Swig Object...   \n",
       "1  <csmapi.csmapi.RasterGM; proxy of <Swig Object...   \n",
       "2  <csmapi.csmapi.RasterGM; proxy of <Swig Object...   \n",
       "3  <csmapi.csmapi.RasterGM; proxy of <Swig Object...   \n",
       "4  <csmapi.csmapi.RasterGM; proxy of <Swig Object...   \n",
       "\n",
       "                                          parameters  \\\n",
       "0  [6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...   \n",
       "1  [6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...   \n",
       "2  [6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...   \n",
       "3  [6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...   \n",
       "4  [6 Omega Bias (2): 0.0 m, 7 Phi Bias (2): 0.0 ...   \n",
       "\n",
       "                                        zij   rhox   rhoy  \\\n",
       "0  [302.06913127502867, 200.85677699800772]  0.001  0.001   \n",
       "1   [15166.790917003123, 744.1819182946748]  0.001  0.001   \n",
       "2   [13311.618299705062, 406.4429423017374]  0.001  0.001   \n",
       "3   [95.88393113304467, 1925.9642063681222]  0.001  0.001   \n",
       "4  [14937.449494029423, 2092.5731615034165]  0.001  0.001   \n",
       "\n",
       "                                                 rij  \\\n",
       "0  [0.7545983067809806, 0.30226941011733066, 0.10...   \n",
       "1  [0.7545983067809806, 0.30226941011733066, 0.10...   \n",
       "2  [0.7545983067809806, 0.30226941011733066, 0.10...   \n",
       "3  [0.18048150856645673, 0.7133177098398927, 0.63...   \n",
       "4  [0.18048150856645673, 0.7133177098398927, 0.63...   \n",
       "\n",
       "                              sji  \\\n",
       "0  [7.0, 1.0, 5.0, 5.0, 8.0, 1.0]   \n",
       "1  [9.0, 7.0, 2.0, 9.0, 2.0, 3.0]   \n",
       "2  [1.0, 2.0, 9.0, 4.0, 2.0, 8.0]   \n",
       "3  [7.0, 1.0, 5.0, 5.0, 8.0, 1.0]   \n",
       "4  [9.0, 7.0, 2.0, 9.0, 2.0, 3.0]   \n",
       "\n",
       "                                                 xij  \\\n",
       "0  [2919361.3500983184, -1194116.8210146269, 1238...   \n",
       "1  [2919361.3500983184, -1194116.8210146269, 1238...   \n",
       "2  [2919361.3500983184, -1194116.8210146269, 1238...   \n",
       "3  [2923282.5660719196, -1184949.09107713, 123813...   \n",
       "4  [2923282.5660719196, -1184949.09107713, 123813...   \n",
       "\n",
       "                              yji               xi  \\\n",
       "0  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  [0.0, 0.0, 0.0]   \n",
       "1  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  [0.0, 0.0, 0.0]   \n",
       "2  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  [0.0, 0.0, 0.0]   \n",
       "3  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  [0.0, 0.0, 0.0]   \n",
       "4  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  [0.0, 0.0, 0.0]   \n",
       "\n",
       "                               yj  vtpv  \n",
       "0  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]     0  \n",
       "1  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]     0  \n",
       "2  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]     0  \n",
       "3  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]     0  \n",
       "4  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]     0  "
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print(len(current))\n",
    "current.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Iteration 0: \n",
      "sigma0 =  7806.3589905018825\n",
      "Iteration 1: \n"
     ]
    }
   ],
   "source": [
    "# Bundle\n",
    "iteration = 0\n",
    "max_iterations = 10\n",
    "simga0_diff_tol = 1e-10\n",
    "update_threshold = 1e-7\n",
    "for i in range(max_iterations):\n",
    "    print(f'Iteration {iteration}: ')\n",
    "    iteration += 1\n",
    "\n",
    "    # transition old values - may not need?\n",
    "    previous = current\n",
    "    old_sigma0 = sigma0\n",
    "    \n",
    "    with concurrent.futures.ThreadPoolExecutor(max_workers=10) as executor:\n",
    "        results = list()\n",
    "        for i, row in current.iterrows():\n",
    "            point_id = row['id']\n",
    "            serial = row['serialnumber']\n",
    "            sensor = row['sensor']\n",
    "            parameters = row['parameters']\n",
    "            apriori_measure = row['zij']\n",
    "            image_weights = compute_image_weight(sensor, parameters)\n",
    "            point_weights = compute_point_weight(cnet, point_id)\n",
    "#             print('point_weights', point_weights)\n",
    "            adjusted_gp = row['xij'] # adjusted ground points\n",
    "            adjusted_sp = row['yji'] # adjusted sensor parameter values\n",
    "\n",
    "            results.append(executor.submit(admm_iteration, \\\n",
    "                                           point_id, serial, sensor, \\\n",
    "                                           parameters, apriori_measure, \\\n",
    "                                           row['rij'], row['sji'], row['rhox'], row['rhoy'], \\\n",
    "                                           image_weights, point_weights, \\\n",
    "                                           adjusted_gp, adjusted_sp, \\\n",
    "                                           threshold=1e-7, max_its=20))\n",
    "            \n",
    "            \n",
    "        for f in concurrent.futures.as_completed(results):\n",
    "            # results structured like (ptid, sn, xij (point), yji (camera))\n",
    "            result = f.result()\n",
    "            ptid = result[0]\n",
    "            sn = result[1]\n",
    "            \n",
    "            # save new point and sensors states\n",
    "            index = current[(current['serialnumber'] == sn) & (current['id'] == ptid)].index[0]\n",
    "            adjusted_gp = current.loc[index, 'xij']+result[2]\n",
    "            adjusted_sp = current.loc[index, 'yji']+result[3]\n",
    "            current.loc[index, 'xij':'yji'] = adjusted_gp, adjusted_sp\n",
    "            \n",
    "            # apply the sensor corrections to the sensor and the parameter.values\n",
    "            parameters = current.loc[index, 'parameters']\n",
    "            update_single_sensor(current.loc[index, 'sensor'], result[3], parameters)\n",
    "            for i in range(0, len(parameters)):\n",
    "                current.loc[index, 'parameters'][i].value += result[3][i]\n",
    "                \n",
    "            # calculate portion of vtpv - may not need?\n",
    "            sensor = current.loc[index, 'sensor']\n",
    "            parameters = current.loc[index, 'parameters']\n",
    "            ground_pt = current.loc[index, 'xij']\n",
    "            measure = current.loc[index, 'zij']\n",
    "            image_weights = compute_image_weight(sensor, parameters)\n",
    "            point_weights = compute_point_weight(cnet, point_id)\n",
    "            measure_v = compute_single_residual(sensor, ground_pt, measure)\n",
    "            c = image_weights.shape[0]\n",
    "            p = point_weights.shape[0]\n",
    "            param_weights = np.block([[point_weights, np.zeros((p, c))],\n",
    "                                     [np.zeros((c,p)), image_weights]])\n",
    "            dX = np.hstack([result[2], result[3]])\n",
    "            current.loc[index, 'vtpv'] = measure_v.dot(measure_v) + dX.dot(param_weights).dot(dX)\n",
    "            \n",
    "            \n",
    "    # Calculate consenseous variables\n",
    "    for sn, group in current.groupby('serialnumber'):\n",
    "        # calculate consenseous variable\n",
    "        xij_sum = sum(group['xij'])\n",
    "        rhox = np.array(group['rhox'])\n",
    "        rij = np.array(group['rij'])\n",
    "        constraint_sum = sum((1/rhox)*rij)\n",
    "        xi = (1/len(group))*(xij_sum + constraint_sum)\n",
    "        # update concenseous variable\n",
    "        indices = list(group.index)\n",
    "        for index in indices:\n",
    "            current.at[index, 'xi'] = xi\n",
    "    for pt, group in current.groupby('id'):\n",
    "        # calculate consenseous variable\n",
    "        yji_sum = sum(group['yji'])\n",
    "        rhoy = np.array(group['rhoy'])\n",
    "        sji = np.array(group['sji'])\n",
    "        constraint_sum = sum((1/rhoy)*sji)\n",
    "        yj = (1/len(group))*(yji_sum + constraint_sum)\n",
    "        # update concenseous variable\n",
    "        indices = list(group.index)\n",
    "        for index in indices:\n",
    "            current.at[index, 'yj'] = yj\n",
    "            \n",
    "    for i, row in current.iterrows():\n",
    "        # update lagrangian multiplier estimates\n",
    "        rij = row['rij'] + row['rhox']*(row['xij']-row['xi'])\n",
    "        sji = row['sji'] + row['rhoy']*(row['yji']-row['yj'])\n",
    "        current.loc[i, 'rij':'sji'] = rij, sji\n",
    "        # update penalty parameters\n",
    "        rhox = row['rhox']*10\n",
    "        rhoy = row['rhoy']*10\n",
    "        \n",
    "    # Check on full bundle convergence - two methods presented here\n",
    "    # 1.) sum all VTPV and divided by (num_obs - num_param)\n",
    "    sigma0 = np.sqrt(current['vtpv'].sum()/(num_observations-num_parameters))\n",
    "    print('sigma0 = ', sigma0)\n",
    "    if (abs(sigma0 - old_sigma0) < simga0_diff_tol):\n",
    "        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} - Converged!')\n",
    "        break\n",
    "    # 2.) take norm consensus variable update\n",
    "#     update = \n",
    "#     if (np.linalg.norm(update) < update_threshold):\n",
    "#         print(f\"update nrom of {np.linalg.norm(update)} - Converged!\")\n",
    "#         break"
   ]
  },
  {
   "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": 2
}
