{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# ICP #\n", "This notebook is all about ICP and it's different implementations. It should be visual and self - descriptive.\n", "\n", "## Contents:\n", "* [Overview](#Overview)\n", "* [ICP based on SVD](#ICP-based-on-SVD)\n", "* [Non linear Least squares based ICP](#Non-linear-Least-squares-based-ICP)\n", "* [Using point to plane metric with Least Squares ICP](#Using-point-to-plane-metric-with-Least-Squares-ICP)\n", "* [Dealing with outliers](#Dealing-with-outliers)\n", "\n", "## Overview\n", "Having two scans $P = \\{p_i\\}$ and $Q = \\{q_i\\}$ we want to find a transformation (rotation $R$ and translation $t$) to apply to $P$ to match $Q$ as good as possible. In the remainder of this notebook we will try to define what does \"as good as possible mean\" as well as ways to find such a transformation." ] }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "import sys\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from matplotlib import animation, rc\n", "from math import sin, cos, atan2, pi\n", "from IPython.display import display, Math, Latex, Markdown, HTML" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### The way we will plot the data" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "def plot_data(data_1, data_2, label_1, label_2, markersize_1=8, markersize_2=8):\n", " fig = plt.figure(figsize=(10, 6))\n", " ax = fig.add_subplot(111)\n", " ax.axis('equal')\n", " if data_1 is not None:\n", " x_p, y_p = data_1\n", " ax.plot(x_p, y_p, color='#336699', markersize=markersize_1, marker='o', linestyle=\":\", label=label_1)\n", " if data_2 is not None:\n", " x_q, y_q = data_2\n", " ax.plot(x_q, y_q, color='orangered', markersize=markersize_2, marker='o', linestyle=\":\", label=label_2)\n", " ax.legend()\n", " return ax\n", "\n", "def plot_values(values, label):\n", " fig = plt.figure(figsize=(10, 4))\n", " ax = fig.add_subplot(111)\n", " ax.plot(values, label=label)\n", " ax.legend()\n", " ax.grid(True)\n", " plt.show()\n", " \n", "def animate_results(P_values, Q, corresp_values, xlim, ylim):\n", " \"\"\"A function used to animate the iterative processes we use.\"\"\"\n", " fig = plt.figure(figsize=(10, 6))\n", " anim_ax = fig.add_subplot(111)\n", " anim_ax.set(xlim=xlim, ylim=ylim)\n", " anim_ax.set_aspect('equal')\n", " plt.close()\n", " x_q, y_q = Q\n", " # draw initial correspondeces\n", " corresp_lines = []\n", " for i, j in correspondences:\n", " corresp_lines.append(anim_ax.plot([], [], 'grey')[0])\n", " # Prepare Q data.\n", " Q_line, = anim_ax.plot(x_q, y_q, 'o', color='orangered')\n", " # prepare empty line for moved data\n", " P_line, = anim_ax.plot([], [], 'o', color='#336699')\n", "\n", " def animate(i):\n", " P_inc = P_values[i]\n", " x_p, y_p = P_inc\n", " P_line.set_data(x_p, y_p)\n", " draw_inc_corresp(P_inc, Q, corresp_values[i])\n", " return (P_line,)\n", " \n", " def draw_inc_corresp(points_from, points_to, correspondences):\n", " for corr_idx, (i, j) in enumerate(correspondences):\n", " x = [points_from[0, i], points_to[0, j]]\n", " y = [points_from[1, i], points_to[1, j]]\n", " corresp_lines[corr_idx].set_data(x, y)\n", " \n", " anim = animation.FuncAnimation(fig, animate,\n", " frames=len(P_values), \n", " interval=500, \n", " blit=True)\n", " return HTML(anim.to_jshtml())" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Generate example data\n", "Thoughout this notebook we will be working wigh generated data that looks like this:" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [ { "output_type": "display_data", "data": { "text/plain": "