point-set-registration.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as  np\n",
    "import os\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "\n",
    "from sys import argv, exit\n",
    "\n",
    "np.random.seed(42)\n",
    "np.set_printoptions(formatter={'float': lambda x: \"{0:0.5f}\".format(x)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def frobNorm(P1, P2, str1=\"mat1\", str2=\"mat2\"):\n",
    "    np.set_printoptions(suppress=True)\n",
    "    val = np.linalg.norm(P1 - P2, 'fro')\n",
    "    print(f\"Frobenius norm between {str1} and {str2} is: {val}\")\n",
    "    return val"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Have 2/3/4 points\n",
    "\n",
    "\n",
    "p = np.array([[0,9,0],[1,1,1],[1,2,3],[7,8,13]]) # You can add more points if you want to.\n",
    "# Hardcoded the (\"first 3\") points to ensure they are linearly independent\n",
    "# \"first 3\": In 3D space, max no of linearly independent vectors is 3. Refer to Notion\n",
    "# page for knowing why that's relevant.\n",
    "\n",
    "# Homogenising\n",
    "p_homo = np.hstack((p, np.ones(p.shape[0]).reshape((p.shape[0],1))))\n",
    "\n",
    "# Setting random transformation to transform points p to q.\n",
    "T = np.identity(4)\n",
    "T[0:3, 0:3] = R.from_euler('zyx', np.random.rand(3) * 90, degrees = True).as_matrix()\n",
    "T[0,3], T[1,3], T[2,3] = np.random.rand(3) * 10"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_homo_T = np.matmul(T, p_homo.T)\n",
    "q_homo = q_homo_T.T\n",
    "q = np.delete(q_homo, -1, 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def icp(p, q, min_points, method=\"Wahba\"): \n",
    "    # \"OP\" stands for Orthogonal Procrustes which needs minimum 4 points\n",
    "    # \"Wahba\" only needs 3 points\n",
    "    \n",
    "    min_points = min_points\n",
    "    print(f\"min_points={min_points}\")\n",
    "    u_p = np.mean(p[:min_points], axis=0)\n",
    "    u_q = np.mean(q[:min_points], axis=0)    \n",
    "    p_dash = p[:min_points] - u_p\n",
    "    q_dash = q[:min_points] - u_q\n",
    "    W = np.zeros((3,3))\n",
    "    for i in range(min_points):\n",
    "        W += np.matmul(q_dash[i, np.newaxis].T, p_dash[i, np.newaxis])\n",
    "    u, s, vh = np.linalg.svd(W, full_matrices=True)\n",
    "    print(f\"Singular values are {str(s)}\")\n",
    "    M = np.diag([1,1, np.linalg.det(u) * np.linalg.det(vh)])\n",
    "    if method == \"Wahba\":\n",
    "        R_recovered = u @ M @ vh\n",
    "    elif method == \"OP\":\n",
    "        R_recovered = u @ vh\n",
    "    t_recovered = u_q - R_recovered @ u_p\n",
    "    T_recovered = np.hstack((R_recovered, np.array([[t_recovered[0]], [t_recovered[1]], [t_recovered[2]]])))\n",
    "    T_recovered = np.vstack((T_recovered, np.array([[0,0,0,1]]))) \n",
    "\n",
    "    return T_recovered"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Set method: Orthogonal Procrustes or Wahba?\n",
    "\n",
    "* \"OP\" stands for Orthogonal Procrustes which needs minimum 4 points\n",
    "* \"Wahba\" only needs 3 points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "method = \"OP\" # \"Wahba\" or \"OP\"\n",
    "if method == \"OP\":\n",
    "    method_full = \"Orthogonal Procrustes\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "min_points=2\n",
      "Singular values are [33.00000 0.00000 0.00000]\n",
      "Frobenius norm between original T and Recovered T is: 2.8372086335363798\n",
      "\n",
      " Orthogonal Procrustes VERDICT: Using 2 points, it is False that I recovered the original transform \n",
      " \n",
      "\n",
      "min_points=3\n",
      "Singular values are [40.9395607   2.39377263  0.        ]\n",
      "Frobenius norm between original T and Recovered T is: 2.8996833043120636\n",
      "\n",
      " Orthogonal Procrustes VERDICT: Using 3 points, it is False that I recovered the original transform \n",
      " \n",
      "\n",
      "min_points=4\n",
      "Singular values are [146.11953215  40.92832629   0.45214156]\n",
      "Frobenius norm between original T and Recovered T is: 4.1514596760159754e-15\n",
      "\n",
      " Orthogonal Procrustes VERDICT: Using 4 points, it is True that I recovered the original transform \n",
      " \n",
      "\n"
     ]
    }
   ],
   "source": [
    "min_points = 2\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method_full} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\n\")\n",
    "\n",
    "\n",
    "\n",
    "min_points = 3\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method_full} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\n\")\n",
    "\n",
    "\n",
    "\n",
    "min_points = 4\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method_full} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\n\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "method = \"Wahba\" # \"Wahba\" or \"OP\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "min_points=2\n",
      "Singular values are [33.  0.  0.]\n",
      "Frobenius norm between original T and Recovered T is: 2.8372086335363798\n",
      "\n",
      " Wahba VERDICT: Using 2 points, it is False that I recovered the original transform \n",
      " \n",
      "\n",
      "min_points=3\n",
      "Singular values are [40.9395607   2.39377263  0.        ]\n",
      "Frobenius norm between original T and Recovered T is: 4.7775582088979414e-15\n",
      "\n",
      " Wahba VERDICT: Using 3 points, it is True that I recovered the original transform \n",
      " \n",
      "\n",
      "min_points=4\n",
      "Singular values are [146.11953215  40.92832629   0.45214156]\n",
      "Frobenius norm between original T and Recovered T is: 4.328229281970082e-15\n",
      "\n",
      " Wahba VERDICT: Using 4 points, it is True that I recovered the original transform \n",
      " \n",
      "\n"
     ]
    }
   ],
   "source": [
    "min_points = 2\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\n\")\n",
    "\n",
    "\n",
    "\n",
    "min_points = 3\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\n\")\n",
    "\n",
    "\n",
    "\n",
    "min_points = 4\n",
    "T_recovered = icp(p, q, min_points, method=method)\n",
    "recovered_bool = float(frobNorm(T, T_recovered, str1=\"original T\", str2=\"Recovered T\")) < 1e-5\n",
    "print(f\"\\n {method} VERDICT: Using {min_points} points, it is {recovered_bool} that I recovered the original transform \\n \\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.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}