notebooks/03-1_CV_Deprojection.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%env OPEN3D_DISABLE_WEB_VISUALIZER=true\n",
    "\n",
    "import numpy as np\n",
    "import cv2\n",
    "import json\n",
    "import open3d as o3d\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Prepare the function to display the image and points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def display_image(image, title='Image'):\n",
    "    plt.figure()\n",
    "    plt.imshow(image)\n",
    "    plt.title(title)\n",
    "    plt.axis('off')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Read RGB Image\n",
    "\n",
    "- When we use the `cv2.imread()` function to read an image, it reads the image in the BGR format.\n",
    "- For each color channel, the pixel values range from 0 to 255.\n",
    "- We can convert the image to RGB format using the `cv2.cvtColor()` function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_rgb_image(image_file):\n",
    "    image = cv2.imread(image_file, cv2.IMREAD_COLOR)    # By default, OpenCV reads images in BGR format\n",
    "    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)    # Convert BGR to RGB\n",
    "    return image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "image_file = \"../demo/recordings/20231022_193630/105322251564/color_000000.jpg\"\n",
    "rgb_img = read_rgb_image(image_file)\n",
    "\n",
    "print(\"RGB Image Shape: \", rgb_img.shape)\n",
    "print(\"RGB Image Type: \", rgb_img.dtype)\n",
    "\n",
    "display_image(rgb_img, title='RGB Image')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Read Depth Image\n",
    "\n",
    "- The depth value is stored in the 16-bit image in mm.\n",
    "- Normally, we need to convert it to meters by dividing by 1000."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_depth_image(image_file):\n",
    "    depth_image = cv2.imread(image_file, cv2.IMREAD_ANYDEPTH)\n",
    "    return depth_image\n",
    "\n",
    "depth_image_file = \"../demo/recordings/20231022_193630/105322251564/depth_000000.png\"\n",
    "depth_img = read_depth_image(depth_image_file)\n",
    "\n",
    "print(\"Depth Image Shape: \", depth_img.shape)\n",
    "print(\"Depth Image Type: \", depth_img.dtype)\n",
    "print(\"Depth Image range: \", np.min(depth_img), np.max(depth_img))\n",
    "\n",
    "depth_img = depth_img.astype(np.float32) / 1000.0    # Convert depth values from mm to meters\n",
    "print(\"Depth Image range: \", np.min(depth_img), np.max(depth_img))\n",
    "\n",
    "display_image(depth_img, title='Depth Image')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Load the K matrix for the \n",
    "\n",
    "- The camera intrinsic is stored as a JSON file in following format.\n",
    "- We can load the JSON file using the `json.load()` function.\n",
    "- Because the depth image is aligned with the RGB image, we can use the same color intrinsics for both images.\n",
    "- The loaded data is stored in a dictionary format. We will create the $K$ matrix from the dictionary.\n",
    "\n",
    "```json\n",
    "{\n",
    "    \"serial\": \"105322251564\",\n",
    "    \"color\": {\n",
    "        \"width\": 640,\n",
    "        \"height\": 480,\n",
    "        \"fx\": 378.30926513671875,\n",
    "        \"fy\": 378.024658203125,\n",
    "        \"ppx\": 323.66534423828125,\n",
    "        \"ppy\": 253.562744140625,\n",
    "        \"coeffs\": [\n",
    "            -0.05498598515987396,\n",
    "            0.06602264940738678,\n",
    "            -0.000507863238453865,\n",
    "            0.0001927213161252439,\n",
    "            -0.020694954320788383\n",
    "        ]\n",
    "    },\n",
    "    \"depth\": ...,\n",
    "    \"depth2color\": ...\n",
    "}\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_data_from_json(json_file):\n",
    "    with open(str(json_file), \"r\", encoding=\"utf-8\") as f:\n",
    "        data = json.load(f)\n",
    "    return data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Read intrinsics from JSON file as a dictionary\n",
    "json_file = \"../demo/calibration/intrinsics/105322251564_640x480.json\"\n",
    "intrinsics = read_data_from_json(json_file)\n",
    "\n",
    "print(intrinsics)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the camera matrix\n",
    "K = np.array(\n",
    "    [\n",
    "        [intrinsics[\"color\"][\"fx\"], 0, intrinsics[\"color\"][\"ppx\"]],\n",
    "        [0, intrinsics[\"color\"][\"fy\"], intrinsics[\"color\"][\"ppy\"]],\n",
    "        [0, 0, 1],\n",
    "    ],\n",
    "    dtype=np.float32\n",
    ")\n",
    "\n",
    "print(\"Camera K Matrix: \\n\", K)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create Point Cloud from Depth Image\n",
    "\n",
    "#### Camera Intrinsics\n",
    "\n",
    "The camera intrinsics matrix $K$ relates the 3D coordinates in the camera frame to the 2D coordinates in the image plane. It includes the focal lengths $(f_x, f_y)$ and the principal point offsets $(c_x, c_y)$.\n",
    "\n",
    "$K = \\begin{bmatrix} f_x & 0 & c_x \\\\ 0 & f_y & c_y \\\\ 0 & 0 & 1 \\end{bmatrix}$\n",
    "\n",
    "#### Camera Intrinsics Inverse\n",
    "\n",
    "The inverse of the camera intrinsics matrix $K^{-1}$ can be used for backprojection.\n",
    "\n",
    "$K^{-1} = \\begin{bmatrix} 1/f_x & 0 & -c_x/f_x \\\\ 0 & 1/f_y & -c_y/f_y \\\\ 0 & 0 & 1 \\end{bmatrix}$\n",
    "\n",
    "#### Camera Extrinsics\n",
    "\n",
    "The camera extrinsics matrix $[R|t]$ describes the transformation from the world coordinate system to the camera coordinate system. It includes a rotation matrix $R$ and a translation vector $t$.\n",
    "\n",
    "$[R|t] = \\begin{bmatrix} R & t \\\\ 0 & 1 \\end{bmatrix}$\n",
    "\n",
    "#### Projection of a 3D Point (x, y, z) to a 2D Point (u, v)\n",
    "\n",
    "- To project a 3D point $(x, y, z)$ in the camera coordinate system to a 2D point $(u, v)$ in the image plane:\n",
    "\n",
    "$\\begin{bmatrix} u \\\\ v \\\\ 1 \\end{bmatrix} = K \\cdot \\begin{bmatrix} x \\\\ y \\\\ z \\end{bmatrix}$\n",
    "\n",
    "- Expanding this equation, we get:\n",
    "\n",
    "$\\begin{bmatrix} u \\\\ v \\\\ 1 \\end{bmatrix} = \\begin{bmatrix} f_x \\cdot \\frac{x}{z} + c_x \\\\ f_y \\cdot \\frac{y}{z} + c_y \\\\ 1 \\end{bmatrix}$\n",
    "\n",
    "#### Backprojection of a 2D Point (u, v) to a 3D Point (x, y, z)\n",
    "\n",
    "- To backproject a 2D point $(u, v)$ from the image plane to a 3D point $(x, y, z)$ in the camera coordinate system, given a depth value $z$:\n",
    "\n",
    "$\\begin{bmatrix} x \\\\ y \\\\ z \\end{bmatrix} = K^{-1} \\cdot \\begin{bmatrix} u \\\\ v \\\\ 1 \\end{bmatrix} \\cdot z$\n",
    "\n",
    "- Expanding this equation, we get:\n",
    "\n",
    "$\\begin{bmatrix} x \\\\ y \\\\ z \\end{bmatrix} = \\begin{bmatrix} (u - c_x) \\cdot \\frac{z}{f_x} \\\\ (v - c_y) \\cdot \\frac{z}{f_y} \\\\ z \\end{bmatrix}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Given the $(u, v)$ pixel coordinates in the depth image and the depth value $d$ at that pixel, we can deproject the pixel to a 3D point in the camera coordinate frame.\n",
    "\n",
    "$\\begin{bmatrix} x \\\\ y \\\\ z \\end{bmatrix} = \\begin{bmatrix} (u - c_x) \\cdot \\frac{z}{f_x} \\\\ (v - c_y) \\cdot \\frac{z}{f_y} \\\\ z \\end{bmatrix}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_xyz_from_uvd(u, v, d, fx, fy, cx, cy):\n",
    "    x = (u - cx) * d / fx\n",
    "    y = (v - cy) * d / fy\n",
    "    z = d\n",
    "    return np.array([x, y, z], dtype=np.float32)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "points = []\n",
    "\n",
    "# Get the points by iterating over the depth image\n",
    "height, width = depth_img.shape\n",
    "for u in range(width):\n",
    "    for v in range(height):\n",
    "        d = depth_img[v, u]\n",
    "        xyz = get_xyz_from_uvd(u, v, d, K[0, 0], K[1, 1], K[0, 2], K[1, 2])\n",
    "        points.append(xyz)\n",
    "\n",
    "# Convert the list of points to a numpy array\n",
    "points = np.array(points)\n",
    "\n",
    "print(\"Points Shape: \", points.shape, points.dtype)\n",
    "\n",
    "# Display the point cloud\n",
    "pcd = o3d.geometry.PointCloud()\n",
    "pcd.points = o3d.utility.Vector3dVector(points)\n",
    "pcd.paint_uniform_color([0.0, 0.0, 0.7])\n",
    "o3d.visualization.draw([pcd], point_size=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the lookup table for the depth image, we can get the depth value at each pixel. We can then use the camera intrinsics to convert the pixel to a 3D point, but it takes a lot of time to do this for every pixel in the image.\n",
    "\n",
    "Instead, we will use the matrix multiplication to convert the depth image to a point cloud in a single step.\n",
    "\n",
    "${cam\\_coords} = {K\\_inv} \\cdot {pixel\\_coords} \\cdot {depths}$\n",
    "\n",
    "$P$ = $K^{-1} \\cdot \\begin{bmatrix} u_1 & ... & u_{H \\cdot W} \\\\ v_1 & ... & u_{H \\cdot W} \\\\ 1 & ... & 1 \\end{bmatrix} \\cdot \\begin{bmatrix} d_1 \\\\ ... \\\\ d_{H \\cdot W}\\end{bmatrix}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def deproject_depth_image(depth_img, K):\n",
    "    height, width = depth_img.shape\n",
    "\n",
    "    # use function np.linalg.inv to compute the inverse of the camera intrinsic matrix\n",
    "    K_inv = np.linalg.inv(K)\n",
    "\n",
    "    # use function np.meshgrid to construct the pixel coordinates\n",
    "    # and then convert the pixel coordinates into homogenenous coordinates\n",
    "    u, v = np.meshgrid(np.arange(width), np.arange(height), indexing='xy')\n",
    "    ones = np.ones((height, width))\n",
    "    pixel_coords = np.stack([u, v, ones], axis=-1, dtype=np.float32)   # (height, width, 3)\n",
    "\n",
    "    # reshape the pixel coordinates to a 2D array\n",
    "    pixel_coords = pixel_coords.reshape(-1, 3)    # (height*width, 3)\n",
    "    pixel_coords = pixel_coords.T    # (3, height*width)\n",
    "\n",
    "    # reshape the depth image to a 1D array\n",
    "    depths = depth_img.reshape(-1)   # d is a 1D array\n",
    "\n",
    "    # compute the 3D coordinates of the pixels\n",
    "    cam_coords = (K_inv @ pixel_coords) * depths\n",
    "    cam_coords = cam_coords.T    # (height*width, 3)\n",
    "\n",
    "    return cam_coords"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "points = deproject_depth_image(depth_img, K)\n",
    "\n",
    "print(\"Points Shape: \", points.shape, points.dtype)\n",
    "\n",
    "# Display the point cloud\n",
    "pcd = o3d.geometry.PointCloud()\n",
    "pcd.points = o3d.utility.Vector3dVector(points)\n",
    "pcd.paint_uniform_color([0.0, 0.0, 0.7])\n",
    "o3d.visualization.draw([pcd], point_size=1)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "summer_camp",
   "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.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}