notebook/sfm/sfm_v1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Structure from Motion (SfM)\n",
    "Author: Chul Min Yeum  \n",
    "Email: cmyeum@uwaterloo.ca  \n",
    "\n",
    "Last updated: 2024-03-13"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Triangulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1.14692591 0.59982009 1.218014  ]\n",
      "[1.14692591 0.59982009 1.218014  ]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "from scipy.linalg import null_space, svd\n",
    "\n",
    "# Function to create rotation matrix about x-axis\n",
    "def rotx(angle):\n",
    "    c = np.cos(np.deg2rad(angle))\n",
    "    s = np.sin(np.deg2rad(angle))\n",
    "    return np.array([[1, 0, 0],\n",
    "                     [0, c, -s],\n",
    "                     [0, s, c]])\n",
    "\n",
    "# Function to create rotation matrix about y-axis\n",
    "def roty(angle):\n",
    "    c = np.cos(np.deg2rad(angle))\n",
    "    s = np.sin(np.deg2rad(angle))\n",
    "    return np.array([[c, 0, s],\n",
    "                     [0, 1, 0],\n",
    "                     [-s, 0, c]])\n",
    "\n",
    "# Function to create rotation matrix about z-axis\n",
    "def rotz(angle):\n",
    "    c = np.cos(np.deg2rad(angle))\n",
    "    s = np.sin(np.deg2rad(angle))\n",
    "    return np.array([[c, -s, 0],\n",
    "                     [s, c, 0],\n",
    "                     [0, 0, 1]])\n",
    "\n",
    "# Synthetic projection matrix creation\n",
    "P1 = np.hstack([np.eye(3), np.zeros((3, 1))])\n",
    "P2 = np.dot(np.eye(3), np.hstack([np.dot(np.dot(rotx(10), roty(20)), rotz(30)), [[5], [5], [1]]]))\n",
    "\n",
    "# Synthetic 100 numbers of 3D points (X)\n",
    "X = np.random.rand(4, 1)\n",
    "\n",
    "# Image points corresponding to each X\n",
    "x1 = np.dot(P1, X)\n",
    "x1 = x1[0:2] / x1[2]\n",
    "x2 = np.dot(P2, X)\n",
    "x2 = x2[0:2] / x2[2]\n",
    "\n",
    "# Triangulation\n",
    "A = np.zeros((4, 4))\n",
    "A[0, :] = x1[0] * P1[2, :] - P1[0, :]\n",
    "A[1, :] = x1[1] * P1[2, :] - P1[1, :]\n",
    "A[2, :] = x2[0] * P2[2, :] - P2[0, :]\n",
    "A[3, :] = x2[1] * P2[2, :] - P2[1, :]\n",
    "\n",
    "X_comp = null_space(A)\n",
    "\n",
    "# Using SVD to get the solution\n",
    "U, D, Vt = svd(A)\n",
    "X_comp_svd = Vt.T[:, -1]\n",
    "\n",
    "print(X[0:3, 0].T / X[3, 0])\n",
    "print(X_comp[:, 0][0:3] / X_comp[:, 0][3])\n"
   ]
  }
 ],
 "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.11.8"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}