{
  "cells": [
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "\n# DLT reconstruction code\nPython translation (with slight modifications) of Ty Hedrick's \ndlt_reconstruct from the easyWand package\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "source": [
        "import numpy as np \n\ndef dlt_reconstruct(c,camPts):\n    '''\n    TODO: \n        * Implement rmse calculation\n    \n    '''\n\n    # number of cameras\n    nCams = camPts.shape[1]/2\n    \n    #s etup output variables\n    rmse = np.tile(np.nan, 3)\n    \n    # if we have 2+ cameras, begin reconstructing\n    if nCams < 2:\n        raise ValueError(f'At least >= 2 camera xy points must be there. nCams:{nCams}')\n\n    all_camera_m1 = []\n    all_camera_m2 = []\n    for i in range(int(nCams)):\n        u,v = camPts[:,2*i], camPts[:,2*i+1]\n        m1_thiscam = np.zeros((2,3))\n        m2_thiscam = np.zeros((2,1))\n        \n        m1_thiscam[0,0] = u*c[8,i] - c[0,i]\n        m1_thiscam[0,1] = u*c[9,i] - c[1,i]\n        m1_thiscam[0,2] = u*c[10,i] - c[2,i]\n        m1_thiscam[1,0] = v*c[8,i] - c[4,i]\n        m1_thiscam[1,1] = v*c[9,i] - c[5,i]\n        m1_thiscam[1,2] = v*c[10,i] - c[6,i]\n\n        all_camera_m1.append(m1_thiscam)\n        \n        m2_thiscam[0] = c[3,i]-u\n        m2_thiscam[1] = c[7,i]-v\n        all_camera_m2.append(m2_thiscam)\n   \n    m1_overall = np.row_stack(all_camera_m1)\n    m2_overall = np.row_stack(all_camera_m2)\n        \n    xyz = np.linalg.lstsq(m1_overall, m2_overall)\n    return  xyz"
      ]
    }
  ],
  "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.9"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}