notebook/registration-call-script.ipynb


{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "ancient-cement",
   "metadata": {},
   "source": [
    "# TEASER++ with headless\n",
    "\n",
    "Jupyter may not support rendering in some case but we need output result, so we need to set make options in `CMake`. Before start, you should clone `TEASER-plusplus` repository first :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "listed-porter",
   "metadata": {},
   "outputs": [],
   "source": [
    "!git clone --recursive https://github.com/MIT-SPARK/TEASER-plusplus.git"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "internal-excitement",
   "metadata": {},
   "source": [
    "Create build directory :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "international-payday",
   "metadata": {},
   "outputs": [],
   "source": [
    "!cd TEASER-plusplus && mkdir -p build && cd build"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "soviet-grammar",
   "metadata": {},
   "source": [
    "Set build options:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "changed-gates",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-- Enable printing of diagnostic messages.\n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build/googletest-download\n",
      "[100%] Built target googletest\n",
      "CMake Deprecation Warning at build/googletest-src/CMakeLists.txt:1 (cmake_minimum_required):\n",
      "  Compatibility with CMake < 2.8.12 will be removed from a future version of\n",
      "  CMake.\n",
      "\n",
      "  Update the VERSION argument <min> value or use a ...<max> suffix to tell\n",
      "  CMake that the project does not need compatibility with older versions.\n",
      "\n",
      "\n",
      "CMake Deprecation Warning at build/googletest-src/googlemock/CMakeLists.txt:42 (cmake_minimum_required):\n",
      "  Compatibility with CMake < 2.8.12 will be removed from a future version of\n",
      "  CMake.\n",
      "\n",
      "  Update the VERSION argument <min> value or use a ...<max> suffix to tell\n",
      "  CMake that the project does not need compatibility with older versions.\n",
      "\n",
      "\n",
      "CMake Deprecation Warning at build/googletest-src/googletest/CMakeLists.txt:49 (cmake_minimum_required):\n",
      "  Compatibility with CMake < 2.8.12 will be removed from a future version of\n",
      "  CMake.\n",
      "\n",
      "  Update the VERSION argument <min> value or use a ...<max> suffix to tell\n",
      "  CMake that the project does not need compatibility with older versions.\n",
      "\n",
      "\n",
      "-- Found PythonInterp: /usr/local/bin/python (found version \"3.6.9\") \n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build/pmc-download\n",
      "[ 11%] Performing update step for 'pmc'\n",
      "[ 22%] No patch step for 'pmc'\n",
      "[ 33%] No configure step for 'pmc'\n",
      "[ 44%] No build step for 'pmc'\n",
      "[ 55%] No install step for 'pmc'\n",
      "[ 66%] No test step for 'pmc'\n",
      "[ 77%] Completed 'pmc'\n",
      "[100%] Built target pmc\n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build/tinyply-download\n",
      "[ 11%] Performing update step for 'pmc'\n",
      "[ 22%] No patch step for 'pmc'\n",
      "[ 33%] No configure step for 'pmc'\n",
      "[ 44%] No build step for 'pmc'\n",
      "[ 55%] No install step for 'pmc'\n",
      "[ 66%] No test step for 'pmc'\n",
      "[ 77%] Completed 'pmc'\n",
      "[100%] Built target pmc\n",
      "CMake Deprecation Warning at build/tinyply-src/CMakeLists.txt:1 (cmake_minimum_required):\n",
      "  Compatibility with CMake < 2.8.12 will be removed from a future version of\n",
      "  CMake.\n",
      "\n",
      "  Update the VERSION argument <min> value or use a ...<max> suffix to tell\n",
      "  CMake that the project does not need compatibility with older versions.\n",
      "\n",
      "\n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build/spectra-download\n",
      "[ 11%] Performing update step for 'spectral'\n",
      "[ 22%] No patch step for 'spectral'\n",
      "[ 33%] No configure step for 'spectral'\n",
      "[ 44%] No build step for 'spectral'\n",
      "[ 55%] No install step for 'spectral'\n",
      "[ 66%] No test step for 'spectral'\n",
      "[ 77%] Completed 'spectral'\n",
      "[100%] Built target spectral\n",
      "-- Failed to find sphinx-build executable (missing: SPHINX_EXECUTABLE) \n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build/pybind11-download\n",
      "[ 11%] Performing update step for 'pmc'\n",
      "[ 22%] No patch step for 'pmc'\n",
      "[ 33%] No configure step for 'pmc'\n",
      "[ 44%] No build step for 'pmc'\n",
      "[ 55%] No install step for 'pmc'\n",
      "[ 66%] No test step for 'pmc'\n",
      "[ 77%] Completed 'pmc'\n",
      "[100%] Built target pmc\n",
      "-- Found PythonInterp: /usr/local/bin/python (found suitable version \"3.6.9\", minimum required is \"3.6\") \n",
      "-- pybind11 v2.4.3\n",
      "-- TEASER++ Python binding will be built.\n",
      "-- Python Interpreter Version: 3.6\n",
      "-- Configuring done\n",
      "-- Generating done\n",
      "-- Build files have been written to: /app/notebook/bom/TEASER-plusplus/build\n",
      "[ 60%] Built target pmc\n",
      "[ 86%] Built target teaser_registration\n",
      "[100%] Built target teaserpp_python\n"
     ]
    }
   ],
   "source": [
    "!cd TEASER-plusplus/build && cmake -DENABLE_HEADLESS_RENDERING=ON \\\n",
    "                 -DBUILD_GUI=OFF \\\n",
    "                 -DUSE_SYSTEM_GLEW=OFF \\\n",
    "                 -DUSE_SYSTEM_GLFW=OFF \\\n",
    "                 .. && make teaserpp_python\n",
    "\n",
    "# [optional for normal case] :\n",
    "# !cd TEASER-plusplus/build && cmake -DTEASERPP_PYTHON_VERSION=3.6 .. && make teaserpp_python"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "controlled-harmony",
   "metadata": {},
   "source": [
    "Install dependencies:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "purple-record",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Processing /app/notebook/bom/TEASER-plusplus/build/python\n",
      "Building wheels for collected packages: teaserpp-python\n",
      "  Building wheel for teaserpp-python (setup.py) ... \u001b[?25ldone\n",
      "\u001b[?25h  Created wheel for teaserpp-python: filename=teaserpp_python-1.0.0-py3-none-any.whl size=96787 sha256=f9e2ea4de487bf147bf4b0c8d0ba4ab20e05427163c7d25994e47ae914724582\n",
      "  Stored in directory: /tmp/pip-ephem-wheel-cache-4ov5nct4/wheels/63/7f/01/46fd10fab12e4155da32528e9bc54173b032a2639ced68522e\n",
      "Successfully built teaserpp-python\n",
      "Installing collected packages: teaserpp-python\n",
      "  Attempting uninstall: teaserpp-python\n",
      "    Found existing installation: teaserpp-python 1.0.0\n",
      "    Uninstalling teaserpp-python-1.0.0:\n",
      "      Successfully uninstalled teaserpp-python-1.0.0\n",
      "Successfully installed teaserpp-python-1.0.0\n"
     ]
    }
   ],
   "source": [
    "!cd TEASER-plusplus/build && cd python && pip install ."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "residential-bahamas",
   "metadata": {},
   "outputs": [],
   "source": [
    "!pip3 -q install open3d==0.9.0 scikit-learn numpy matplotlib opencv-python"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "driven-alliance",
   "metadata": {},
   "source": [
    "Let's test robust registration :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "isolated-grounds",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "==================================================\n",
      "        TEASER++ Python registration example      \n",
      "==================================================\n",
      "Starting scale solver.\n",
      "Scale estimation complete.\n",
      "Max core number: 751\n",
      "Num vertices: 1890\n",
      "*** [pmc heuristic: thread 1]   current max clique = 602,  time = 0.00973892 sec\n",
      "*** [pmc heuristic: thread 1]   current max clique = 607,  time = 0.086772 sec\n",
      "*** [pmc heuristic: thread 10]   current max clique = 609,  time = 0.109637 sec\n",
      "*** [pmc heuristic: thread 27]   current max clique = 610,  time = 0.178972 sec\n",
      "[pmc heuristic]\t mc = 610\n",
      "Created adjacency matrix in 0.00241804 seconds\n",
      "[pmc: initial k-core pruning]  before pruning: |V| = 1889, |E| = 327867\n",
      "[pmc: initial k-core pruning]  after pruning:  |V| = 1889, |E| = 327818\n",
      "[pmc]  initial pruning took 0.00223303 sec\n",
      "[pmc: sorting neighbors]  |E| = 655636, |E_sorted| = 655636\n",
      "|V| = 814\n",
      ">>> [pmc: thread 20]\n",
      "[pmc: bounds updated - thread [pmc: bounds updated - thread 31]  time = 130.019861 sec, ]  time = |V| = 1879 (10 / 1889), |E| = 327818, w = 610, p = 0.183835, d_min = 0, d_avg = 0.019877 sec, |V| = 1886 (3 / 1889), |E| = 327818, w = 610, p = 347.133, d_max = 813, k_max = 7510.183835\n",
      ", d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 20]  time = 0.021276 sec, |V| = 1884 (5 / 1889), |E| = 327818, w = 610, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 14]  time = 0.0322189 sec, |V| = 1888 (1 / 1889), |E| = 327818, w = 610, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "*** [pmc: thread 3]   current max clique = 620,  time = 0.111119 sec\n",
      "[pmc: bounds updated - thread 28]  time = 0.114884 sec, |V| = 1854 (35 / 1889), |E| = 327818, w = 620, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 19]  time = 0.114952 sec, |V| = 1885 (4 / 1889), |E| = 327818, w = 620, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 16]  time = [pmc: bounds updated - thread 32]  time = 0.115065 sec, |V| = 1883 (6 / 1889), |E| = 327818, w = 620, p = 0.183835, d_min = 0.115068 sec, |V| = 01872, d_avg =  (17 / 1889), |E| = 327818347.133, w = , d_max = 620813, p = , k_max = 751\n",
      "0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 17]  time = 0.115132 sec, |V| = 1866 (23 / 1889), |E| = 327818, w = 620, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "[pmc: bounds updated - thread 15]  time = 0.115165 sec, |V| = 1877 (12 / 1889), |E| = 327818, w = 620, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "*** [pmc: thread 4]   current max clique = 622,  time = 0.174745 sec\n",
      "*** [pmc: thread 25]   current max clique = 626,  time = 0.178253 sec\n",
      "*** [pmc: thread 14]   current max clique = 627,  time = 0.222178 sec\n",
      "[pmc: bounds updated - thread 25]  time = 0.225257 sec, |V| = 1861 (28 / 1889), |E| = 327818, w = 627, p = 0.183835, d_min = 0, d_avg = 347.133, d_max = 813, k_max = 751\n",
      "*** [pmc: thread 26]   current max clique = 628,  time = 0.281434 sec\n",
      "*** [pmc: thread 15]   current max clique = 629,  time = 0.283731 sec\n",
      "*** [pmc: thread 7]   current max clique = 630,  time = 0.350826 sec\n",
      "-----------------------------------------------------------------------\n",
      "Max Clique of scale estimation inliers: \n",
      "Using chain graph for GNC rotation.\n",
      "Starting rotation solver.\n",
      "GNC rotation estimation noise bound:0.1\n",
      "GNC rotation estimation noise bound squared:0.01\n",
      "GNC-TLS solver terminated due to cost convergence.\n",
      "Cost diff: 0\n",
      "Iterations: 25\n",
      "Rotation estimation complete.\n",
      "Starting translation solver.\n",
      "Translation estimation complete.\n",
      "=====================================\n",
      "          TEASER++ Results           \n",
      "=====================================\n",
      "Expected rotation: \n",
      "[[ 0.99692656  0.06687358 -0.04066644]\n",
      " [-0.06612899  0.99761788  0.01940087]\n",
      " [ 0.04186755 -0.01665178  0.99897777]]\n",
      "Estimated rotation: \n",
      "[[ 0.99702146  0.02654314 -0.07241321]\n",
      " [-0.02796141  0.99943514 -0.01864276]\n",
      " [ 0.07187747  0.020612    0.99720047]]\n",
      "Error (deg): \n",
      "0.0627323834905469\n",
      "Expected translation: \n",
      "[-0.11557694 -0.03877054  0.11487489]\n",
      "Estimated translation: \n",
      "[-0.11087659 -0.04013182  0.11341195]\n",
      "Error (m): \n",
      "0.005107495251783068\n",
      "Number of correspondences:  1889\n",
      "Number of outliers:  1700\n",
      "Time taken (s):  0.6712841987609863\n",
      "Segmentation fault (core dumped)\n"
     ]
    }
   ],
   "source": [
    "!cd TEASER-plusplus/examples/teaser_python_ply && \\\n",
    "python teaser_python_ply.py\n",
    "\n",
    "# !cd TEASER-plusplus/examples/teaser_python_3dsmooth && \\\n",
    "# python teaser_python_3dsmooth.py"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "original-studio",
   "metadata": {},
   "source": [
    "---------------------------------------"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "functioning-match",
   "metadata": {},
   "source": [
    "### Memory issue\n",
    "\n",
    "Technically, It should not consider as a bug, more like an algorithm failure.\n",
    "I compare Teaser++ with ICP kind registration like the T-RO.\n",
    "ICP kind method can handle 50k points (nearest point correspondence ) with no problem.\n",
    "However, Teaser++ was crushed with a huge memory leak.\n",
    "I trace it to\n",
    "\n",
    "registration.cc\n",
    "\n",
    "```c\n",
    "   computeTIMs(const Eigen::Matrix<double, 3, Eigen::Dynamic>& v,\n",
    "                                          Eigen::Matrix<int, 2, Eigen::Dynamic>* map)\n",
    "```\n",
    "\n",
    "and find\n",
    "map->resize(2, N * (N - 1) / 2);\n",
    "Eigen::Matrix<double, 3, Eigen::Dynamic> vtilde(3, N * (N - 1) / 2);\n",
    "\n",
    "and in loop\n",
    "\n",
    "```c\n",
    "// populate the index map\n",
    "Eigen::Matrix<int, 2, Eigen::Dynamic> map_addition(2, N);\n",
    "for (size_t j = 0; j < N; ++j) {\n",
    "  map_addition(0, j) = i;\n",
    "  map_addition(1, j) = j;\n",
    "}\n",
    "```\n",
    "\n",
    "if N = 50k, vtilde,map will be 500000^2 / 2.\n",
    "In the following loop, the memory reached its limit.\n",
    "\n",
    "Ref. https://github.com/MIT-SPARK/TEASER-plusplus/issues/70"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "optimum-jimmy",
   "metadata": {},
   "source": [
    "### [Optional] Test with our custom script\n",
    "\n",
    "I provided custom script(in current directory) to test registration function, you can pass 3 arguments:\n",
    " - source point cloud file.\n",
    " - target point cloud file.\n",
    " - number of point of point cloud\n",
    " \n",
    "note: the script just an example, don't use in production."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "matched-graduation",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "==================================================\n",
      "        TEASER++ Python registration example      \n",
      "==================================================\n",
      "src_pcs: 060c4f8c4dec4c7688d713bde9f0cfab_0.ply\n",
      "dst_pcs: 1611745410data_060c4f8c4dec4c7688d713bde9f0cfab_frame0.ply\n",
      "src: (3, 2000)\n",
      "dst: (3, 2000)\n",
      "Starting scale solver.\n",
      "Scale estimation complete.\n",
      "Max core number: 2\n",
      "Num vertices: 2001\n",
      "*** [pmc heuristic: thread 4]   current max clique = 3,  time = 5.79357e-05 sec\n",
      "[pmc heuristic]\t mc = 3\n",
      "Max Clique of scale estimation inliers: \n",
      "Using chain graph for GNC rotation.\n",
      "Starting rotation solver.\n",
      "GNC rotation estimation noise bound:0.1\n",
      "GNC rotation estimation noise bound squared:0.01\n",
      "GNC-TLS terminated because maximum residual at initialization is very small.\n",
      "Rotation estimation complete.\n",
      "Starting translation solver.\n",
      "Translation estimation complete.\n",
      "=====================================\n",
      "          TEASER++ Results           \n",
      "=====================================\n",
      "Estimated rotation: \n",
      "[[ 0.78706034  0.39811942  0.47120797]\n",
      " [-0.60617249  0.64081732  0.47107119]\n",
      " [-0.11441564 -0.65639476  0.74569094]]\n",
      "Estimated translation: \n",
      "[-106.51897544  -74.71157668 -312.90982502]\n",
      "Time taken (s):  0.06768655776977539\n"
     ]
    }
   ],
   "source": [
    "!python teaser_pp.py \\\n",
    "'060c4f8c4dec4c7688d713bde9f0cfab_0.ply' \\\n",
    "'1611745410data_060c4f8c4dec4c7688d713bde9f0cfab_frame0.ply' \\\n",
    "2000"
   ]
  }
 ],
 "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.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}