{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": { "nbsphinx": "hidden" }, "outputs": [], "source": [ "import open3d as o3d\n", "import numpy as np\n", "import copy\n", "import os\n", "import sys\n", "\n", "# monkey patches visualization and provides helpers to load geometries\n", "sys.path.append('..')\n", "import open3d_tutorial as o3dtut\n", "# change to True if you want to interact with the visualization windows\n", "o3dtut.interactive = not \"CI\" in os.environ" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# ICP registration\n", "This tutorial demonstrates the ICP (Iterative Closest Point) registration algorithm. It has been a mainstay of geometric registration in both research and industry for many years. The input are two point clouds and an initial transformation that roughly aligns the source point cloud to the target point cloud. The output is a refined transformation that tightly aligns the two point clouds. A helper function `draw_registration_result` visualizes the alignment during the registration process. In this tutorial, we show two ICP variants, the point-to-point ICP and the point-to-plane ICP [\\[Rusinkiewicz2001\\]](../reference.html#rusinkiewicz2001)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Helper visualization function\n", "The function below visualizes a target point cloud and a source point cloud transformed with an alignment transformation. The target point cloud and the source point cloud are painted with cyan and yellow colors respectively. The more and tighter the two point clouds overlap with each other, the better the alignment result." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def draw_registration_result(source, target, transformation):\n", " source_temp = copy.deepcopy(source)\n", " target_temp = copy.deepcopy(target)\n", " source_temp.paint_uniform_color([1, 0.706, 0])\n", " target_temp.paint_uniform_color([0, 0.651, 0.929])\n", " source_temp.transform(transformation)\n", " o3d.visualization.draw_geometries([source_temp, target_temp],\n", " zoom=0.4459,\n", " front=[0.9288, -0.2951, -0.2242],\n", " lookat=[1.6784, 2.0612, 1.4451],\n", " up=[-0.3402, -0.9189, -0.1996])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "
\n", " \n", "**Note:** \n", "\n", "Since the functions `transform` and `paint_uniform_color` change the point cloud, we call `copy.deepcopy` to make copies and protect the original point clouds.\n", "\n", "
" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Input\n", "The code below reads a source point cloud and a target point cloud from two files. A rough transformation is given.\n", "\n", "
\n", " \n", "**Note:** \n", "\n", "The initial alignment is usually obtained by a global registration algorithm. See [Global registration](../pipelines/global_registration.rst) for examples.\n", "\n", "
" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "demo_icp_pcds = o3d.data.DemoICPPointClouds()\n", "source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])\n", "target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])\n", "threshold = 0.02\n", "trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],\n", " [-0.139, 0.967, -0.215, 0.7],\n", " [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])\n", "draw_registration_result(source, target, trans_init)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The function `evaluate_registration` calculates two main metrics:\n", "\n", "- `fitness`, which measures the overlapping area (# of inlier correspondences / # of points in target). The higher the better.\n", "- `inlier_rmse`, which measures the RMSE of all inlier correspondences. The lower the better." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Initial alignment\")\n", "evaluation = o3d.pipelines.registration.evaluate_registration(\n", " source, target, threshold, trans_init)\n", "print(evaluation)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Point-to-point ICP\n", "In general, the ICP algorithm iterates over two steps:\n", "\n", "1. Find correspondence set $\\mathcal{K}=\\{(\\mathbf{p}, \\mathbf{q})\\}$ from target point cloud $\\mathbf{P}$, and source point cloud $\\mathbf{Q}$ transformed with current transformation matrix $\\mathbf{T}$.\n", "2. Update the transformation $\\mathbf{T}$ by minimizing an objective function $E(\\mathbf{T})$ defined over the correspondence set $\\mathcal{K}$.\n", "\n", "Different variants of ICP use different objective functions $E(\\mathbf{T})$ [\\[BeslAndMcKay1992\\]](../reference.html#beslandmckay1992) [\\[ChenAndMedioni1992\\]](../reference.html#chenandmedioni1992) [\\[Park2017\\]](../reference.html#park2017).\n", "\n", "We first show a point-to-point ICP algorithm [\\[BeslAndMcKay1992\\]](../reference.html#beslandmckay1992) using the objective\n", "\n", "\\begin{equation}\n", "E(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\|\\mathbf{p} - \\mathbf{T}\\mathbf{q}\\|^{2}\n", "\\end{equation}\n", "\n", "The class `TransformationEstimationPointToPoint` provides functions to compute the residuals and Jacobian matrices of the point-to-point ICP objective. The function `registration_icp` takes it as a parameter and runs point-to-point ICP to obtain the results." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Apply point-to-point ICP\")\n", "reg_p2p = o3d.pipelines.registration.registration_icp(\n", " source, target, threshold, trans_init,\n", " o3d.pipelines.registration.TransformationEstimationPointToPoint())\n", "print(reg_p2p)\n", "print(\"Transformation is:\")\n", "print(reg_p2p.transformation)\n", "draw_registration_result(source, target, reg_p2p.transformation)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The `fitness` score increases from 0.174723 to 0.372450. The `inlier_rmse` reduces from 0.011771 to 0.007760. By default, `registration_icp` runs until convergence or reaches a maximum number of iterations (30 by default). It can be changed to allow more computation time and to improve the results further." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "reg_p2p = o3d.pipelines.registration.registration_icp(\n", " source, target, threshold, trans_init,\n", " o3d.pipelines.registration.TransformationEstimationPointToPoint(),\n", " o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000))\n", "print(reg_p2p)\n", "print(\"Transformation is:\")\n", "print(reg_p2p.transformation)\n", "draw_registration_result(source, target, reg_p2p.transformation)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The final alignment is tight. The `fitness` score improves to 0.621123. The `inlier_rmse` reduces to 0.006583." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Point-to-plane ICP\n", "The point-to-plane ICP algorithm [\\[ChenAndMedioni1992\\]](../reference.html#chenandmedioni1992) uses a different objective function\n", "\n", "\\begin{equation}\n", "E(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{p}}\\big)^{2},\n", "\\end{equation}\n", "\n", "where $\\mathbf{n}_{\\mathbf{p}}$ is the normal of point $\\mathbf{p}$. [\\[Rusinkiewicz2001\\]](../reference.html#rusinkiewicz2001) has shown that the point-to-plane ICP algorithm has a faster convergence speed than the point-to-point ICP algorithm.\n", "\n", "`registration_icp` is called with a different parameter `TransformationEstimationPointToPlane`. Internally, this class implements functions to compute the residuals and Jacobian matrices of the point-to-plane ICP objective." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Apply point-to-plane ICP\")\n", "reg_p2l = o3d.pipelines.registration.registration_icp(\n", " source, target, threshold, trans_init,\n", " o3d.pipelines.registration.TransformationEstimationPointToPlane())\n", "print(reg_p2l)\n", "print(\"Transformation is:\")\n", "print(reg_p2l.transformation)\n", "draw_registration_result(source, target, reg_p2l.transformation)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The point-to-plane ICP reaches tight alignment within 30 iterations (a `fitness` score of 0.620972 and an `inlier_rmse` score of 0.006581).\n", "\n", "
\n", " \n", "The point-to-plane ICP algorithm uses point normals. In this tutorial, we load normals from files. If normals are not given, they can be computed with [Vertex normal estimation](pointcloud.ipynb#vertex-normal-estimation).\n", "\n", "
" ] } ], "metadata": { "celltoolbar": "Edit 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": 2 }