{
    "cells":[
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "# ICP for point cloud alignment",
                "\n",
                "In this tutorial we will learn to align several point ",
                "clouds using two variants of the Iterative Closest Point ",
                "(ICP) [[1]](#References) algorithm."
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We begin with loading the required modules."
            ]
        },
        {
            "cell_type":"code",
            "execution_count":null,
            "metadata":{},
            "outputs":[],
            "source":[
                "import numpy as np\n",
                "\n",
                "from pyoints import (\n",
                "\tstorage,\n",
                "\tExtent,\n",
                "\ttransformation,\n",
                "\tfilters,\n",
                "\tregistration,\n",
                "\tnormals,\n",
                ")"
            ]
        },
        {
            "cell_type":"code",
            "execution_count":null,
            "metadata":{},
            "outputs":[

            ],
            "source":[
                "from mpl_toolkits import mplot3d\n",
                "import matplotlib.pyplot as plt\n",
                "from matplotlib import animation\n",
                "from IPython.display import HTML\n",
                "\n",
                "%matplotlib inline"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "## Preparing the data\n",
                "We load three point clouds of the ",
                "Standfort Bunny [[2]](#References) dataset. The ",
                "original PLY files have been converted to a binary format ",
                "to spare disc space and speed up loading time."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "A = storage.loadPly('bun000_binary.ply')\n",
                "print(A.shape)\n",
                "print(A.dtype.descr)"
            ]
        },
                {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "B = storage.loadPly('bun045_binary.ply')\n",
                "print(B.shape)\n",
                "print(B.dtype.descr)"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "C = storage.loadPly('bun090_binary.ply')\n",
                "print(C.shape)\n",
                "print(C.dtype.descr)"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We filter the point cloud to receive sparse point clouds ",
                "more suitable for visualization. Thinning the point clouds ",
                "also speeds up the ICP algorithm."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "r = 0.004\n",
                "A = A[list(filters.ball(A.indexKD(), r))]\n",
                "B = B[list(filters.ball(B.indexKD(), r))]\n",
                "C = C[list(filters.ball(C.indexKD(), r))]"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "Before we visualize the point cloud, we define the colors ",
                "and axes limits."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "axes_lims = Extent([\n",
                "\tA.extent().center - 0.5 * A.extent().ranges.max(),\n",
                "\tA.extent().center + 0.5 * A.extent().ranges.max()\n",
                "])\n",
                "colors = {'A': 'green', 'B': 'blue', 'C': 'red'}"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 15))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "ax.set_xlabel('X (m)')\n",
                "ax.set_ylabel('Y (m)')\n",
                "ax.set_zlabel('Z (m)')\n",
                "\n",
                "ax.scatter(*A.coords.T, color=colors['A'])\n",
                "ax.scatter(*B.coords.T, color=colors['B'])\n",
                "ax.scatter(*C.coords.T, color=colors['C'])\n",
                "plt.show()"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We can see, that the point clouds B and C are rotated by 45 ",
                "and 90 degree. Since the ICP algorithm assumes already ",
                "roughly aligned point clouds as an input, we rotate the ",
                "point clouds accordingly. But, to harden the problem a bit, ",
                "we use slightly differing rotation angles."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "T_A = transformation.r_matrix([90*np.pi/180, 0, 0])\n",
                "A.transform(T_A)\n",
                "T_B = transformation.r_matrix([86*np.pi/180, 0, 45*np.pi/180])\n",
                "B.transform(T_B)\n",
                "T_C = transformation.r_matrix([95*np.pi/180, 0, 90*np.pi/180])\n",
                "C.transform(T_C)\n"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We update axes limits and visualize the point clouds again. ",
                "The resulting point clouds serve as input for the ICP ",
                "algorithm."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "axes_lims = Extent([\n",
                "\tA.extent().center - 0.5 * A.extent().ranges.max(),\n",
                "\tA.extent().center + 0.5 * A.extent().ranges.max()\n",
                "])"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 15))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "ax.set_xlabel('X')\n",
                "ax.set_ylabel('Y')\n",
                "ax.set_zlabel('Z')\n",
                "\n",
                "ax.scatter(*A.coords.T, color=colors['A'], label='A')\n",
                "ax.scatter(*B.coords.T, color=colors['B'], label='B')\n",
                "ax.scatter(*C.coords.T, color=colors['C'], label='C')\n",
                "ax.legend()\n",
                "plt.show()"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "## ICP algorithm\n",
                "We begin with preparing the input data. Our ICP ",
                "implementation expects a dictionary of point sets as ",
                "an input."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "coords_dict = {\n",
                    "\t'A': A.coords,\n",
                    "\t'B': B.coords,\n",
                    "\t'C': C.coords\n",
                "}"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "First, we initialize an ICP object. The algorithm ",
                "iteratively matches the 'k' closest points. To limit the ",
                "ratio of mismatched points, the 'radii' parameter is ",
                "provided. It defines an ellipsoid within points can be ",
                "assigned."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "d_th = 0.04\n",
                "radii = [d_th, d_th, d_th]\n",
                "icp = registration.ICP(\n",
                    "\tradii,\n",
                    "\tmax_iter=60,\n",
                    "\tmax_change_ratio=0.000001,\n",
                    "\tk=1\n",
                ")"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "After initialization, we apply the ICP algorithm to our ",
                "dataset. The algorithm returns three dictionaries. ",
                "The first dictionary provides the final roto-translation ",
                "matrices of the point clouds. The second specifies the ",
                "corresponding point matches. The third dictionary gives ",
                "some information on the convergence of the algorithm."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "T_dict, pairs_dict, report = icp(coords_dict)"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "Let's visualize the resulting point sets and the Root Mean ",
                "Squared Error (RMSE)."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 15))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "ax.set_xlabel('X')\n",
                "ax.set_ylabel('Y')\n",
                "ax.set_zlabel('Z')\n",
                "\n",
                "for key in coords_dict:\n",
                    "\tcoords = transformation.transform(coords_dict[key], T_dict[key])\n",
                    "\tax.scatter(*coords.T, color=colors[key], label=key)\n",
                "ax.legend()\n",
                "plt.show()"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 8))\n",
                "plt.xlim(0, len(report['RMSE']) + 1)\n",
                "plt.xlabel('Iteration')\n",
                "plt.ylabel('RMSE')\n",
                "\n",
                "plt.bar(np.arange(len(report['RMSE']))+1, report['RMSE'], color='gray')\n",
                "plt.show()"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "Although the result is not perfect, the point clouds have ",
                "been aligned. The remaining misalignment is most likely ",
                "caused by a lot of mismatched points."
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "## NICP algorithm\n",
                "Inspired by the idea of a Normal ICP algorithm proposed by ",
                "Serafin and Grisetti [[3]](#References) [[4]](#References), ",
                "a ICP variant has been developed which considers the ",
                "surface orientation of the points."
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We begin with calculating and displaying the surface normals."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "normals_dict = {\n",
                    "\tkey: normals.fit_normals(coords_dict[key], k=5, preferred=[0, -1, 0])\n",
                    "\tfor key in coords_dict\n",
                "}"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 15))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "ax.set_xlabel('X')\n",
                "ax.set_ylabel('Y')\n",
                "ax.set_zlabel('Z')\n",
                "\n",
                "ax.scatter(*A.coords.T, c=normals_dict['A'][:, 2], cmap='coolwarm')\n",
                "for coord, normal in zip(coords_dict['A'], normals_dict['A']):\n",
                    "\tax.plot(*np.vstack([coord, coord + normal*0.01]).T, color='black')\n",
                "plt.show()"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "The surface orientation should reduce the ratio of ",
                "mismatched points. Now, we create the NICP instance. ",
                "A six dimensional ellipsoid is defined which takes into ",
                "account the point distances, as well as the normal ",
                "differences."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "n_th = np.sin(15 * np.pi / 180)\n",
                "radii = [d_th, d_th, d_th, n_th, n_th, n_th]\n",
                "nicp = registration.ICP(\n",
                    "\tradii,\n",
                    "\tmax_iter=60,\n",
                    "\tmax_change_ratio=0.000001,\n",
                    "\tupdate_normals=True,\n",
                    "\tk=1\n",
                ")"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "The NICP instance is used to apply the algorithm."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "T_dict, pairs_dict, report = nicp(coords_dict, normals_dict)"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 15))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "ax.set_xlabel('X')\n",
                "ax.set_ylabel('Y')\n",
                "ax.set_zlabel('Z')\n",
                "\n",
                "for key in coords_dict:\n",
                    "\tcoords = transformation.transform(coords_dict[key], T_dict[key])\n",
                    "\tax.scatter(*coords.T, color=colors[key], label=key)\n",
                "ax.legend()\n",
                "plt.show()"
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(15, 8))\n",
                "plt.xlim(0, len(report['RMSE']) + 1)\n",
                "plt.xlabel('Iteration')\n",
                "plt.ylabel('RMSE')\n",
                "\n",
                "plt.bar(np.arange(len(report['RMSE']))+1, report['RMSE'], color='gray')\n",
                "plt.show()"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "We can see, that the convergence rate of the NICP algorithm ",
                "is much better compared to the traditional ICP algorithm. ",
                "In particular, the alignment is more plausible."
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "Finally, we create an animation to visualize the convergence."
            ]
        },