From bd92be8381a3214394aa7eacb724a0f9421ec5df Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Tue, 12 Dec 2023 17:02:28 +0100 Subject: [PATCH] start for point cloud tutorial --- airo-camera-toolkit/.gitignore | 1 + .../point_clouds/conversions.py | 16 + airo-camera-toolkit/docs/rerun.md | 2 +- .../notebooks/point_cloud_tutorial.ipynb | 372 ++++++++++++++++++ .../rerun_zed_tutorial.ipynb} | 0 .../{docs => scripts}/live_charuco_pose.py | 0 6 files changed, 390 insertions(+), 1 deletion(-) create mode 100644 airo-camera-toolkit/airo_camera_toolkit/point_clouds/conversions.py create mode 100644 airo-camera-toolkit/notebooks/point_cloud_tutorial.ipynb rename airo-camera-toolkit/{docs/rerun-zed-example.ipynb => notebooks/rerun_zed_tutorial.ipynb} (100%) rename airo-camera-toolkit/{docs => scripts}/live_charuco_pose.py (100%) diff --git a/airo-camera-toolkit/.gitignore b/airo-camera-toolkit/.gitignore index 5449f3dc..2fa335b4 100644 --- a/airo-camera-toolkit/.gitignore +++ b/airo-camera-toolkit/.gitignore @@ -1,3 +1,4 @@ airo_camera_toolkit/image_transforms/tutorial_image.jpg airo_camera_toolkit/calibration/saved_calibrations **/calibration_20**/ +notebooks/data \ No newline at end of file diff --git a/airo-camera-toolkit/airo_camera_toolkit/point_clouds/conversions.py b/airo-camera-toolkit/airo_camera_toolkit/point_clouds/conversions.py new file mode 100644 index 00000000..96d20721 --- /dev/null +++ b/airo-camera-toolkit/airo_camera_toolkit/point_clouds/conversions.py @@ -0,0 +1,16 @@ +import numpy as np +import open3d as o3d +from airo_typing import ColoredPointCloudType + + +def pointcloud_open3d_to_numpy(pcd: o3d.geometry.PointCloud) -> ColoredPointCloudType: + points = np.asarray(pcd.points).astype(np.float32) + colors = (np.asarray(pcd.colors) * 255).astype(np.uint8) + return points, colors + + +def pointcloud_numpy_to_open3d(pointcloud: ColoredPointCloudType) -> o3d.geometry.PointCloud: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pointcloud[0].astype(np.float64)) + pcd.colors = o3d.utility.Vector3dVector(pointcloud[1].astype(np.float64) / 255) + return pcd diff --git a/airo-camera-toolkit/docs/rerun.md b/airo-camera-toolkit/docs/rerun.md index c3a70ce2..509b306a 100644 --- a/airo-camera-toolkit/docs/rerun.md +++ b/airo-camera-toolkit/docs/rerun.md @@ -11,7 +11,7 @@ rerun.log_image("zed_top", image) rerun.log_scalar("force_z", force[2]) ... ``` -See the [example notebook](./rerun-zed-example.ipynb) for more. +See the [example notebook](../notebooks/rerun-zed-example.ipynb) for more. > :information_source: A note on starting the Rerun viewer: you can start it by calling `rerun.spawn()` from Python. However when starting Rerun like that, [there is no way to specify a memory limit](https://www.rerun.io/docs/howto/limit-ram). This quickly becomes a problem when logging images, so we recommend starting Rerun from a terminal: >``` diff --git a/airo-camera-toolkit/notebooks/point_cloud_tutorial.ipynb b/airo-camera-toolkit/notebooks/point_cloud_tutorial.ipynb new file mode 100644 index 00000000..4d49d276 --- /dev/null +++ b/airo-camera-toolkit/notebooks/point_cloud_tutorial.ipynb @@ -0,0 +1,372 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Point Cloud Tutorial\n", + "\n", + "In this notebook we will cover:\n", + "* Loading RGBD data and creating an [Open3D](http://www.open3d.org/) point cloud\n", + "* Removing points with inaccurate depth\n", + "* Cropping and downsampling a point cloud\n", + "* Getting the highest, lowest and random points.\n", + "* Project points from 3D to 2D\n", + "\n", + "\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": [ + "%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", + "\n", + "from airo_dataset_tools.data_parsers.camera_intrinsics import CameraIntrinsics\n", + "from airo_dataset_tools.data_parsers.pose import Pose" + ] + }, + { + "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", + "pointcloud_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 pointclouds." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "pcd_in_camera = o3d.io.read_point_cloud(pointcloud_path)\n", + "pcd = pcd_in_camera.transform(camera_pose) # transform to world frame (= base frame of left robot)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "o3d.visualization.draw_geometries([pcd])" + ] + }, + { + "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." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "points_float64 = np.asarray(pcd.points)\n", + "colors_float64 = np.asarray(pcd.colors)\n", + "\n", + "print(f\"width x height = {resolution[0]} x {resolution[1]} = {resolution[0] * resolution[1]}\")\n", + "print(\"points:\", points_float64.shape, points_float64.dtype)\n", + "print(\"colors:\", colors_float64.shape, colors_float64.dtype)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "> Note: the amount of points in the point cloud is equal to the amount of pixels in the images and depth map.\n", + "\n", + "> Note: Open3D uses float64 for pointcloud positions and colors" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_camera_toolkit.point_clouds.conversions import pointcloud_open3d_to_numpy\n", + "\n", + "points, colors = pointcloud_open3d_to_numpy(pcd)\n", + "\n", + "print(\"points:\", points.shape, points.dtype)\n", + "print(\"colors:\", colors.shape, colors.dtype)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "> Note: converting large point clouds to and from Open3D is slow, so only do this when necessary. It also introduces some rounding error." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%%timeit\n", + "points, colors = pointcloud_open3d_to_numpy(pcd)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%%timeit\n", + "pcd = pointcloud_numpy_to_open3d((points, colors))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_camera_toolkit.point_clouds.conversions import pointcloud_numpy_to_open3d\n", + "\n", + "pcd2 = pointcloud_numpy_to_open3d((points, colors))\n", + "\n", + "print(\"Points exactly equal:\", np.all(np.asarray(pcd.points) == np.asarray(pcd2.points)))\n", + "print(\"Colors exactly equal:\", np.all(np.asarray(pcd.colors) == np.asarray(pcd2.colors)))\n", + "print(\"Points close:\", np.allclose(np.asarray(pcd.points), np.asarray(pcd2.points)))\n", + "print(\"Colors close:\", np.allclose(np.asarray(pcd.colors), np.asarray(pcd2.colors)))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "o3d.visualization.draw_geometries([pcd2])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 2. Removing low confidence points" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_typing import NumpyDepthMapType\n", + "from ipywidgets import interact\n", + "\n", + "def binarize_confidence(image: NumpyDepthMapType, threshold=50.0):\n", + " # confident = np.rint(image).astype(np.uint8) <= np.rint(threshold).astype(np.uint8)\n", + " confident = image <= threshold\n", + " return confident\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)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "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 +} diff --git a/airo-camera-toolkit/docs/rerun-zed-example.ipynb b/airo-camera-toolkit/notebooks/rerun_zed_tutorial.ipynb similarity index 100% rename from airo-camera-toolkit/docs/rerun-zed-example.ipynb rename to airo-camera-toolkit/notebooks/rerun_zed_tutorial.ipynb diff --git a/airo-camera-toolkit/docs/live_charuco_pose.py b/airo-camera-toolkit/scripts/live_charuco_pose.py similarity index 100% rename from airo-camera-toolkit/docs/live_charuco_pose.py rename to airo-camera-toolkit/scripts/live_charuco_pose.py