Visual_Odometry.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "acknowledged-cornwall",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2\n",
    "import glob\n",
    "import matplotlib.image as mpimg\n",
    "from numpy import linalg\n",
    "import json"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "distant-couple",
   "metadata": {},
   "outputs": [],
   "source": [
    "class VisualOdometry():\n",
    "    \n",
    "    def read_ground_truth_file(self,path):\n",
    "        with open(path, \"r\") as f:\n",
    "            ground_truth = json.load(f)\n",
    "        return ground_truth\n",
    "    \n",
    "    def __init__(self):\n",
    "        path = './kitti_dataset/imgs/*'\n",
    "        self.images_list = glob.glob(path)\n",
    "        \n",
    "        self.ground_truth = self.read_ground_truth_file(\"ground_truth.json\")\n",
    "        \n",
    "        self.focal = 718.856\n",
    "        self.pp = (607.1928, 185.2157)\n",
    "        \n",
    "        self.R = np.eye(3)\n",
    "        self.t = np.zeros((3, 1))\n",
    "        self.detection_threshold = 2000\n",
    "        \n",
    "        #self.detector = cv2.ORB_create(5000)\n",
    "        \n",
    "        self.lk_params = {\"winSize\": (21, 21),\n",
    "                          \"maxLevel\": 5}\n",
    "        \n",
    "        self.prev_points = np.empty(0)\n",
    "        self.good_points = 0\n",
    "        self.idx = 0\n",
    "        \n",
    "        self.prev_img = self.read_img()\n",
    "        \n",
    "        \n",
    "    def run(self):\n",
    "        while self.idx < len(self.images_list)-1 :\n",
    "        #self.idx apo 0 mexri 4539:\n",
    "        \n",
    "            current = self.read_img()\n",
    "        \n",
    "            if self.good_points < self.detection_threshold:\n",
    "                corners = self.detect()\n",
    "            \n",
    "            prev_points, good_points = self.track(current,corners)\n",
    "            \n",
    "            E = self.find_essential(prev_points, good_points)\n",
    "            \n",
    "            R, t = self.find_rt(E, prev_points, good_points)\n",
    "            \n",
    "            scale = self.find_scale()\n",
    "            \n",
    "            self.t = self.t + scale * self.R.dot(t)\n",
    "            self.R = self.R.dot(R)\n",
    "            \n",
    "            self.prev_points = good_points\n",
    "            self.good_points = good_points.shape[0]\n",
    "            \n",
    "            self.prev_img = current\n",
    "            \n",
    "        \n",
    "    def read_img(self):\n",
    "        image = mpimg.imread(self.images_list[self.idx])\n",
    "        image = np.array(image * 255, dtype=np.uint8)\n",
    "        self.idx += 1\n",
    "        return image\n",
    "    \n",
    "    def detect(self):\n",
    "        corners = cv2.goodFeaturesToTrack(self.prev_img, mask=None, maxCorners = 100, qualityLevel = 0.03, minDistance = 7, blockSize = 7)\n",
    "        corners = np.squeeze(corners)\n",
    "        return corners\n",
    "    \n",
    "    def track(self,image,corners):\n",
    "        next_corners, status, _ = cv2.calcOpticalFlowPyrLK(self.prev_img, image, corners, None, **self.lk_params)\n",
    "        if next_corners is not None:\n",
    "            prev_good = corners[status.ravel() == 1]\n",
    "            good = next_corners[status.ravel() == 1]\n",
    "\n",
    "        return prev_good,good\n",
    "    \n",
    "    def find_essential(self, prev_good, good):\n",
    "        E, _ = cv2.findEssentialMat(prev_good, good,\n",
    "                                    focal=self.focal, pp=self.pp,\n",
    "                                    method = cv2.RANSAC, prob=0.999, threshold=1.0)\n",
    "        return E\n",
    "    \n",
    "    def find_rt(self, E, prev_good, good):\n",
    "        _, R, t, _ = cv2.recoverPose(E, prev_good, good, focal=self.focal, pp=self.pp)\n",
    "        return R, t\n",
    "    \n",
    "    def find_scale(self):\n",
    "        prev_position = self.ground_truth[self.idx -1]\n",
    "        prev_position = np.array(prev_position)\n",
    "        current_position = self.ground_truth[self.idx]\n",
    "        current_position = np.array(current_position)\n",
    "        self.idx += 1\n",
    "        \n",
    "        scale = np.linalg.norm(current_position - prev_position)\n",
    "        return scale\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "municipal-programming",
   "metadata": {},
   "outputs": [],
   "source": [
    "visual_odometry = VisualOdometry()\n",
    "visual_odometry.run()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "intensive-equality",
   "metadata": {},
   "outputs": [
    {
     "ename": "IndexError",
     "evalue": "tuple index out of range",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mIndexError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-3-730e596bf181>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      6\u001b[0m \u001b[0moutput\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;34m'./output/test_mapping.mp4'\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      7\u001b[0m \u001b[0mdata\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mVisualOdometry\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 8\u001b[1;33m \u001b[0mclip\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mImageSequenceClip\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mimages_list\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mfps\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;36m20\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      9\u001b[0m \u001b[0mnew_clip\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mclip\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mfl_image\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mprocess_image\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     10\u001b[0m \u001b[0mget_ipython\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mrun_line_magic\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'time'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;34m'new_clip.write_videofile(output, audio=False)'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mD:\\ancda\\envs\\cv\\lib\\site-packages\\moviepy\\video\\io\\ImageSequenceClip.py\u001b[0m in \u001b[0;36m__init__\u001b[1;34m(self, sequence, fps, durations, with_mask, ismask, load_images)\u001b[0m\n\u001b[0;32m    121\u001b[0m                 \u001b[1;32mreturn\u001b[0m \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mlastimage\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    122\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 123\u001b[1;33m             \u001b[1;32mif\u001b[0m \u001b[0mwith_mask\u001b[0m \u001b[1;32mand\u001b[0m \u001b[1;33m(\u001b[0m\u001b[0mimread\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msequence\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m==\u001b[0m\u001b[1;36m4\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    124\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    125\u001b[0m                 \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mmask\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mVideoClip\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mismask\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;32mTrue\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mIndexError\u001b[0m: tuple index out of range"
     ]
    }
   ],
   "source": [
    "from moviepy.editor import VideoFileClip\n",
    "from moviepy.editor import ImageSequenceClip\n",
    "\n",
    "\n",
    "# Ορίστε ένα όνομα και ένα μονοπάτι αρχείου\n",
    "output = './output/test_mapping.mp4'\n",
    "data = VisualOdometry()\n",
    "clip = ImageSequenceClip(data.images_list, fps=20)\n",
    "new_clip = clip.fl_image(process_image) \n",
    "%time new_clip.write_videofile(output, audio=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "loaded-treasurer",
   "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.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}