
        {
            "cell_type": "code",
            "execution_count": null,
            "metadata": {},
            "outputs": [],
            "source": [
                "fig = plt.figure(figsize=(8, 8))\n",
                "ax = plt.axes(projection='3d')\n",
                "ax.set_xlim(axes_lims[0], axes_lims[3])\n",
                "ax.set_ylim(axes_lims[1], axes_lims[4])\n",
                "ax.set_zlim(axes_lims[2], axes_lims[5])\n",
                "fig.tight_layout()\n",
                "\n",
                "# initializing plot\n",
                "artists={\n",
                    "\tkey: ax.plot([],[],[], '.', color=colors[key], label=key)[0]\n",
                    "\tfor key in coords_dict\n",
                "}\n",
                "ax.legend()\n",
                "\n",
                "# collecting the roto-translation matrices\n",
                "T_iter = [{key: np.eye(4) for key in coords_dict}] + report['T']\n",
                "\n",
                "def animate(i):\n",
                    "\t# updates the frame\n",
                    "\tax.set_xlabel('Iteration %i' % i)\n",
                    "\tfor key in coords_dict:\n",
                        "\t\tcoords = transformation.transform(coords_dict[key], T_iter[i][key])\n",
                        "\t\tartists[key].set_data(coords[:, 0], coords[:, 1])\n",
                        "\t\tartists[key].set_3d_properties(coords[:, 2])\n",
                    "\treturn artists.values()\n",
                "\n",
                "# creates the animation\n",
                "anim = animation.FuncAnimation(fig, animate, frames=range(len(T_iter)), interval=250, blit=True)\n",
                "\n",
                "# save as GIF\n",
                "anim.save('nicp.gif', writer=animation.ImageMagickWriter())\n",
                "plt.close()\n",
                "# display as HTML (online version only)\n",
                "HTML(anim.to_jshtml())"
            ]
        },