airo-camera-toolkit/notebooks/point_cloud_tutorial.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Point Cloud Tutorial\n",
    "\n",
    "In this notebook we will cover:\n",
    "1. Loading RGBD data and creating an [Open3D](http://www.open3d.org/) point cloud\n",
    "2. Removing points with inaccurate depth\n",
    "3. Cropping and downsampling a point cloud\n",
    "4. Getting the highest, lowest and random points.\n",
    "5. Project points from 3D to 2D\n",
    "\n",
    "We will use data of a robot holding a shirt in the air, prerecorded with a ZED 2i.\n",
    "\n",
    "> Download this data from [here](https://ugentbe-my.sharepoint.com/:f:/g/personal/victorlouis_degusseme_ugent_be/EkIZoyySsnZBg56hRq1BqdkBuGlvhAwPWT9HDuqaUB-psA?e=iSehj6) and save the folder in a folder called `data` relative to this notebook.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "data_dir = os.path.join(\"data\", \"competition_sample_0000\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "!ls $data_dir"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import open3d as o3d\n",
    "from ipywidgets import interact\n",
    "\n",
    "from airo_typing import NumpyDepthMapType\n",
    "from airo_dataset_tools.data_parsers.camera_intrinsics import CameraIntrinsics\n",
    "from airo_dataset_tools.data_parsers.pose import Pose\n",
    "from airo_camera_toolkit.point_clouds.conversions import open3d_to_point_cloud, point_cloud_to_open3d\n",
    "from airo_camera_toolkit.point_clouds.operations import filter_point_cloud, crop_point_cloud\n",
    "from airo_camera_toolkit.point_clouds.visualization import open3d_point\n",
    "from airo_camera_toolkit.pinhole_operations.projection import project_points_to_image_plane\n",
    "from airo_typing import PointCloudPositionsType"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "intrinsics_path = os.path.join(data_dir, \"intrinsics.json\")\n",
    "image_left_path = os.path.join(data_dir, \"image_left.png\")\n",
    "image_right_path = os.path.join(data_dir, \"image_right.png\")\n",
    "depth_map_path = os.path.join(data_dir, \"depth_map.tiff\")\n",
    "confidence_map_path = os.path.join(data_dir, \"confidence_map.tiff\")\n",
    "point_cloud_path = os.path.join(data_dir, \"pointcloud.ply\")\n",
    "camera_pose_path = os.path.join(data_dir, \"camera_pose.json\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Loading the data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.1 Loading the camera parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "with open(intrinsics_path, \"r\") as f:\n",
    "    intrinsics_model = CameraIntrinsics.model_validate_json(f.read())\n",
    "    intrinsics = intrinsics_model.as_matrix()\n",
    "    resolution = intrinsics_model.image_resolution.as_tuple()\n",
    "\n",
    "with open(camera_pose_path, \"r\") as f:\n",
    "    camera_pose = Pose.model_validate_json(f.read()).as_homogeneous_matrix()\n",
    "\n",
    "with np.printoptions(precision=3, suppress=True):\n",
    "    print(\"Resolution:\", resolution)\n",
    "    print(\"Intrinsics: \\n\", intrinsics)\n",
    "    print(\"Extrinsics: \\n\", camera_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.2 Loading the color images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "image_left = plt.imread(image_left_path) # you can also use cv2.imread but then you get BGR instead of RGB\n",
    "image_right = plt.imread(image_right_path)\n",
    "\n",
    "plt.figure(figsize=(20, 10))\n",
    "plt.subplot(1, 2, 1)\n",
    "plt.imshow(image_left)\n",
    "plt.title(\"Left image\")\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.imshow(image_right)\n",
    "plt.title(\"Right image\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.3 Loading the depth and confidence map\n",
    "\n",
    "> Note: the confidence map has range [0.0, 100.0] where 0.0 is the **most confident**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_map = cv2.imread(depth_map_path, cv2.IMREAD_ANYDEPTH)\n",
    "confidence_map = cv2.imread(confidence_map_path, cv2.IMREAD_ANYDEPTH)\n",
    "\n",
    "print(\"depth_map.dtype:\", depth_map.dtype)\n",
    "print(\"confidence_map.dtype:\", confidence_map.dtype)\n",
    "\n",
    "plt.figure(figsize=(20, 10))\n",
    "plt.subplot(1, 2, 1)\n",
    "plt.imshow(depth_map)\n",
    "plt.title(\"Depth map\")\n",
    "plt.colorbar(fraction=0.025, pad=0.04)\n",
    "plt.subplot(1, 2, 2)\n",
    "plt.imshow(confidence_map)\n",
    "plt.title(\"Confidence map\")\n",
    "plt.colorbar(fraction=0.025, pad=0.04)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.4 Loading the point cloud\n",
    "\n",
    "Open3D uses the abbreviation `pcd` for their [PointCloud](http://www.open3d.org/docs/release/python_api/open3d.geometry.PointCloud.html#open3d.geometry.PointCloud) object, we will use this too to distinguish between them and numpy point_clouds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd_in_camera = o3d.t.io.read_point_cloud(point_cloud_path)\n",
    "\n",
    "# This conversion to float32 can be removed once the data is saved as float32\n",
    "pcd_in_camera.point.positions = o3d.core.Tensor(pcd_in_camera.point.positions.numpy().astype(np.float32))\n",
    "\n",
    "pcd = pcd_in_camera.transform(camera_pose) # transform to world frame (= base frame of left robot)\n",
    "pcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries([pcd.to_legacy()])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.5 Numpy point clouds\n",
    "\n",
    "Open3D provides a lot functionality for point clouds, see their [tutorial](http://www.open3d.org/docs/release/tutorial/geometry/pointcloud.html).\n",
    "However sometimes you need something custom, (e.g. getting the lowest and highest points).\n",
    "This can be done easily by converting the Open3D point cloud to numpy arrays.\n",
    "We've also found that some operations such as filtering with a boolean mask are faster in numpy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "point_cloud = open3d_to_point_cloud(pcd)\n",
    "points, colors = point_cloud.points, point_cloud.colors\n",
    "\n",
    "print(\"points:\", points.shape, points.dtype)\n",
    "if colors is not None:\n",
    "    print(\"colors:\", colors.shape, colors.dtype)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Removing low confidence points"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.1 Binarizing the confidence map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def binarize_confidence(image: NumpyDepthMapType, threshold=50.0):\n",
    "    confident = image <= threshold\n",
    "    return confident\n",
    "\n",
    "\n",
    "@interact(threshold=(0.0, 100.0, 1.0))\n",
    "def show_confidence_binarized(threshold=50.0):\n",
    "    confidence_binarized = binarize_confidence(confidence_map, threshold)\n",
    "    confidence_image = confidence_binarized.astype(np.uint8) * 255\n",
    "    plt.figure(figsize=(10, 5))\n",
    "    plt.imshow(confidence_image, vmin=0, vmax=255)\n",
    "    plt.colorbar(fraction=0.025, pad=0.04)\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "threshold = 1.0  # a value of 1.0 means only the most confidence points will be kept\n",
    "confidence_binarized = binarize_confidence(confidence_map, threshold)\n",
    "confidence_mask = confidence_binarized.reshape(-1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "confidence_mask.shape, confidence_mask.dtype, confidence_mask[:5], "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 Filtering with a binary map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "point_cloud_filtered = filter_point_cloud(point_cloud, confidence_mask)\n",
    "\n",
    "n = len(point_cloud.points)\n",
    "n_filtered = len(point_cloud_filtered.points)\n",
    "\n",
    "print(f\"Number of points: {n} -> {n_filtered} ({n_filtered / n * 100:.1f}%)\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries([point_cloud_to_open3d(point_cloud_filtered).to_legacy()])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Cropping and downsampling"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.1 Cropping"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bbox = (0.35, -0.3, 0.1), (0.7, 0.1, 0.95)\n",
    "\n",
    "point_cloud_cropped = crop_point_cloud(point_cloud_filtered, bbox)\n",
    "\n",
    "n = point_cloud.points.shape[0]\n",
    "n_cropped = point_cloud_cropped.points.shape[0]\n",
    "\n",
    "print(f\"Number of points: {n} -> {n_cropped} ({n_cropped / n * 100:.1f}%)\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bbox_o3d = o3d.geometry.AxisAlignedBoundingBox(*bbox)\n",
    "bbox_o3d.color = (1.0, 0.0, 1.0)\n",
    "\n",
    "o3d.visualization.draw_geometries([point_cloud_to_open3d(point_cloud_cropped).to_legacy(), bbox_o3d])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2 Downsampling"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd_cropped = point_cloud_to_open3d(point_cloud_cropped)\n",
    "pcd_downsampled = pcd_cropped.voxel_down_sample(voxel_size=0.01)\n",
    "\n",
    "n_cropped = len(pcd_cropped.point.positions)\n",
    "n_downsampled = len(pcd_downsampled.point.positions)\n",
    "\n",
    "\n",
    "print(f\"Number of points: {n_cropped} -> {n_downsampled} ({n_downsampled / n_cropped * 100:.2f}%)\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries([pcd_downsampled.to_legacy()])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Getting the highest, lowest and random points\n",
    "\n",
    "### 4.1 Highest and lowest points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def highest_point(points: PointCloudPositionsType) -> np.ndarray:\n",
    "    return points[np.argmax(points[:, 2])]\n",
    "\n",
    "def lowest_point(points: PointCloudPositionsType) -> np.ndarray:\n",
    "    return points[np.argmin(points[:, 2])]\n",
    "\n",
    "highest = highest_point(point_cloud_cropped.points)\n",
    "lowest = lowest_point(point_cloud_cropped.points)\n",
    "\n",
    "with np.printoptions(precision=3, suppress=True):\n",
    "    print(\"Highest point:\", highest)\n",
    "    print(\"Lowest point:\", lowest)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "o3d.visualization.draw_geometries(\n",
    "    [pcd_cropped.to_legacy(), open3d_point(highest, (0.0, 1.0, 0.0)), open3d_point(lowest, (1.0, 0.0, 0.0))]\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2 Random points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# set random seed for reproducibility\n",
    "np.random.seed(0)\n",
    "\n",
    "random_indices = np.random.choice(len(point_cloud_cropped.points), size=10, replace=False)\n",
    "random_indices"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "random_points = point_cloud_cropped.points[random_indices]\n",
    "\n",
    "open3d_points = [open3d_point(p, (0.0, 0.0, 1.0)) for p in random_points]\n",
    "\n",
    "o3d.visualization.draw_geometries([pcd_cropped.to_legacy(), *open3d_points])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Projection\n",
    "\n",
    "### 5.1 Projecting points from 3D to 2D"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from airo_spatial_algebra import transform_points\n",
    "from airo_spatial_algebra.operations import _HomogeneousPoints\n",
    "\n",
    "# the points are currentlty expressed in the world frame p_W, we need p_C\n",
    "# p_C = X_C_W @ p_W\n",
    "X_C_W = np.linalg.inv(camera_pose)\n",
    "\n",
    "# lowest\n",
    "lowest_2d = project_points_to_image_plane(\n",
    "    transform_points(X_C_W, lowest),\n",
    "    intrinsics,\n",
    ").squeeze()\n",
    "\n",
    "lowest_2d_int = np.rint(lowest_2d).astype(int)\n",
    "lowest_2d, lowest_2d_int"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "highest_2d = project_points_to_image_plane(\n",
    "    transform_points(X_C_W, highest),\n",
    "    intrinsics,\n",
    ").squeeze()\n",
    "\n",
    "random_2d = project_points_to_image_plane(\n",
    "    transform_points(X_C_W, random_points),\n",
    "    intrinsics,\n",
    ")\n",
    "\n",
    "highest_2d_int = np.rint(highest_2d).astype(int)\n",
    "random_2d_int = np.rint(random_2d).astype(int)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "image_annotated = image_left.copy()\n",
    "cv2.circle(image_annotated, tuple(lowest_2d_int), 10, (0, 1.0, 0), thickness=2)\n",
    "cv2.circle(image_annotated, tuple(highest_2d_int), 10, (1.0, 0, 0), thickness=2)\n",
    "for p in random_2d_int:\n",
    "    cv2.circle(image_annotated, tuple(p), 10, (0, 0, 1.0), thickness=2)\n",
    "\n",
    "plt.figure(figsize=(10, 5))\n",
    "plt.imshow(image_annotated)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5.2 Point cloud to segmented image (advanced)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rgbd_image = pcd_cropped.project_to_rgbd_image(*resolution, intrinsics, extrinsics=np.linalg.inv(camera_pose))\n",
    "\n",
    "image_rgb_float = np.asarray(rgbd_image.color)\n",
    "depth_map_float = np.asarray(rgbd_image.depth).squeeze()\n",
    "\n",
    "print(depth_map_float.shape)\n",
    "\n",
    "# make background white where depth is 0.0\n",
    "image_rgb_float[depth_map_float == 0.0] = 1.0\n",
    "\n",
    "plt.figure(figsize=(10, 5))\n",
    "plt.imshow(image_rgb_float)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "execution": {
   "allow_errors": true,
   "timeout": 300
  },
  "kernelspec": {
   "display_name": "airo-mono",
   "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.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}