examples/notebooks/kitti_odometry.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# VDBFusion example\n",
    "\n",
    "This notebook is rather a small example to get you started with the use of VDBFusion.\n",
    "\n",
    "If you use our work please consider citing us:\n",
    "\n",
    "```bibtex\n",
    "@article{vizzo2022sensors,\n",
    "  author         = {Vizzo, Ignacio and Guadagnino, Tiziano and Behley, Jens and Stachniss, Cyrill},\n",
    "  title          = {VDBFusion: Flexible and Efficient TSDF Integration of Range Sensor Data},\n",
    "  journal        = {Sensors},\n",
    "  volume         = {22},\n",
    "  year           = {2022},\n",
    "  number         = {3},\n",
    "  article-number = {1296},\n",
    "  url            = {https://www.mdpi.com/1424-8220/22/3/1296},\n",
    "  issn           = {1424-8220},\n",
    "  doi            = {10.3390/s22031296}\n",
    "}\n",
    "```\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 1. Install vdbfusion\n",
    "\n",
    "The first we need to check is that if we can install the `vdbfusion` library. If you ran into any issue at this step you should check our [github repository](https://github.com/PRBonn/vdbfusion) and submit an issue there."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%pip install vdbfusion\n",
    "import vdbfusion\n",
    "print(f\"VDBFusion version {vdbfusion.__version__} properly installed at {vdbfusion.__file__}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 2. Install example dependencies\n",
    "\n",
    "While `vdbfusion` only depends on `numpy` the tools we will be using in this tutorial does not, therefore we will install some common dependencies and make sure we can import them"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%pip install trimesh pandas tqdm | grep -v 'already satisfied'"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tqdm import tqdm\n",
    "import pandas\n",
    "import trimesh"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 3. Get some toy data\n",
    "\n",
    "In order to get you started with the use of the library we will use a subset of the sequence `00` from the [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php). We selected the first 100 scans just to avoid downloading big datasets. If you plan to use the library for real work, you should rather consider the complete examples at the [examples repository](https://github.com/PRBonn/vdbfusion/examples)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Downloading a small subset of the kitti odometry dataset\")\n",
    "!wget -c https://www.ipb.uni-bonn.de/html/software/vdbfusion/kitti-odometry.tar.gz -O - | tar -xz"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 4. Create Dataloader"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "WT2xubn3215N"
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import glob\n",
    "\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "\n",
    "from trimesh import transform_points\n",
    "\n",
    "\n",
    "class Config:\n",
    "    # Data specific params\n",
    "    apply_pose = True\n",
    "    min_range = 2.0\n",
    "    max_range = 70.0\n",
    "\n",
    "\n",
    "class KITTIOdometryDataset:\n",
    "    def __init__(self, kitti_root_dir: str, sequence: int):\n",
    "        \"\"\"Simple KITTI DataLoader to provide a ready-to-run example.\n",
    "\n",
    "        Heavily inspired in PyLidar SLAM\n",
    "        \"\"\"\n",
    "        # Config stuff\n",
    "        self.sequence = str(int(sequence)).zfill(2)\n",
    "        self.config = Config()\n",
    "        self.kitti_sequence_dir = os.path.join(kitti_root_dir, \"sequences\", self.sequence)\n",
    "        self.velodyne_dir = os.path.join(self.kitti_sequence_dir, \"velodyne/\")\n",
    "\n",
    "        # Read stuff\n",
    "        self.calibration = self.read_calib_file(os.path.join(self.kitti_sequence_dir, \"calib.txt\"))\n",
    "        self.poses = self.load_poses(os.path.join(kitti_root_dir, f\"poses/{self.sequence}.txt\"))\n",
    "        self.scan_files = sorted(glob.glob(self.velodyne_dir + \"*.bin\"))\n",
    "\n",
    "    def __getitem__(self, idx):\n",
    "        return self.scans(idx), self.poses[idx]\n",
    "\n",
    "    def __len__(self):\n",
    "        return len(self.scan_files)\n",
    "\n",
    "    def scans(self, idx):\n",
    "        return self.read_point_cloud(idx, self.scan_files[idx], self.config)\n",
    "\n",
    "    def read_point_cloud(self, idx: int, scan_file: str, config: Config):\n",
    "        points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 4))[:, :-1]\n",
    "        points = points[np.linalg.norm(points, axis=1) <= config.max_range]\n",
    "        points = points[np.linalg.norm(points, axis=1) >= config.min_range]\n",
    "        points = transform_points(points, self.poses[idx]) if config.apply_pose else points\n",
    "        return points\n",
    "\n",
    "    def load_poses(self, poses_file):\n",
    "        def _lidar_pose_gt(poses_gt):\n",
    "            _tr = self.calibration[\"Tr\"].reshape(3, 4)\n",
    "            tr = np.eye(4, dtype=np.float64)\n",
    "            tr[:3, :4] = _tr\n",
    "            left = np.einsum(\"...ij,...jk->...ik\", np.linalg.inv(tr), poses_gt)\n",
    "            right = np.einsum(\"...ij,...jk->...ik\", left, tr)\n",
    "            return right\n",
    "\n",
    "        poses = pd.read_csv(poses_file, sep=\" \", header=None).values\n",
    "        n = poses.shape[0]\n",
    "        poses = np.concatenate(\n",
    "            (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), axis=1\n",
    "        )\n",
    "        poses = poses.reshape((n, 4, 4))  # [N, 4, 4]\n",
    "        return _lidar_pose_gt(poses)\n",
    "\n",
    "    @staticmethod\n",
    "    def read_calib_file(file_path: str) -> dict:\n",
    "        calib_dict = {}\n",
    "        with open(file_path, \"r\") as calib_file:\n",
    "            for line in calib_file.readlines():\n",
    "                tokens = line.split(\" \")\n",
    "                if tokens[0] == \"calib_time:\":\n",
    "                    continue\n",
    "                # Only read with float data\n",
    "                if len(tokens) > 0:\n",
    "                    values = [float(token) for token in tokens[1:]]\n",
    "                    values = np.array(values, dtype=np.float32)\n",
    "                    # The format in KITTI's file is <key>: <f1> <f2> <f3> ...\\n -> Remove the ':'\n",
    "                    key = tokens[0][:-1]\n",
    "                    calib_dict[key] = values\n",
    "        return calib_dict"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "d4E7ccz72WzJ"
   },
   "source": [
    "## Step 5. Run the fusion pipeline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {
     "base_uri": "https://localhost:8080/"
    },
    "id": "qR5XDLQ82WzJ",
    "outputId": "84fe25e9-6ca2-4a35-a225-331941ac3869"
   },
   "outputs": [],
   "source": [
    "from vdbfusion import VDBVolume\n",
    "\n",
    "# Create a VDB Volume to integrate scans\n",
    "vdb_volume = VDBVolume(voxel_size=0.1, sdf_trunc=0.3, space_carving=False)\n",
    "\n",
    "# You need to define your own Dataset.\n",
    "dataset = KITTIOdometryDataset(kitti_root_dir=\"./kitti-odometry/dataset/\", sequence=0)\n",
    "\n",
    "for scan, pose in tqdm(dataset):\n",
    "    vdb_volume.integrate(scan, pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 6. Visualize the results\n",
    "\n",
    "In order to store the results on this notebook, we use the trimesh library. But if you are running this locally, you can use `Open3D` or any other 3D library you like."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Extract a mesh from vdbfusion\n",
    "vertices, triangles = vdb_volume.extract_triangle_mesh(fill_holes=True, min_weight=5.0)\n",
    "\n",
    "mesh = trimesh.Trimesh(vertices=vertices, faces=triangles)\n",
    "mesh.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Where to go next?\n",
    "\n",
    "If you think the library could be useful for your work, now is time to check our open source code at our [github](https://github.com/PRBonn/vdbfusion)"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}