docs/kitti.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "a60b1fbd-ac67-4f1e-a91f-6bcf6bfd6671",
   "metadata": {},
   "source": [
    "# Dataset Visual Odometry / SLAM Evaluation\n",
    "\n",
    "1. [Download odometry data set (grayscale, 22 GB)](https://s3.eu-central-1.amazonaws.com/avg-kitti/data_odometry_gray.zip)\n",
    "2. [Download odometry data set (color, 65 GB)](https://s3.eu-central-1.amazonaws.com/avg-kitti/data_odometry_color.zip)\n",
    "3. [Download odometry data set (velodyne laser data, 80 GB)](https://s3.eu-central-1.amazonaws.com/avg-kitti/data_odometry_velodyne.zip)\n",
    "4. [Download odometry data set (calibration files, 1 MB)](https://s3.eu-central-1.amazonaws.com/avg-kitti/data_odometry_calib.zip)\n",
    "5. [Download odometry ground truth poses (4 MB)](https://s3.eu-central-1.amazonaws.com/avg-kitti/data_odometry_poses.zip)\n",
    "\n",
    "\n",
    "\n",
    "## Sensor setup \n",
    "<img src=\"images/setup_top_view.png\" />\n",
    "\n",
    "<img src=\"images/passat_sensors_920.png\" />\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9513ca23-c655-4ded-868f-705a7fcabe4e",
   "metadata": {},
   "source": [
    "## Calibration Files and Projection Matrices\n",
    "\n",
    "to get the calibration data run:\n",
    "```\n",
    "python kitti_calibration.py\n",
    "```\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "81e6b120-cc40-4087-851e-c2d0d5800f61",
   "metadata": {},
   "source": [
    "\n",
    "- $P0$: Reference camera (left of stereo pair 1), extrinsics are identity.\n",
    "- $P1$: Right camera of stereo pair 1, extrinsics include baseline offset.\n",
    "- $P2$: Left camera of stereo pair 2, extrinsics depend on setup.\n",
    "- $P3$: Right camera of stereo pair 2, extrinsics depend on setup.\n",
    "\n",
    "\n",
    "---\n",
    "\n",
    "Camera: $P0$:\n",
    "\n",
    "```\n",
    "Projection Matrix:\n",
    "[[707.0912   0.     601.8873   0.    ]\n",
    " [  0.     707.0912 183.1104   0.    ]\n",
    " [  0.       0.       1.       0.    ]]\n",
    "Intrinsic Matrix:\n",
    "[[707.0912   0.     601.8873]\n",
    " [  0.     707.0912 183.1104]\n",
    " [  0.       0.       1.    ]]\n",
    "Rotation Matrix:\n",
    "[[1. 0. 0.]\n",
    " [0. 1. 0.]\n",
    " [0. 0. 1.]]\n",
    "Translation Vector:\n",
    "[[0.]\n",
    " [0.]\n",
    " [0.]]\n",
    "```\n",
    "---\n",
    "\n",
    "Camera: $P1$:\n",
    "```\n",
    "Projection Matrix:\n",
    "[[ 707.0912    0.      601.8873 -379.8145]\n",
    " [   0.      707.0912  183.1104    0.    ]\n",
    " [   0.        0.        1.        0.    ]]\n",
    "Intrinsic Matrix:\n",
    "[[707.0912   0.     601.8873]\n",
    " [  0.     707.0912 183.1104]\n",
    " [  0.       0.       1.    ]]\n",
    "Rotation Matrix:\n",
    "[[1. 0. 0.]\n",
    " [0. 1. 0.]\n",
    " [0. 0. 1.]]\n",
    "Translation Vector:\n",
    "[[ 5.37150653e-01]\n",
    " [-1.34802944e-17]\n",
    " [ 0.00000000e+00]]\n",
    "```\n",
    "\n",
    "From the above image the distance between two camera is `0.54` on $x$ axis and from decomposition we have: `5.37150653e-01`.\n",
    "\n",
    "Refs: [1](https://www.cvlibs.net/datasets/kitti/setup.php)\n",
    "[2](https://stackoverflow.com/questions/29407474/how-to-understand-the-kitti-camera-calibration-files), [3](https://github.com/yanii/kitti-pcl/blob/master/KITTI_README.TXT), [4](https://www.cvlibs.net/datasets/kitti/eval_odometry.php), [5](https://github.com/avisingh599/mono-vo/), [6](https://github.com/alishobeiri/Monocular-Video-Odometery), [7](https://avisingh599.github.io/vision/monocular-vo/)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "71705b1a-2cd5-4af1-afe5-02d8df2ae9e0",
   "metadata": {},
   "source": [
    "\n",
    "## Ground Truth Poses\n",
    "each row of the data has 12 columns, 12 come from flattening a `3x4` transformation matrix of the left:\n",
    "\n",
    "```\n",
    "r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz\n",
    "```\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2db51f82-cb11-4909-bef4-9e87742df5e2",
   "metadata": {},
   "source": [
    "## Display Ground Truth Poses in rerun \n",
    "just run: \n",
    "\n",
    "```\n",
    "python kitti_gt_to_rerun.py\n",
    "```\n",
    "\n",
    "\n",
    "<img src=\"images/display_ground_truth_poses_rerun.png\" />\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "499cfedf-9c39-42de-8d06-74a036f59667",
   "metadata": {},
   "source": [
    "## Visual Odometry\n",
    "\n",
    "If you use `SIFT` run: \n",
    "\n",
    "```\n",
    "python kitti_vo_sift.py\n",
    "```\n",
    "\n",
    "<img src=\"images/kitti_vo_sift.png\" />\n",
    "\n",
    "or if you use `cv2.goodFeaturesToTrack` you will get poor results:\n",
    "\n",
    "\n",
    "```\n",
    "python kitti_vo.py\n",
    "```\n",
    "\n",
    "<img src=\"images/kitti_vo.png\" />\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6d151a70-e67b-45df-9624-e74c9e3aaa8b",
   "metadata": {},
   "source": [
    "## Stereo Vision\n",
    "just run:\n",
    "```\n",
    "python kitti_stereo.py\n",
    "```\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7655e3c3-5ce8-42b8-a7c1-610f4a5665cf",
   "metadata": {},
   "source": [
    "## Reconstruct Sparse/Dense Model From Known Camera Poses with Colmap\n",
    "\n",
    "Your data should have the following structure: \n",
    "\n",
    "```\n",
    "├── database.db\n",
    "├── dense\n",
    "│   ├── refined\n",
    "│   │   └── model\n",
    "│   │       └── 0\n",
    "│   └── sparse\n",
    "│       └── model\n",
    "│           └── 0\n",
    "├── images\n",
    "│   ├── 00000.png\n",
    "│   ├── 00001.png\n",
    "│   ├── 00002.png\n",
    "│   └── 00003.png\n",
    "└── sparse\n",
    "    └── model\n",
    "        └── 0\n",
    "            ├── cameras.txt\n",
    "            ├── images.txt\n",
    "            └── points3D.txt\n",
    "```\n",
    "\n",
    "1. `cameras.txt`: the format is:\n",
    "\n",
    "```\n",
    "CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n",
    "```\n",
    "so for KITTI dataset the camera model is `PINHOLE`, and it has four parameters which are the focal lengths (`fx`, `fy`) and principal point coordinates (`cx`, `cy`).\n",
    "\n",
    "- `CAMERA_ID`: 1\n",
    "- `MODEL`: PINHOLE\n",
    "- `WIDTH`: 1226\n",
    "- `HEIGHT`: 370\n",
    "- `fx`: 707.0912\n",
    "- `fy`: 707.0912\n",
    "- `cx`: 601.8873\n",
    "- `cy`: 183.1104\n",
    "\n",
    "should be like this:\n",
    "\n",
    "```\n",
    "1 PINHOLE 1226 370 707.0912 707.0912 601.8873 183.1104\n",
    "```\n",
    "\n",
    "2. `images.txt`: the format is\n",
    "```\n",
    "IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n",
    "```\n",
    "\n",
    "so you data should be like this, mind the extra line after each line:\n",
    "\n",
    "```\n",
    "1 1.0 0.0 0.0 0.0 0.031831570484910754 -0.2020180259287443 -0.05988511865826446 1 000000.png\n",
    "\n",
    "2 0.9999990698095921 -0.000486454947446343 0.0008155417501438222 -0.0009790981505847082 -0.026717887515950233 -0.09385561937368328 -0.38812196090339146 1 000001.png\n",
    "\n",
    "3 0.9999976159395401 -0.0011567120445530273 0.0013793515824379724 -0.0012359294859380324 -0.23100950491953082 -0.05900910756124116 -0.9698261247623092 1 000002.png\n",
    "\n",
    "4 0.9999950283825452 -0.0017604272641239351 0.0022926784138869423 -0.0012600522730534293 0.17578254454768152 -0.014474209460539546 -1.9112790713853196 1 000003.png\n",
    "```\n",
    "and finally:\n",
    "\n",
    "3. `points3D.txt`: This file should be empty."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1b8e1f63-d8b1-454a-981d-fa7a9b085cb8",
   "metadata": {},
   "source": [
    "You can run the following command to convert some colmap dataset into TXT to compare with your dataset:\n",
    "\n",
    "```\n",
    "colmap model_converter --input_path $DATASET_PATH/sparse/0 --output_path $DATASET_PATH/ --output_type TXT\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd90c5bc-e993-401f-8afa-db235f99593c",
   "metadata": {},
   "source": [
    "KITTI format for ground truth poses (for instance, for the file `data/kitti/odometry/05/poses/05.txt`) is:\n",
    "\n",
    "```\n",
    "r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz\n",
    "```\n",
    "The colmap format for `images.txt` is: \n",
    "\n",
    "```\n",
    "IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n",
    "```\n",
    "\n",
    "Run the script [kitti_to_colmap.py](../scripts/kitti/kitti_to_colmap.py). It dumps the output into `images.txt` file. \n",
    "\n",
    "\n",
    "You can run the following script to add noise: [kitti_to_colmap_noise.py](../scripts/kitti/kitti_to_colmap_noise.py).\n",
    "\n",
    "\n",
    "The inside of `~/colmap_projects/kitti_noisy` create a soft link pointing to KITTI images:\n",
    "ln -s <path-to-kitti-odometry-image> images\n",
    "\n",
    "in my case:\n",
    "\n",
    "```\n",
    " ln -s /home/$USER/workspace/OpenCVProjects/data/kitti/odometry/05/image_0/ images\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a9013dd7-dc63-4c5f-81dd-44affb14862c",
   "metadata": {},
   "source": [
    "### Setting up parameters\n",
    "\n",
    "Then set the camera param:\n",
    "\n",
    "```\n",
    "CAM=707.0912,707.0912,601.8873,183.1104\n",
    "```\n",
    "\n",
    "set the project:\n",
    "```\n",
    "project_name=kitti_noisy\n",
    "DATASET_PATH=/home/$USER/colmap_projects/$project_name\n",
    "```\n",
    "\n",
    "### Feature extraction\n",
    "\n",
    "extract the features:\n",
    "```\n",
    "colmap feature_extractor  \\\n",
    "--database_path $DATASET_PATH/database.db  \\\n",
    "--image_path $DATASET_PATH/images  \\\n",
    "--ImageReader.single_camera=true --ImageReader.camera_model=PINHOLE --ImageReader.camera_params=$CAM \\\n",
    "--SiftExtraction.use_gpu 1 \\\n",
    "--SiftExtraction.estimate_affine_shape=true \\\n",
    "--SiftExtraction.domain_size_pooling=true\n",
    "```\n",
    "\n",
    "or \n",
    "\n",
    "```\n",
    "colmap feature_extractor  \\\n",
    "--database_path $DATASET_PATH/database.db  \\\n",
    "--image_path $DATASET_PATH/images  \\\n",
    "--ImageReader.single_camera=true --ImageReader.camera_model=PINHOLE --ImageReader.camera_params=$CAM\n",
    "```\n",
    "\n",
    "### Matcher\n",
    "run the matcher:\n",
    "\n",
    "```\n",
    "colmap sequential_matcher \\\n",
    "   --database_path $DATASET_PATH/database.db \\\n",
    "   --SequentialMatching.overlap=3 \\\n",
    "   --SequentialMatching.loop_detection=true \\\n",
    "   --SequentialMatching.loop_detection_period=2 \\\n",
    "   --SequentialMatching.loop_detection_num_images=50 \\\n",
    "   --SequentialMatching.vocab_tree_path=\"$DATASET_PATH/../vocab_tree/vocab_tree_flickr100K_words256K.bin\" \\\n",
    "   --SiftMatching.use_gpu 1 --SiftMatching.gpu_index=-1  --SiftMatching.guided_matching=true \n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "baa53eaa-736b-439c-a821-272792f1fba9",
   "metadata": {},
   "source": [
    "So now if you run colmap, create new project, set the path for images and select the `database.db` and from **File> Import Model** and point to `kitti_noisy/sparse/model/0/` you will get the followings:\n",
    "\n",
    "<img src=\"images/kitti_sparse_noisy_colmap.png\" />\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0c1bd6b6-7449-491e-9e0e-e0e2e1670250",
   "metadata": {},
   "source": [
    "\n",
    "### Triangulation\n",
    "then run the \n",
    "\n",
    "```\n",
    "colmap point_triangulator \\\n",
    "    --database_path $DATASET_PATH/database.db \\\n",
    "    --image_path $DATASET_PATH/images\\\n",
    "    --input_path $DATASET_PATH/sparse/model/0 \\\n",
    "    --output_path $DATASET_PATH/dense/sparse/model/0\n",
    "```\n",
    "\n",
    "Now run bundle adjuster to only optimize the extrinsic (camera position and orientations) and **NOT** intrinsic (camera parameter)\n",
    "\n",
    "\n",
    "```\n",
    "colmap bundle_adjuster  \\\n",
    "  --input_path $DATASET_PATH/dense/sparse/model/0 \\\n",
    "  --output_path $DATASET_PATH/dense/refined/model/0 \\\n",
    "  --BundleAdjustment.refine_focal_length  0 \\\n",
    "  --BundleAdjustment.refine_principal_point   0 \\\n",
    "  --BundleAdjustment.refine_extra_params  0 \\\n",
    "  --BundleAdjustment.refine_extrinsics  1\n",
    "```\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "386c1c5a-42fc-47f3-9a9a-43098c74c134",
   "metadata": {},
   "source": [
    "Ok now if you run colmap, create new project, set the path for images and select the `database.db` and from **File> Import Model** and point to `kitti_noisy/dense/refined/model/0/` you will get the followings:\n",
    "\n",
    "<img src=\"images/kitti_sparse_refined_colmap.png\" />\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2c3d5a47-aa10-43c8-887e-52dffdb55791",
   "metadata": {},
   "source": [
    "Refs [1](https://colmap.github.io/faq.html#reconstruct-sparse-dense-model-from-known-camera-poses)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}