examples/example_multi_pose_6d.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2\n",
    "import pyprogressivex\n",
    "from time import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Number of correspondences loaded =  1886\n",
      "Number of ground truth poses loaded =  2\n"
     ]
    }
   ],
   "source": [
    "# Load the correspondences from file\n",
    "correspondences = np.loadtxt('img/tless.txt')\n",
    "n = len(correspondences)\n",
    "\n",
    "imagePoints = np.float32([correspondences[i][0:2] for i in np.arange(n)]).reshape(-1,2)\n",
    "worldPoints = np.float32([correspondences[i][2:5] for i in np.arange(n)]).reshape(-1,3)\n",
    "\n",
    "groundTruthPoses = np.loadtxt('img/tless_poses.txt')\n",
    "intrinsicParams = np.loadtxt('img/tless_intrinsics.txt')\n",
    "groundTruthModelNumber = groundTruthPoses.shape[0];\n",
    "\n",
    "print(\"Number of correspondences loaded = \", str(len(correspondences)))\n",
    "print(\"Number of ground truth poses loaded = \", str(groundTruthModelNumber))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def verify_pyprogressivex(imagePoints, worldPoints, intrinsicParams):    \n",
    "    \n",
    "    threshold = 4.0\n",
    "    \n",
    "    poses, labeling = pyprogressivex.find6DPoses(np.ascontiguousarray(imagePoints), np.ascontiguousarray(worldPoints), np.ascontiguousarray(intrinsicParams), threshold)    \n",
    "    return poses, labeling\n",
    "\n",
    "def calculate_error(gt_pose, est_pose):\n",
    "    \n",
    "    R2R1 = np.dot(gt_pose[:, 0:3].T, est_pose[:, 0:3])\n",
    "    cos_angle = max(-1.0, min(1.0, 0.5 * (R2R1.trace() - 1.0)))\n",
    "    \n",
    "    err_R = np.arccos(cos_angle) * 180.0 / np.pi\n",
    "    err_t = np.linalg.norm(gt_pose[:, 3] - est_pose[:, 3])\n",
    "    \n",
    "    return err_R, err_t\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Time =  57.572970151901245  sec\n",
      "Models found = 9.0\n"
     ]
    }
   ],
   "source": [
    "t = time()\n",
    "\n",
    "poses, labeling = verify_pyprogressivex(imagePoints, worldPoints, intrinsicParams)\n",
    "model_number = poses.size / 12\n",
    "\n",
    "print ('Time = ', time()-t, ' sec')\n",
    "print ('Models found = {}'.format(model_number))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Evaluating the 1. ground truth pose\n",
      "Rotation error =  8.249138186163485 °\n",
      "Translation error =  2.4035577603771756  cm\n",
      "\n",
      "Evaluating the 2. ground truth pose\n",
      "Rotation error =  0.9491160622888057 °\n",
      "Translation error =  1.2155621577125257  cm\n",
      "\n"
     ]
    }
   ],
   "source": [
    "for i in np.arange(groundTruthModelNumber):\n",
    "    gtPose = groundTruthPoses[i].reshape(3, 4)\n",
    "    minRotationError = 1e10\n",
    "    minTranslationError = 1e10\n",
    "    \n",
    "    for j in np.arange(model_number):\n",
    "        \n",
    "        estPose = poses[int(j * 3) : int((j + 1) * 3)]\n",
    "    \n",
    "        errR, errt = calculate_error(gtPose, estPose)\n",
    "        \n",
    "        if errR + errt < minRotationError + minTranslationError:\n",
    "            minRotationError = errR\n",
    "            minTranslationError = errt\n",
    "\n",
    "    print ('Evaluating the {}. ground truth pose'.format(i + 1))\n",
    "    print ('Rotation error = ', minRotationError, '°')\n",
    "    print ('Translation error = ', minTranslationError / 10.0, ' cm\\n')\n"
   ]
  },
  {
   "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.9.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}