stereoReconAndPnP.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Project 2\n",
    "\n",
    "## Topic : Stereo reconstruction and Non-linear optimization\n",
    "\n",
    "#### Instructions\n",
    "<ul>\n",
    "    <li> The second project of the course is designed to get you familiar with stereo reconstruction, and non-linear optimization </li>\n",
    "    <li> Use python for this project. PILLOW and OpenCV are permitted for image I/O. </li>\n",
    "    <li> Submit this notebook as a zipped file on moodle. The format should be $<$team_id$>$_$<$team_ name$>$.zip. Both members have to submit this zip file. </li>\n",
    "    <li> A seperate report is not needed if you're coding in the notebook itself. Please provide adequate descriptions of the approaches you've taken. Also mention work distribution for the two members. </li>\n",
    "    <li> Refer to the late day policy. Start early </li> \n",
    "    <li> Download data from here: https://iiitaphyd-my.sharepoint.com/:f:/g/personal/aryan_sakaria_students_iiit_ac_in/Er5C7351IAlFsvwHUesFeSQBQtlSiAS7AORSEJT2qH_8_w?e=ol98k9  </li>\n",
    "</ul>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "----\n",
    "### Stereo dense reconstruction\n",
    "\n",
    "3-D point clouds are very useful in robotics for several tasks such as object detection, motion estimation (3D-3D matching or 3D-2D matching), SLAM, and other forms of scene understanding.  Stereo cameras provide  us  with  a  convenient  way  to  generate  dense  point  clouds.Dense here, in contrast to sparse, means that all the image points are used for the reconstruction.  In this part we generating a dense 3D point cloud reconstruction of a scene from stereo images.\n",
    "\n",
    "#### Procedure: \n",
    "\n",
    "<ol>\n",
    "    <li> Generate a disparity map for each stereo pair.  Use OpenCV (e.g.  StereoSGBM) for this.  Notethat the images provided are already rectified and undistorted. </li>\n",
    "    <li> Then, using the camera parameters and baseline information generate colored point clouds fromeach disparity map.  Some points will have invalid disparity values, so ignore them.  Use [Open3D]for storing your point clouds. </li>\n",
    "    <li> Register (or transform) all the generated point clouds into your world frame by using the providedground truth poses. </li>\n",
    "    <li> Visualize the registered point cloud data, in color.  Use Open3D for this </li>\n",
    "</ol>\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#import libraries:\n",
    "import numpy as np \n",
    "# from sklearn.preprocessing import normalize #normalizing gives better results. Experiment with this\n",
    "import cv2\n",
    "import open3d as o3d\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_transformations(filename):\n",
    "    f = open(filename, 'r')\n",
    "    data = np.loadtxt(filename)\n",
    "    transformation_list = data.reshape(len(data),3,4);\n",
    "    return transformation_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd = o3d.geometry.PointCloud()\n",
    "pcd_final = o3d.geometry.PointCloud()\n",
    "\n",
    "calib_mat = np.array([[7.070912e+02, 0.000000e+00, 6.018873e+02],\n",
    "             [0.000000e+00, 7.070912e+02, 1.831104e+02],\n",
    "             [0.000000e+00, 0.000000e+00, 1.000000e+00]])\n",
    "\n",
    "f = calib_mat[0,0]\n",
    "T = read_transformations('data/poses.txt')\n",
    "nums = 21\n",
    "b = 0.53790448812\n",
    "\n",
    "def convert_img_world(disp_map,Q_mat):\n",
    "    points = []\n",
    "    for i in range(len(disp_map)):\n",
    "        points.append(np.matmul(Q_mat,disp_map[i]))\n",
    "    return np.array(points)\n",
    "\n",
    "\n",
    "def get_parallax(disp):\n",
    "    disp_map = []\n",
    "    for i in range(disp.shape[0]):\n",
    "        disp_2 = disp[i]\n",
    "        for j in range(disp.shape[1]):\n",
    "            disp_map.append([j,i,disp_2[j],1])\n",
    "    return np.array(disp_map)\n",
    "\n",
    "\n",
    "def get_disp(img_l,img_r):\n",
    "    win_size = 5\n",
    "    stereo = cv2.StereoSGBM_create(minDisparity = -39,\n",
    "        numDisparities = 144,\n",
    "        disp12MaxDiff = 1,\n",
    "        blockSize=5,\n",
    "        speckleRange = 32,\n",
    "        uniquenessRatio = 10,\n",
    "        speckleWindowSize = 100,\n",
    "        P1= 8*3* win_size**2,\n",
    "        P2= 32*3*win_size**2,\n",
    "        preFilterCap= 63\n",
    "        )\n",
    "    num_disp = 144\n",
    "    min_disp = -39\n",
    "    \n",
    "    disp = stereo.compute(img_l, img_r).astype(np.float32)/64.0\n",
    "    disp = disp-min_disp\n",
    "    disp = disp/num_disp\n",
    "    return disp\n",
    "\n",
    "def getQ_mat(disp,f,w):\n",
    "    Q_mat = np.array([[ 1,  0,  0, -disp.shape[1]/2],\n",
    "                  [ 0,  -1,  0, disp.shape[0]/2],\n",
    "                  [ 0,  0,  0, f],\n",
    "                  [ 0,  0,  1/b, 0]])\n",
    "    return Q_mat\n",
    "    \n",
    "for i in range(nums):    \n",
    "\n",
    "    img_l = cv2.imread(\"data/img2/00000004\"+str(60+i)+\".png\")\n",
    "    img_r = cv2.imread(\"data/img3/00000004\"+str(60+i)+\".png\")\n",
    "    \n",
    "    disp = get_disp(img_l,img_r)\n",
    "    \n",
    "    Q_mat = getQ_mat(disp,f,disp[1])\n",
    "    disp_map = get_parallax(disp)\n",
    "    \n",
    "    points_arr = convert_img_world(disp_map,Q_mat)\n",
    "    \n",
    "    colors = cv2.cvtColor(img_l, cv2.COLOR_BGR2RGB)\n",
    "    colors_ar = np.array(colors[(disp >= disp.min())]/255)\n",
    "   \n",
    "    for k in range(4):\n",
    "        points_arr[:,k] = points_arr[:,k]/points_arr[:,3]\n",
    "\n",
    "    points_arr = np.matmul(T[i],points_arr.T).T\n",
    "    thresh = 1500\n",
    "    mask =((-thresh <= points_arr[:,0]) & (points_arr[:,0] < thresh)&\n",
    "            (-thresh <= points_arr[:,1]) & (points_arr[:,1] < thresh) &\n",
    "            (-thresh <= points_arr[:,2]) & (points_arr[:,2] < thresh))\n",
    "            \n",
    "\n",
    "    points_arr = points_arr[mask]\n",
    "    points_arr[:,0] *= -1\n",
    "\n",
    "\n",
    "    pcd.points = o3d.utility.Vector3dVector(points_arr)\n",
    "    pcd.colors = o3d.utility.Vector3dVector(colors_ar[mask])\n",
    "    pcd_final = pcd_final + pcd\n",
    "    print(60+i)\n",
    "\n",
    "#Saving the combined pcd \n",
    "o3d.io.write_point_cloud(\"Output.pcd\", pcd_final)\n",
    "\n",
    "output = o3d.io.read_point_cloud(\"Output.pcd\")\n",
    "o3d.visualization.draw_geometries([output])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Explanation\n",
    "\n",
    "This is essentially an alternate implementation of the code in my point-cloud-registration repo. The data for this code is the same as the data in that repo thus I am not uploading the stereo image-pairs to this repo.\n",
    "\n",
    "Stereo vision systems can be used to generate real time disparity maps and extract 3D information from scenes without the use of super expensive and ugly sensors like LiDAR's. All that's needed is an image pair with sufficient qualilty. A drawback of using this method to obtain 3D world points of a scene is that there should be sufficient lighting. If the 3D world points are obtained, point clouds for the scene can be manufactured and visualised with the help of software such as open3D.\n",
    "\n",
    "Our KITTI data is \"perfect\" for this task as it contains rectified stereo images and the readings from the IMU are taken to be very accurate.\n",
    "\n",
    "Initially, we use a semi global block matching algorithm to find correspondances in the two images. This algorithm works by performing a line optimisation (along the epipolar line) in multiple directions and computing an averge cost function between the intensities of each greyscale image but is highly memory inefficient. Doing this results in a disparity map which is fed to the next part of the code. There are multiple hyperparameters that need tuning in the stereoSGBM matcher which are need to be tuned. Each dataset has unique hyperparameters. The hyper parameters are tuned such that they produce a beautiful disparity for our stereo pair. These parameters dictate the feature matching done along the epipolar line.\n",
    "\n",
    "In order to reproject the image points into the 3D space, we need something called a baseline matrix. This turns points 180 degrees around the x-axis such that the y-axis looks up. \n",
    "\n",
    "We are able to reproject images back to 3d by post multiplying the Q matrix with $\\begin{bmatrix}x & y  &disp[x][y] & 1\\end{bmatrix}^{T}$. This gives us 3D points in the coordinate frame used by opencv. This alone is enough to generate the individual dense point-clouds for each stereo-pair.\n",
    "\n",
    "We now have the points. We extract the colours from the image and bring the colourspace from BGR (which is how opencv reads images) back to RGB.\n",
    "\n",
    "With the 3D points and their corresponding colours, we can generate a point cloud for a given stereo pair.\n",
    "\n",
    "What we aim to do in this project is to register all the point clouds according to the given odometry data obtained from the KITTI IMU. To do this, we must take care that the points of the point cloud are in the same frame as the IMU. Then using matrix multiplication, we get out points into the world frame. A sanity check can be done here to check if the registration is happening properly or not. Instead of passing the points from the image, one could pass a sparse NumPy array which has a small block of ones and every other entry to be zero. This array can then be registered using the given odometry. If a curve appears in the final output then the registration is happening properly."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "----\n",
    "### Motion estimation using iterative PnP\n",
    "\n",
    "Using the generated reconstruction, synthesize a new image taken by a virtualmonocular camera fixed at any arbitrary position and orientation.  We recover this pose using an iterative Perspective-from-n-Points (PnP) algorithm. \n",
    "\n",
    "#### Procedure: \n",
    "\n",
    "<ol>\n",
    "    <li> Obtain a set of 2D-3D correspondences between the the image and the point cloud.  Since here you’re generating the image, this should be easy to obtain. </li>\n",
    "    <li> For this set of correspondences compute the total reprojection error c= $\\sum_{i} ‖x_i−P_{k}X_i‖^2 $    where $P_{k}= K[R_{k}|t_{k}]$, $X_{i}$ is the 3D point in the world frame, $x_{i}$ is its corresponding projection. </li>\n",
    "    <li> Solve for the pose $T_{k}$ that minimizes this non-linear reprojection error using a Gauss-Newton (GN)scheme.  Recall that in GN we start with some initial estimated value $x_{o}$ and iteratively refine the estimate using $x_{1}$= $∆x+x_0$, where $∆x$ is obtained by solving the normal equations $J^{T}J∆x$= -$J^{T}e$, until convergence.The main steps in this scheme are computing the corresponding Jacobians and updating the estimates correctly.  For our problem,  use a 12×1 vector parameterization for $T_{k}$(the top 3×4submatrix).  Run the optimization for different choices of initialization and report your observations. </li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Poses Ground truth \n",
      " [[-8.90929517e+02  5.38387637e+01 -2.56109933e+02 -1.53083396e+05]\n",
      " [-3.68165080e+01  7.13415332e+02 -1.52284250e+02  2.00006057e+01]\n",
      " [-4.11338164e-01  4.10740947e-02 -9.10556891e-01 -2.77880277e+01]]\n",
      "\n",
      "\n",
      "Transforms ground truth \n",
      " [[-9.09854768e-01  4.11782636e-02  4.12878417e-01 -1.92843773e+02]\n",
      " [ 5.44537786e-02  9.98307203e-01  2.04332712e-02  7.22435448e+00]\n",
      " [-4.11338164e-01  4.10740947e-02 -9.10556891e-01 -2.77880277e+01]]\n",
      "\n",
      "\n",
      "Initialised P from DLP\n",
      " [[ 3.20616341e+01 -1.93747852e+00  9.21655800e+00  5.50896991e+03]\n",
      " [ 1.32490589e+00 -2.56734829e+01  5.48021064e+00 -7.19790961e-01]\n",
      " [ 1.48027138e-02 -1.47812755e-03  3.27679579e-02  1.00000000e+00]]\n",
      "\n",
      "Initial cost_arr \n",
      " 712.6278083526804\n",
      "\n",
      "\n",
      "Gauss Newton converged at iteration  13\n",
      "cost  1.187623822830479\n",
      "\n",
      "\n",
      "\n",
      "\n",
      "\n",
      "P_Optimized \n",
      " [[-8.90928661e+02  5.38386754e+01 -2.56107723e+02 -1.53083119e+05]\n",
      " [-3.68143895e+01  7.13415119e+02 -1.52279176e+02  2.04081935e+01]\n",
      " [-4.11338163e-01  4.10740946e-02 -9.10556890e-01 -2.77880277e+01]]\n",
      "\n",
      "\n",
      "T_Output \n",
      " [[-9.09853558e-01  4.11781387e-02  4.12881542e-01 -1.92843382e+02]\n",
      " [ 5.44567747e-02  9.98306902e-01  2.04404477e-02  7.22493091e+00]\n",
      " [-4.11338163e-01  4.10740946e-02 -9.10556890e-01 -2.77880277e+01]]\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 1152x504 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "def read_pcd(file_name):\n",
    "    pcd = o3d.io.read_point_cloud(file_name) \n",
    "    pcd_points = np.hstack((np.asarray(pcd.points),np.ones((len(pcd.points),1))))\n",
    "    return pcd_points\n",
    "\n",
    "\n",
    "def find_Transfroms_gt(T):\n",
    "    Transfroms_gt = np.append(T[0],[0.000000e+00,  0.000000e+00,  0.000000e+00,  1.000000e+00]).reshape(4,4)\n",
    "    Transfroms_gt = np.linalg.inv(Transfroms_gt)\n",
    "    return Transfroms_gt\n",
    "\n",
    "\n",
    "def perform_DLT(x,X):\n",
    "    a = x[0]\n",
    "    x = x[:10,:]\n",
    "    a_X = X[0]\n",
    "    X = X[:10,:]\n",
    "    \n",
    "    for i in range(2):\n",
    "        x[:,i] = x[:,i]/x[:,2]\n",
    "  \n",
    "    a1 = x[:,0]*X[:,2]\n",
    "    a2 = x[:,0]*X[:,3]\n",
    "    u = np.vstack((a1,a2))\n",
    "    v = np.vstack((x[:,0]*X[:,1],u))\n",
    "    w = np.vstack((x[:,0]*X[:,0],v)).T\n",
    "   \n",
    "    a1 = x[:,1]*X[:,2]\n",
    "    a2 = x[:,1]*X[:,3]\n",
    "    u_ = np.vstack((a1,a2))\n",
    "    v_ = np.vstack((x[:,1]*X[:,1],u_))\n",
    "    w_ = np.vstack((x[:,1]*X[:,0],v_)).T\n",
    "    \n",
    "    zeros = np.zeros((len(X),4))\n",
    "    X = -X\n",
    "    a = np.hstack((zeros,w))\n",
    "    a_ = np.hstack((X,w_))\n",
    "    b = np.hstack((zeros,a_))   \n",
    "    \n",
    "    mat = np.vstack((np.hstack((X,a)),b))\n",
    "    \n",
    "    U,D,VT = np.linalg.svd(mat)\n",
    "    \n",
    "    P = np.array(VT[-1])\n",
    "    P = P.reshape(3,4)\n",
    "    \n",
    "    return P/P[-1,-1]\n",
    "\n",
    "\n",
    "def get_Jacobian(x,X):\n",
    "    for i in range(4):\n",
    "        X[:,i] = X[:,i]/x[:,2]\n",
    "        \n",
    "    for i in range(2):\n",
    "        x[:,i] = x[:,i]/x[:,2]\n",
    "  \n",
    "    zeros = np.zeros((len(X),4))\n",
    "    a1 = x[:,0]*X[:,2]\n",
    "    a2 = x[:,0]*X[:,3]\n",
    "    u = np.vstack((a1,a2))\n",
    "    v = np.vstack((x[:,0]*X[:,1],u))\n",
    "    w = np.vstack((x[:,0]*X[:,0],v)).T\n",
    "   \n",
    "    a1 = x[:,1]*X[:,2]\n",
    "    a2 = x[:,1]*X[:,3]\n",
    "    u_ = np.vstack((a1,a2))\n",
    "    v_ = np.vstack((x[:,1]*X[:,1],u_))\n",
    "    w_ = np.vstack((x[:,1]*X[:,0],v_)).T\n",
    "\n",
    "    Jaco = np.vstack((np.hstack((-X,np.hstack((zeros,w)))),np.hstack((zeros,np.hstack((-X,w_))))))\n",
    "    return Jaco\n",
    "\n",
    "def get_residual(x,x_ground_truth):\n",
    "    den = x_ground_truth[:,2]\n",
    "    res_2= x_ground_truth[:,1]/den\n",
    "    res_2 -= x[:,1]/x[:,2]\n",
    "    res_1 = x_ground_truth[:,0]/den\n",
    "    res_1 -= x[:,0]/x[:,2]\n",
    "    residual = np.hstack((res_1,res_2))\n",
    "    residual = residual.reshape(-1,1)\n",
    "    return residual\n",
    "\n",
    "\n",
    "def perform_Gauss_newton(P,x_ground_truth,X,nums,tolerance):\n",
    "    residual = np.zeros(X.shape)\n",
    "    cost_arr = np.zeros(nums)\n",
    "    for i in range(nums):\n",
    "        x = np.matmul(P,X.T).T\n",
    "        residual = get_residual(x,x_ground_truth) \n",
    "        cost_arr[i] = (np.matmul(residual.T,residual)/2/len(X)).item(0)\n",
    "        J = get_Jacobian(x,X)\n",
    "        H = np.matmul(J.T,J)\n",
    "        delta = np.linalg.inv(H)@J.T@residual\n",
    "        P = (P.reshape(12,1) - delta).reshape(3,4)    \n",
    "        if(i==0):\n",
    "            print(\"\\nInitial cost_arr \\n\",cost_arr[0])\n",
    "        norm = np.linalg.norm(delta)   \n",
    "        if(norm<tolerance):\n",
    "            print(\"\\n\")\n",
    "            print(\"Gauss Newton converged at iteration \",i)\n",
    "            print(\"cost \",cost_arr[i])\n",
    "            print(\"\\n\\n\")\n",
    "            break   \n",
    "    return P, cost_arr, i\n",
    "X_ground_truth = read_pcd(\"Output.pcd\")[0::10,:]\n",
    "Transfroms_gt = find_Transfroms_gt(T)\n",
    "P_ground_truth = np.matmul(calib_mat,Transfroms_gt[:3,:4])\n",
    "x_ground_truth = np.matmul(P_ground_truth,X_ground_truth.T).T\n",
    "\n",
    "P_init = perform_DLT(x_ground_truth,X_ground_truth)\n",
    "print(\"Poses Ground truth \\n\",P_ground_truth)\n",
    "k_inv = np.linalg.inv(calib_mat)\n",
    "Transfroms_gt = np.matmul(k_inv,P_ground_truth)\n",
    "print('\\n')\n",
    "print(\"Transforms ground truth \\n\",Transfroms_gt)\n",
    "print('\\n')\n",
    "print(\"Initialised P from DLP\\n\",P_init)\n",
    "\n",
    "\n",
    "P_optimized, cost_arr, it_conv = perform_Gauss_newton(P_init,x_ground_truth,X_ground_truth,1000,1e-7)\n",
    "P_optimized = P_ground_truth[-1,-1]*P_optimized/P_optimized[-1,-1] \n",
    "\n",
    "print('\\n')\n",
    "print(\"P_Optimized \\n\",P_optimized)\n",
    "\n",
    "k_inv = np.linalg.inv(calib_mat)\n",
    "T_op = np.matmul(k_inv,P_optimized)\n",
    "print('\\n')\n",
    "print(\"T_Output \\n\",T_op)\n",
    "\n",
    "def plot(it_conv,cost_arr):\n",
    "    plt.figure(figsize=(16,7))\n",
    "    y = cost_arr[0:it_conv+1]\n",
    "    x = range(it_conv+1)\n",
    "    plt.plot(x, y)\n",
    "    plt.xlabel('Iterations')\n",
    "    plt.ylabel('cost_arr ')\n",
    "\n",
    "\n",
    "plot(it_conv,cost_arr)"
   ]
  }
 ],
 "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
}