{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\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": [
    "# RGBD Odometry\n",
    "An RGBD odometry finds the camera movement between two consecutive RGBD image pairs. The input are two instances of `RGBDImage`. The output is the motion in the form of a rigid body transformation. Open3D implements the method of [\\[Steinbrucker2011\\]](../reference.html#steinbrucker2011) and [\\[Park2017\\]](../reference.html#park2017)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Read camera intrinsic\n",
    "We first read the camera intrinsic matrix from a json file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(\n",
    "        \"../../TestData/camera_primesense.json\")\n",
    "print(pinhole_camera_intrinsic.intrinsic_matrix)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-info\">\n",
    "    \n",
    "**Note:** \n",
    "\n",
    "Lots of small data structures in Open3D can be read from / written into `json` files. This includes camera intrinsics, camera trajectory, pose graph, etc.\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Read RGBD image\n",
    "This code block reads two pairs of RGBD images in the Redwood format. We refer to [Redwood dataset](rgbd_image.ipynb#Redwood-dataset) for a comprehensive explanation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "source_color = o3d.io.read_image(\"../../TestData/RGBD/color/00000.jpg\")\n",
    "source_depth = o3d.io.read_image(\"../../TestData/RGBD/depth/00000.png\")\n",
    "target_color = o3d.io.read_image(\"../../TestData/RGBD/color/00001.jpg\")\n",
    "target_depth = o3d.io.read_image(\"../../TestData/RGBD/depth/00001.png\")\n",
    "source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(\n",
    "        source_color, source_depth)\n",
    "target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(\n",
    "        target_color, target_depth)\n",
    "target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "        target_rgbd_image, pinhole_camera_intrinsic)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-info\">\n",
    "    \n",
    "**Note:** \n",
    "\n",
    "Open3D assumes the color image and depth image are synchronized and registered in the same coordinate frame. This can usually be done by turning on both the synchronization and registration features in the RGBD camera settings.\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Compute odometry from two RGBD image pairs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "option = o3d.odometry.OdometryOption()\n",
    "odo_init = np.identity(4)\n",
    "print(option)\n",
    "\n",
    "[success_color_term, trans_color_term, info] = o3d.odometry.compute_rgbd_odometry(\n",
    "         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,\n",
    "         odo_init, o3d.odometry.RGBDOdometryJacobianFromColorTerm(), option)\n",
    "[success_hybrid_term, trans_hybrid_term, info] = o3d.odometry.compute_rgbd_odometry(\n",
    "         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,\n",
    "         odo_init, o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This code block calls two different RGBD odometry methods. The first one is from [\\[Steinbrucker2011\\]](../reference.html#steinbrucker2011). It minimizes photo consistency of aligned images. The second one is from [\\[Park2017\\]](../reference.html#park2017). In addition to photo consistency, it implements constraint for geometry. Both functions run in similar speed. But [\\[Park2017\\]](../reference.html#park2017) is more accurate in our test on benchmark datasets. It is recommended.\n",
    "\n",
    "Several parameters in `OdometryOption()`:\n",
    "\n",
    "- `minimum_correspondence_ratio`: After alignment, measure the overlapping ratio of two RGBD images. If overlapping region of two RGBD image is smaller than specified ratio, the odometry module regards that this is a failure case.\n",
    "- `max_depth_diff`: In depth image domain, if two aligned pixels have a depth difference less than specified value, they are considered as a correspondence. Larger value induce more aggressive search, but it is prone to unstable result.\n",
    "- `min_depth` and `max_depth`: Pixels that has smaller or larger than specified depth values are ignored."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualize RGBD image pairs\n",
    "The RGBD image pairs are converted into point clouds and rendered together. Note that the point cloud representing the first (source) RGBD image is transformed with the transformation estimated by the odometry. After this transformation, both point clouds are aligned."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if success_color_term:\n",
    "    print(\"Using RGB-D Odometry\")\n",
    "    print(trans_color_term)\n",
    "    source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "            source_rgbd_image, pinhole_camera_intrinsic)\n",
    "    source_pcd_color_term.transform(trans_color_term)\n",
    "    o3d.visualization.draw_geometries([target_pcd, source_pcd_color_term],\n",
    "                                      zoom=0.48,\n",
    "                                      front=[0.0999, -0.1787, -0.9788],\n",
    "                                      lookat=[0.0345, -0.0937, 1.8033],\n",
    "                                      up=[-0.0067, -0.9838, 0.1790])\n",
    "if success_hybrid_term:\n",
    "    print(\"Using Hybrid RGB-D Odometry\")\n",
    "    print(trans_hybrid_term)\n",
    "    source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "            source_rgbd_image, pinhole_camera_intrinsic)\n",
    "    source_pcd_hybrid_term.transform(trans_hybrid_term)\n",
    "    o3d.visualization.draw_geometries([target_pcd, source_pcd_hybrid_term],\n",
    "                                      zoom=0.48,\n",
    "                                      front=[0.0999, -0.1787, -0.9788],\n",
    "                                      lookat=[0.0345, -0.0937, 1.8033],\n",
    "                                      up=[-0.0067, -0.9838, 0.1790])"
   ]
  }
 ],
 "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
}