{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\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 images\n",
    "Open3D has a data structure for images. It supports various functions such as `read_image`, `write_image`, `filter_image` and `draw_geometries`. An Open3D Image can be directly converted to/from a numpy array.\n",
    "\n",
    "An Open3D `RGBDImage` is composed of two images, `RGBDImage.depth` and `RGBDImage.color`. We require the two images to be registered into the same camera frame and have the same resolution. The following tutorials show how to read and use RGBD images from a number of well known RGBD datasets."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Redwood dataset\n",
    "In this section we show how to read and visualize an `RGBDImage` from the [Redwood dataset](http://redwood-data.org/) [Choi2015]_.\n",
    "\n",
    "The Redwood format stored depth in a 16-bit single channel image. The integer value represents the depth measurement in millimeters. It is the default format for Open3D to parse depth images."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Read Redwood dataset\")\n",
    "color_raw = o3d.io.read_image(\"../../TestData/RGBD/color/00000.jpg\")\n",
    "depth_raw = o3d.io.read_image(\"../../TestData/RGBD/depth/00000.png\")\n",
    "rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(\n",
    "    color_raw, depth_raw)\n",
    "print(rgbd_image)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The default conversion function `create_rgbd_image_from_color_and_depth` creates an RGBDImage from a pair of color and depth image. The color image is converted into a grayscale image, stored in `float` ranged in [0, 1]. The depth image is stored in `float`, representing the depth value in meters.\n",
    "\n",
    "The converted images can be rendered as numpy arrays."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.subplot(1, 2, 1)\n",
    "plt.title('Redwood grayscale image')\n",
    "plt.imshow(rgbd_image.color)\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.title('Redwood depth image')\n",
    "plt.imshow(rgbd_image.depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The RGBD image can be converted into a point cloud, given a set of camera parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "        rgbd_image,\n",
    "        o3d.camera.PinholeCameraIntrinsic(\n",
    "            o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))\n",
    "# Flip it, otherwise the pointcloud will be upside down\n",
    "pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])\n",
    "o3d.visualization.draw_geometries([pcd], zoom=0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we use `PinholeCameraIntrinsicParameters.PrimeSenseDefault` as default camera parameter. It has image resolution 640x480, focal length (fx, fy) = (525.0, 525.0), and optical center (cx, cy) = (319.5, 239.5). An identity matrix is used as the default extrinsic parameter. `pcd.transform` applies an up-down flip transformation on the point cloud for better visualization purpose. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## SUN dataset\n",
    "In this section we show how to read and visualize an RGBDImage of the [SUN dataset](http://rgbd.cs.princeton.edu/) [Song2015]_.\n",
    "\n",
    "This tutorial is almost the same as the tutorial processing Redwood dataset above. The only difference is that we use the conversion function `create_rgbd_image_from_sun_format` to parse depth images in the SUN dataset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Read SUN dataset\")\n",
    "color_raw = o3d.io.read_image(\n",
    "        \"../../TestData/RGBD/other_formats/SUN_color.jpg\")\n",
    "depth_raw = o3d.io.read_image(\n",
    "        \"../../TestData/RGBD/other_formats/SUN_depth.png\")\n",
    "rgbd_image = o3d.geometry.RGBDImage.create_from_sun_format(\n",
    "        color_raw, depth_raw)\n",
    "print(rgbd_image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.subplot(1, 2, 1)\n",
    "plt.title('SUN grayscale image')\n",
    "plt.imshow(rgbd_image.color)\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.title('SUN depth image')\n",
    "plt.imshow(rgbd_image.depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "        rgbd_image,\n",
    "        o3d.camera.PinholeCameraIntrinsic(\n",
    "            o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))\n",
    "# Flip it, otherwise the pointcloud will be upside down\n",
    "pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])\n",
    "o3d.visualization.draw_geometries([pcd], zoom=0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## NYU dataset\n",
    "This section shows how to read and visualize an `RGBDImage` from the [NYU dataset](https://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html) [Silberman2012]_. \n",
    "\n",
    "This tutorial is almost the same as the tutorial processing Redwood dataset above, with two differences. First, NYU images are not in standard `jpg` or `png` formats. Thus, we use `mpimg.imread` to read the color image as a numpy array and convert it to an Open3D `Image`. An additional helper function `read_nyu_pgm` is called to read depth images from the special big endian `pgm` format used in the NYU dataset. Second, we use a different conversion function `create_rgbd_image_from_nyu_format` to parse depth images in the SUN dataset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.image as mpimg\n",
    "import re\n",
    "\n",
    "# This is special function used for reading NYU pgm format\n",
    "# as it is written in big endian byte order.\n",
    "def read_nyu_pgm(filename, byteorder='>'):\n",
    "    with open(filename, 'rb') as f:\n",
    "        buffer = f.read()\n",
    "    try:\n",
    "        header, width, height, maxval = re.search(\n",
    "            b\"(^P5\\s(?:\\s*#.*[\\r\\n])*\"\n",
    "            b\"(\\d+)\\s(?:\\s*#.*[\\r\\n])*\"\n",
    "            b\"(\\d+)\\s(?:\\s*#.*[\\r\\n])*\"\n",
    "            b\"(\\d+)\\s(?:\\s*#.*[\\r\\n]\\s)*)\", buffer).groups()\n",
    "    except AttributeError:\n",
    "        raise ValueError(\"Not a raw PGM file: '%s'\" % filename)\n",
    "    img = np.frombuffer(buffer,\n",
    "                        dtype=byteorder + 'u2',\n",
    "                        count=int(width) * int(height),\n",
    "                        offset=len(header)).reshape((int(height), int(width)))\n",
    "    img_out = img.astype('u2')\n",
    "    return img_out\n",
    "\n",
    "\n",
    "print(\"Read NYU dataset\")\n",
    "# Open3D does not support ppm/pgm file yet. Not using o3d.io.read_image here.\n",
    "# MathplotImage having some ISSUE with NYU pgm file. Not using imread for pgm.\n",
    "color_raw = mpimg.imread(\"../../TestData/RGBD/other_formats/NYU_color.ppm\")\n",
    "depth_raw = read_nyu_pgm(\"../../TestData/RGBD/other_formats/NYU_depth.pgm\")\n",
    "color = o3d.geometry.Image(color_raw)\n",
    "depth = o3d.geometry.Image(depth_raw)\n",
    "rgbd_image = o3d.geometry.RGBDImage.create_from_nyu_format(color, depth)\n",
    "print(rgbd_image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.subplot(1, 2, 1)\n",
    "plt.title('NYU grayscale image')\n",
    "plt.imshow(rgbd_image.color)\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.title('NYU depth image')\n",
    "plt.imshow(rgbd_image.depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "        rgbd_image,\n",
    "        o3d.camera.PinholeCameraIntrinsic(\n",
    "            o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))\n",
    "# Flip it, otherwise the pointcloud will be upside down\n",
    "pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])\n",
    "o3d.visualization.draw_geometries([pcd], zoom=0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## TUM dataset\n",
    "This section shows how to read and visualize an RGBDImage from the [TUM dataset](https://vision.in.tum.de/data/datasets/rgbd-dataset) [Strum2012]_.\n",
    "\n",
    "This tutorial is almost the same as the tutorial processing Redwood dataset above. The only difference is that we use conversion function `create_rgbd_image_from_tum_format` to parse depth images in the TUM dataset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Read TUM dataset\")\n",
    "color_raw = o3d.io.read_image(\n",
    "        \"../../TestData/RGBD/other_formats/TUM_color.png\")\n",
    "depth_raw = o3d.io.read_image(\n",
    "        \"../../TestData/RGBD/other_formats/TUM_depth.png\")\n",
    "rgbd_image = o3d.geometry.RGBDImage.create_from_tum_format(\n",
    "        color_raw, depth_raw)\n",
    "print(rgbd_image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.subplot(1, 2, 1)\n",
    "plt.title('TUM grayscale image')\n",
    "plt.imshow(rgbd_image.color)\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.title('TUM depth image')\n",
    "plt.imshow(rgbd_image.depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd = o3d.geometry.PointCloud.create_from_rgbd_image(\n",
    "        rgbd_image,\n",
    "        o3d.camera.PinholeCameraIntrinsic(\n",
    "            o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))\n",
    "# Flip it, otherwise the pointcloud will be upside down\n",
    "pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])\n",
    "o3d.visualization.draw_geometries([pcd], zoom=0.35)"
   ]
  }
 ],
 "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
}