2_Alignment/ICP_SVD_Gaussian_hill.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0bdca05d-c21e-4325-9657-4b235418b6ba",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Iterative Closest Point\n",
    "\n",
    "*Aljoscha Rheinwalt, <aljoscha.rheinwalt@uni-potsdam.de> (2022)*\n",
    "\n",
    "## Introduction\n",
    "\n",
    "This lab is about the basics of the Singular Value Decomposition (SVD) based Iterative Closest Point (ICP) point cloud alignment approach. The goal is to develop our own implementation using only Numpy, which we can then illustrate and test on synthetic point clouds. Our toy model for this lab is a Gaussian hill which is a smooth and easy to sample surface, because the elevation $z$ of the hill can be expressed as a function of the coordinates $x$ and $y$:\n",
    "$$z(x, y) = e^{-x^2-y^2} \\;.$$\n",
    "\n",
    "A synthetic point cloud $p$ from this model can be generated by randomly sampling $(x, y)$ coordinates and then calculating the corresponding $z$ coordinates from the model function. For testing purposes a second synthetic point cloud $q$ that is not aligned to the first can be retrieved from the first by rotating and translating it in three dimensions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "3ad24c44-be00-4b2e-9736-593a31052c6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# first point cloud p\n",
    "n = 1000\n",
    "x = np.random.random(n) * 2 - 1\n",
    "y = np.random.random(n) * 2 - 1\n",
    "z = np.exp(- x * x - y * y)\n",
    "p = np.c_[x, y, z]\n",
    "\n",
    "def rot3d(p, yaw, pitch, roll):\n",
    "    a = np.array([[+np.cos(yaw), -np.sin(yaw), 0],\n",
    "                  [+np.sin(yaw), +np.cos(yaw), 0],\n",
    "                  [0, 0, 1]])\n",
    "    b = np.array([[+np.cos(pitch), 0, +np.sin(pitch)],\n",
    "                  [0, 1, 0],\n",
    "                  [-np.sin(pitch), 0, +np.cos(pitch)]])\n",
    "    c = np.array([[1, 0, 0],\n",
    "                  [0, +np.cos(roll), -np.sin(roll)],\n",
    "                  [0, +np.sin(roll), +np.cos(roll)]])\n",
    "    return a.dot(b.dot(c.dot(p.T))).T\n",
    "\n",
    "\n",
    "# second point cloud q\n",
    "q = rot3d(p, np.pi/4, np.pi/4, np.pi/4)\n",
    "q += np.array([0.25, 0.50, 0.75])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0821db9-44dc-48be-8881-0a12d855da25",
   "metadata": {},
   "source": [
    "## Visualisation\n",
    "\n",
    "There are probably more ways to visualize a 3D point cloud than there are ways to align them. All have their advantages and disadvantages. Although Matplotlib is probably the most common visualisation Python module, its 3D rendering capabilities are not very suitable for larger point clouds. Here we'll use Open3d for visualisation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "39933aa1-5308-4c59-a9dd-487f19cb8efd",
   "metadata": {},
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "\n",
    "# Open3d point cloud objects\n",
    "po = o3d.geometry.PointCloud()\n",
    "po.points = o3d.utility.Vector3dVector(p)\n",
    "po.paint_uniform_color([1, 0.5, 0])\n",
    "\n",
    "qo = o3d.geometry.PointCloud()\n",
    "qo.points = o3d.utility.Vector3dVector(q)\n",
    "qo.paint_uniform_color([0.5, 0, 1])\n",
    "\n",
    "# open interactive visualization window\n",
    "o3d.visualization.draw_geometries([po, qo])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "979ce9d3-932c-45a0-8f39-9950b5adcf88",
   "metadata": {},
   "source": [
    "## Absolute Orientation Problem and Alignment\n",
    "\n",
    "Since our point clouds $p$ and $q$ are actually the same point cloud in two different coordinate systems, there is a rigid-body transformation that transforms one exactly into the other. Finding this transformation is called absolute orientation problem and for known associations, i.e., which point in q corresponds to which point in p, there are direct solutions. One direct approach is to translate by the difference between centroids and then decompose the cross co-variance matrix in its rotational parts, combine those and take that as the rotational difference."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "ade4687b-a62c-466d-92db-92fdcdcf693f",
   "metadata": {},
   "outputs": [],
   "source": [
    "def cross_cov(p, q, matches=None):\n",
    "    n = p.shape[1]\n",
    "    assert q.shape[1] == n, \"dimension mismatch in cross_cov()\"\n",
    "    if matches == None:\n",
    "        matches = [(i, i) for i in range(p.shape[0])]\n",
    "    cov = np.zeros((n, n))\n",
    "    for i, j in matches:\n",
    "        pi = p[i]\n",
    "        qj = q[j]\n",
    "        cov += np.outer(pi, qj)\n",
    "    return cov\n",
    "\n",
    "\n",
    "# centroids\n",
    "pm = p.mean(axis=0)\n",
    "qm = q.mean(axis=0)\n",
    "\n",
    "# cross co-variance\n",
    "cc = cross_cov(p - pm, q - qm)\n",
    "\n",
    "# rotational parts u and v\n",
    "u, s, v = np.linalg.svd(cc)\n",
    "\n",
    "# rotation matrix\n",
    "r = u.dot(v)\n",
    "\n",
    "# translation vector\n",
    "t = pm - r.dot(qm)\n",
    "\n",
    "# transformation\n",
    "q1 = r.dot(q.T).T + t"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "057bab57-94f3-4f85-9b29-8819b66a74e6",
   "metadata": {},
   "source": [
    "Let us see how $q1$ compares to $q$ and $p$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "82b69b04-09a8-4ea2-8b75-de86b2d3876e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.4951071195475887e-31\n"
     ]
    }
   ],
   "source": [
    "# point cloud object\n",
    "q1o = o3d.geometry.PointCloud()\n",
    "q1o.points = o3d.utility.Vector3dVector(q1)\n",
    "q1o.paint_uniform_color([0, 1, 0.5])\n",
    "\n",
    "# visualize all three point clouds\n",
    "o3d.visualization.draw_geometries([po, qo, q1o])\n",
    "\n",
    "# the same with a tiny z offset\n",
    "q1o = o3d.geometry.PointCloud()\n",
    "q1o.points = o3d.utility.Vector3dVector(q1 + np.array([0, 0, 0.01]))\n",
    "q1o.paint_uniform_color([0, 1, 0.5])\n",
    "o3d.visualization.draw_geometries([po, qo, q1o])\n",
    "\n",
    "# mean square error\n",
    "print(np.mean((p - q1)**2))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f850e8a0-56e8-4ae5-94f9-751979afba69",
   "metadata": {},
   "source": [
    "This works perfectly because data associations are perfect. However, often associations are not known and have to be estimated. This leads to the concept of ICP:\n",
    "\n",
    "1. Associations are estimated by proximity.\n",
    "2. The resulting transformation brings point clouds closer together.\n",
    "3. Iterate over previous steps until point clouds converge.\n",
    "\n",
    "For one iteration and with a KD-tree for efficient nearest neighbor queries this could be implemented in the following way."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "c95d8e76-d573-4907-b69b-14518b7593f2",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.0732030625502793\n"
     ]
    }
   ],
   "source": [
    "from scipy.spatial import cKDTree as kdtree\n",
    "\n",
    "# compute nearest neighbors\n",
    "tr = kdtree(p - pm)\n",
    "dd, ii = tr.query(q - qm, k=1, workers=-1)\n",
    "\n",
    "# match points in p to points in q\n",
    "matches = [(ii[i], i) for i in range(ii.shape[0])]\n",
    "\n",
    "# compute transformation\n",
    "cc = cross_cov(p - pm, q - qm, matches)\n",
    "u, s, v = np.linalg.svd(cc)\n",
    "r = u.dot(v)\n",
    "t = pm - r.dot(qm)\n",
    "q1 = r.dot(q.T).T + t\n",
    "\n",
    "# mean square error\n",
    "print(np.mean((p - q1)**2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "4298f29c-2539-4f77-973c-3f5d1efe1727",
   "metadata": {},
   "outputs": [],
   "source": [
    "# point cloud object\n",
    "q1o = o3d.geometry.PointCloud()\n",
    "q1o.points = o3d.utility.Vector3dVector(q1)\n",
    "q1o.paint_uniform_color([0, 1, 0.5])\n",
    "\n",
    "# visualize all three point clouds\n",
    "o3d.visualization.draw_geometries([po, qo, q1o])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bfa9c547-9bbb-429f-9d2d-65cc75888ef8",
   "metadata": {},
   "source": [
    "## ICP Implementation\n",
    "\n",
    "With a for loop over several iterations we can implement the ICP concept in the following function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "0949c6d2-594f-4bee-afef-ecf41955b436",
   "metadata": {},
   "outputs": [],
   "source": [
    "def icp_svd(p, q, iterations=10):\n",
    "    def matches(tr, q):\n",
    "        dd, ii = tr.query(q, k=1, workers=-1)\n",
    "        return [(ii[i], i) for i in range(ii.shape[0])]\n",
    "\n",
    "    nq = q.shape[0]\n",
    "    qs = np.zeros((iterations * nq, 4))\n",
    "    pm = p.mean(axis=0)\n",
    "    tr = kdtree(p - pm)\n",
    "    qj = q.copy()\n",
    "    for j in range(iterations):\n",
    "        qm = qj.mean(axis=0)\n",
    "        cc = cross_cov(p - pm, qj - qm, matches(tr, qj - qm))\n",
    "        u, s, v = np.linalg.svd(cc)\n",
    "        r = u.dot(v)\n",
    "        t = pm - r.dot(qm)\n",
    "        qj = r.dot(qj.T).T + t\n",
    "        qs[j*nq:(j+1)*nq] = np.c_[qj, j * np.ones(nq)]\n",
    "\n",
    "    return qs\n",
    "\n",
    "\n",
    "qs = icp_svd(p, q, iterations = 10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "016bee16-ec49-4972-89a9-145c00c8ce79",
   "metadata": {},
   "source": [
    "That function returns not just the final transformed $q$, but also all intermediate versions. This way we can visualize the progress of the algorithm and how it converges. For the ten different $q$-transformations we color points according to its iteration in a color gradient."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "fb8f4feb-34bd-4f43-94c7-3891bd2835f0",
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as pl\n",
    "\n",
    "# color gradient\n",
    "cmap = pl.cm.viridis\n",
    "\n",
    "# point cloud object for icp svd iterations\n",
    "qso = o3d.geometry.PointCloud()\n",
    "qso.points = o3d.utility.Vector3dVector(qs[:,:3])\n",
    "qso.colors = o3d.utility.Vector3dVector(cmap(qs[:,3]/qs[:,3].max())[:,:3])\n",
    "o3d.visualization.draw_geometries([po, qo, qso])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6abbb33b-66ea-4226-93ae-4ce45b8ef7cd",
   "metadata": {},
   "source": [
    "We can also color each point of all $q$-transformations according to its distance $d$ to $p$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f3fe9ea-1522-4dc7-b455-e84274aa36b4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# number of iterations (e.g., 10)\n",
    "niter = qs.shape[0]//p.shape[0]\n",
    "\n",
    "# distance d\n",
    "d = np.sqrt(np.sum((qs[:,:3] - np.tile(p, (niter, 1)))**2, axis = 1))\n",
    "\n",
    "# color gradient\n",
    "cmap = pl.cm.magma_r\n",
    "\n",
    "# point cloud object for icp svd iterations\n",
    "qso = o3d.geometry.PointCloud()\n",
    "qso.points = o3d.utility.Vector3dVector(qs[:,:3])\n",
    "qso.colors = o3d.utility.Vector3dVector(cmap(d/d.max())[:,:3])\n",
    "o3d.visualization.draw_geometries([po, qo, qso])\n",
    "\n",
    "# mean distance per iteration\n",
    "d.shape = (niter, p.shape[0])\n",
    "d = np.mean(d, axis = 1)\n",
    "\n",
    "fg, ax = pl.subplots()\n",
    "ax.plot(d)\n",
    "ax.set_xlabel(\"Iterations\")\n",
    "ax.set_ylabel(\"Mean distance\")\n",
    "ax.set_title(\"Convergence\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a120d41-e48e-4a46-a9ff-5c71027a51fe",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.10.4"
  },
  "toc-autonumbering": false,
  "toc-showcode": false,
  "toc-showmarkdowntxt": false,
  "toc-showtags": false
 },
 "nbformat": 4,
 "nbformat_minor": 5
}