{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import re\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\n",
    "# if running on Travis CI, the number of iterations is reduced\n",
    "is_ci = \"CI\" in os.environ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Color Map Optimization\n",
    "Consider color mapping to the geometry reconstructed from depth cameras. As color and depth frames are not perfectly aligned, the texture mapping using color images is subject to results in blurred color map. Open3D provides color map optimization method proposed by [\\[Zhou2014\\]](../reference.html#zhou2014). The following script shows an example of color map optimization."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Input\n",
    "This code below reads color and depth image pairs and makes `rgbd_image`. Note that `convert_rgb_to_intensity` flag is `False`. This is to preserve 8-bit color channels instead of using single channel float type image.\n",
    "\n",
    "It is always good practice to visualize the RGBD image before applying it to the color map optimization. The `debug_mode` switch can be set to `True` to visualize the RGBD image."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def load_fountain_dataset():\n",
    "    rgbd_images = []\n",
    "    fountain_rgbd_dataset = o3d.data.SampleFountainRGBDImages()\n",
    "    for i in range(len(fountain_rgbd_dataset.depth_paths)):\n",
    "        depth = o3d.io.read_image(fountain_rgbd_dataset.depth_paths[i])\n",
    "        color = o3d.io.read_image(fountain_rgbd_dataset.color_paths[i])\n",
    "        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(\n",
    "            color, depth, convert_rgb_to_intensity=False)\n",
    "        rgbd_images.append(rgbd_image)\n",
    "\n",
    "    camera_trajectory = o3d.io.read_pinhole_camera_trajectory(\n",
    "        fountain_rgbd_dataset.keyframe_poses_log_path)\n",
    "    mesh = o3d.io.read_triangle_mesh(\n",
    "        fountain_rgbd_dataset.reconstruction_path)\n",
    "\n",
    "    return mesh, rgbd_images, camera_trajectory"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The code below reads a camera trajectory and a mesh."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load dataset\n",
    "mesh, rgbd_images, camera_trajectory = load_fountain_dataset()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To visualize how the camera poses are not good for color mapping, this code intentionally sets the iteration number to 0, which means no optimization. `color_map_optimization` paints a mesh using corresponding RGBD images and camera poses. Without optimization, the texture map is blurred."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Before full optimization, let's visualize texture map\n",
    "# with given geometry, RGBD images, and camera poses.\n",
    "mesh, camera_trajectory = o3d.pipelines.color_map.run_rigid_optimizer(\n",
    "    mesh, rgbd_images, camera_trajectory,\n",
    "    o3d.pipelines.color_map.RigidOptimizerOption(maximum_iteration=0))\n",
    "o3d.visualization.draw_geometries([mesh],\n",
    "                                  zoom=0.5399,\n",
    "                                  front=[0.0665, -0.1107, -0.9916],\n",
    "                                  lookat=[0.7353, 0.6537, 1.0521],\n",
    "                                  up=[0.0136, -0.9936, 0.1118])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Rigid Optimization\n",
    "The next step is to optimize camera poses to get a sharp color map.\n",
    "\n",
    "The code below sets `maximum_iteration = 300` for actual iterations."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimize texture and save the mesh as texture_mapped.ply\n",
    "# This is implementation of following paper\n",
    "# Q.-Y. Zhou and V. Koltun,\n",
    "# Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras,\n",
    "# SIGGRAPH 2014\n",
    "\n",
    "# Run rigid optimization.\n",
    "maximum_iteration = 100 if is_ci else 300\n",
    "with o3d.utility.VerbosityContextManager(\n",
    "        o3d.utility.VerbosityLevel.Debug) as cm:\n",
    "    mesh, camera_trajectory = o3d.pipelines.color_map.run_rigid_optimizer(\n",
    "        mesh, rgbd_images, camera_trajectory,\n",
    "        o3d.pipelines.color_map.RigidOptimizerOption(\n",
    "            maximum_iteration=maximum_iteration))\n",
    "\n",
    "o3d.visualization.draw_geometries([mesh],\n",
    "                                  zoom=0.5399,\n",
    "                                  front=[0.0665, -0.1107, -0.9916],\n",
    "                                  lookat=[0.7353, 0.6537, 1.0521],\n",
    "                                  up=[0.0136, -0.9936, 0.1118])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The residual error implies inconsistency of image intensities. Lower residual leads to a better color map quality. By default, `ColorMapOptimizationOption` enables rigid optimization. It optimizes 6-dimensional pose of every cameras."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Non-rigid Optimization\n",
    "For better alignment quality, there is an option for non-rigid optimization. To enable this option, simply set `option.non_rigid_camera_coordinate` to `True` before calling `color_map_optimization`. Besides 6-dimensional camera poses, non-rigid optimization even considers local image warping represented by anchor points. This adds even more flexibility and leads to an even higher quality color mapping. The residual error is smaller than the case of rigid optimization."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run non-rigid optimization.\n",
    "maximum_iteration = 100 if is_ci else 300\n",
    "with o3d.utility.VerbosityContextManager(\n",
    "        o3d.utility.VerbosityLevel.Debug) as cm:\n",
    "    mesh, camera_trajectory = o3d.pipelines.color_map.run_non_rigid_optimizer(\n",
    "        mesh, rgbd_images, camera_trajectory,\n",
    "        o3d.pipelines.color_map.NonRigidOptimizerOption(\n",
    "            maximum_iteration=maximum_iteration))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries([mesh],\n",
    "                                  zoom=0.5399,\n",
    "                                  front=[0.0665, -0.1107, -0.9916],\n",
    "                                  lookat=[0.7353, 0.6537, 1.0521],\n",
    "                                  up=[0.0136, -0.9936, 0.1118])"
   ]
  }
 ],
 "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.6.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}