Add notebook example

notebook/pyviso2.ipynb

@@ -0,0 +1,280 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from skimage.io import imread\n",
+    "import numpy as np\n",
+    "import viso2\n",
+    "from pathlib import Path\n",
+    "from mayavi import mlab"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# set the most relevant parameters\n",
+    "params = viso2.Stereo_parameters()\n",
+    "params.calib.f  = 721.5377\n",
+    "params.calib.cu = 609.5593\n",
+    "params.calib.cv = 172.854\n",
+    "params.base     = 0.537\n",
+    "\n",
+    "# initialize visual odometry \n",
+    "viso = viso2.VisualOdometryStereo(params)\n",
+    "#recon = viso2.Reconstruction()\n",
+    "#recon.setCalibration(params.calib.f, params.calib.cu, params.calib.cv)\n",
+    "matcher_params = viso2.Matcher_parameters()\n",
+    "\n",
+    "matcher_params.f = 721.5377\n",
+    "matcher_params.cu = 609.5593\n",
+    "matcher_params.cv = 172.854\n",
+    "matcher_params.base = 0.537\n",
+    "matcher_params.nms_n = 13\n",
+    "matcher_params.nms_tau = 50\n",
+    "matcher_params.match_binsize = 50 \n",
+    "matcher_params.match_radius = 200\n",
+    "matcher_params.match_disp_tolerance = 2\n",
+    "matcher_params.outlier_flow_tolerance = 10\n",
+    "matcher_params.outlier_disp_tolerance = 10\n",
+    "matcher_params.multi_stage = 1\n",
+    "matcher_params.half_resolution = 0\n",
+    "matcher_params.refinement = 1\n",
+    "\n",
+    "matcher = viso2.Matcher(matcher_params)\n",
+    "matcher.setIntrinsics(params.calib.f, params.calib.cu, params.calib.cv, params.base)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 17,
+   "metadata": {
+    "scrolled": false
+   },
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Matches: 356 Inliers: 89.04494382022472 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000724   -0.0000091    0.0001944 \n",
+      "   0.0000724    1.0000000    0.0002859   -0.0016383 \n",
+      "   0.0000091   -0.0002859    1.0000000    0.0005430 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 92.13483146067416 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000895   -0.0000114    0.0002298 \n",
+      "   0.0000895    1.0000000    0.0000545   -0.0001418 \n",
+      "   0.0000114   -0.0000545    1.0000000    0.0004024 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.85393258426966 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001164   -0.0000154    0.0002958 \n",
+      "   0.0001164    1.0000000    0.0001767   -0.0009776 \n",
+      "   0.0000154   -0.0001767    1.0000000    0.0006759 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 92.69662921348315 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001858   -0.0000186    0.0004430 \n",
+      "   0.0001858    1.0000000    0.0001795   -0.0008655 \n",
+      "   0.0000186   -0.0001795    1.0000000   -0.0000121 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 89.88764044943821 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0002762   -0.0000308    0.0006580 \n",
+      "   0.0002762    0.9999999    0.0002824   -0.0019426 \n",
+      "   0.0000307   -0.0002824    1.0000000    0.0004384 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.29213483146067 %\n",
+      "Estimated motion:\n",
+      "   1.0000000    0.0000454    0.0000044   -0.0001180 \n",
+      "  -0.0000454    1.0000000    0.0000073    0.0008894 \n",
+      "  -0.0000044   -0.0000073    1.0000000    0.0011392 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 92.97752808988764 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0002753   -0.0000296    0.0006527 \n",
+      "   0.0002753    1.0000000    0.0001301   -0.0005682 \n",
+      "   0.0000296   -0.0001301    1.0000000    0.0005342 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 89.88764044943821 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000872   -0.0000108    0.0002079 \n",
+      "   0.0000872    1.0000000    0.0000711    0.0005846 \n",
+      "   0.0000108   -0.0000711    1.0000000    0.0013462 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 90.1685393258427 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000968   -0.0000107    0.0002296 \n",
+      "   0.0000968    1.0000000    0.0001462   -0.0011203 \n",
+      "   0.0000107   -0.0001463    1.0000000    0.0010456 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.85393258426966 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001930   -0.0000206    0.0004743 \n",
+      "   0.0001930    0.9999999    0.0003160   -0.0021588 \n",
+      "   0.0000205   -0.0003160    0.9999999   -0.0003496 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 90.73033707865169 %\n",
+      "Estimated motion:\n",
+      "   1.0000000    0.0002380    0.0000193   -0.0004839 \n",
+      "  -0.0002380    1.0000000    0.0001851   -0.0020887 \n",
+      "  -0.0000192   -0.0001851    1.0000000   -0.0017022 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.01123595505618 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000071   -0.0000012    0.0000093 \n",
+      "   0.0000071    1.0000000    0.0000663    0.0005272 \n",
+      "   0.0000012   -0.0000663    1.0000000    0.0012105 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 90.4494382022472 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0000762   -0.0000082    0.0001748 \n",
+      "   0.0000762    1.0000000    0.0001564   -0.0002688 \n",
+      "   0.0000082   -0.0001564    1.0000000    0.0011194 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.57303370786516 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001954   -0.0000184    0.0004344 \n",
+      "   0.0001954    0.9999999    0.0002576   -0.0015971 \n",
+      "   0.0000184   -0.0002576    1.0000000    0.0000587 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 91.01123595505618 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001684   -0.0000195    0.0004314 \n",
+      "   0.0001684    0.9999999    0.0003710   -0.0030661 \n",
+      "   0.0000195   -0.0003710    0.9999999    0.0000909 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 90.73033707865169 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001414   -0.0000141    0.0003366 \n",
+      "   0.0001414    1.0000000    0.0002105   -0.0011175 \n",
+      "   0.0000141   -0.0002105    1.0000000    0.0006190 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 89.32584269662921 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0002093   -0.0000215    0.0004778 \n",
+      "   0.0002093    1.0000000    0.0000278    0.0008144 \n",
+      "   0.0000215   -0.0000278    1.0000000    0.0014697 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 88.76404494382022 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0002540   -0.0000271    0.0005986 \n",
+      "   0.0002540    1.0000000    0.0001620   -0.0022803 \n",
+      "   0.0000271   -0.0001620    1.0000000   -0.0016476 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n",
+      "Matches: 356 Inliers: 90.1685393258427 %\n",
+      "Estimated motion:\n",
+      "   1.0000000   -0.0001491   -0.0000168    0.0003601 \n",
+      "   0.0001491    1.0000000    0.0001314   -0.0002892 \n",
+      "   0.0000167   -0.0001314    1.0000000    0.0005784 \n",
+      "   0.0000000    0.0000000    0.0000000    1.0000000 \n"
+     ]
+    }
+   ],
+   "source": [
+    "left_images = [f for f in Path(\"left\").rglob(\"*.png\")]\n",
+    "right_images = [f for f in Path(\"right\").rglob(\"*.png\")]\n",
+    "\n",
+    "left_img = imread(left_images[0])\n",
+    "right_img = imread(right_images[0])\n",
+    "\n",
+    "pose = viso2.Matrix_eye(4)\n",
+    "\n",
+    "for i in range(1, 20):\n",
+    "    left_img = imread(left_images[i])\n",
+    "    right_img = imread(right_images[i])\n",
+    "\n",
+    "    matcher.pushBack(left_img, right_img)\n",
+    "\n",
+    "    left_img = imread(\"left/0000000081.png\")\n",
+    "    right_img = imread(\"right/0000000081.png\")\n",
+    "\n",
+    "    matcher.pushBack(left_img, right_img)\n",
+    "\n",
+    "    matcher.matchFeatures(2)\n",
+    "    matches = matcher.getMatches()\n",
+    "    matches_mat = np.empty([8, matches.size()])\n",
+    "    # print(matches.size())\n",
+    "    \n",
+    "    if viso.process_frame(left_img, right_img):\n",
+    "        motion = viso.getMotion()\n",
+    "        est_motion = viso2.Matrix_inv(motion)\n",
+    "        pose = pose * est_motion\n",
+    "\n",
+    "        num_matches = viso.getNumberOfMatches()\n",
+    "        num_inliers = viso.getNumberOfInliers()\n",
+    "        print('Matches:', num_matches, \"Inliers:\", 100*num_inliers/num_matches, '%')\n",
+    "        print('Estimated motion:')\n",
+    "        print(est_motion)\n",
+    "\n",
+    "        #matches = viso.getMatches()\n",
+    "        #recon.update(matches, motion, 0)\n",
+    "        \n",
+    "    else:\n",
+    "        print('.... failed!')\n",
+    "        \n",
+    "#points = recon.getPoints()\n",
+    "#print(\"Reconstructed\", points.size(), \"points...\")\n",
+    "\n",
+    "#if points.size() > 0:\n",
+    "#    pts = np.empty((points.size(),3))\n",
+    "#    for i,p in enumerate(points):\n",
+    "#        pts[i,:] = (p.x, p.y, p.z)\n",
+    "\n",
+    "#    mlab.figure()\n",
+    "#    mlab.points3d(pts[:,0], pts[:,1], pts[:,2], colormap='copper')\n",
+    "#    mlab.show()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "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.5"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}