
        {
            "cell_type":"markdown",
            "metadata":{},
            "source":[
                "## References\n",
                "[1] P.J. Besl and N.D. McKay (1992): 'A Method for ",
                "Registration of 3-D Shapes', IEEE Transactions on Pattern ",
                "Analysis and Machine Intelligence, vol. 14 (2): 239-256.\n",
                "\n",
                "[2] Stanford University Computer Graphics Laboratory, (1993): ",
                "'Stanford Bunny', ",
                "URL [https://graphics.stanford.edu/data/3Dscanrep/](https://graphics.stanford.edu/data/3Dscanrep/)\n",
                "(accessed January 17, 2019).\n",
                "\n",
                "[3] J. Serafin and G. Grisetti (2014): ",
                "'Using augmented measurements to improve the convergence of icp', ",
                "International Conference on Simulation, Modeling, and ",
                "Programming for Autonomous Robots. ",
                "Springer, Cham: 566-577.\n",
                "\n",
                "[4] J. Serafin and G. Grisetti (2014): ",
                "'NICP: Dense normal based pointcloud registration', ",
                "International Conference on Intelligent Robots and Systems (IROS): ",
                "742-749."
            ]
        }
    ],
    "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.6.4"
        }
    },
    "nbformat":4,
    "nbformat_minor":2
}