{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "d7727921",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "import pandas as pd\n",
    "import argparse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "9c12de1e",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "usage: ipykernel_launcher.py [-h] folder\n",
      "ipykernel_launcher.py: error: unrecognized arguments: -f\n"
     ]
    },
    {
     "ename": "SystemExit",
     "evalue": "2",
     "output_type": "error",
     "traceback": [
      "An exception has occurred, use %tb to see the full traceback.\n",
      "\u001b[0;31mSystemExit\u001b[0m\u001b[0;31m:\u001b[0m 2\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/ivan/.local/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3449: UserWarning: To exit: use 'exit', 'quit', or Ctrl-D.\n",
      "  warn(\"To exit: use 'exit', 'quit', or Ctrl-D.\", stacklevel=1)\n"
     ]
    }
   ],
   "source": [
    "parser = argparse.ArgumentParser(description='''This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. ''')\n",
    "parser.add_argument(\"folder\")\n",
    "args = parser.parse_args()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "id": "36d8cbb6",
   "metadata": {},
   "outputs": [],
   "source": [
    "#folder = args.folder\n",
    "folder = \"dataset-room3_512_16\"\n",
    "df = pd.read_csv(folder + \"/trajectory.csv\", sep=\",\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "id": "281a4aa2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>#timestamp [ns]</th>\n",
       "      <th>p_RS_R_x [m]</th>\n",
       "      <th>p_RS_R_y [m]</th>\n",
       "      <th>p_RS_R_z [m]</th>\n",
       "      <th>q_RS_w []</th>\n",
       "      <th>q_RS_x []</th>\n",
       "      <th>q_RS_y []</th>\n",
       "      <th>q_RS_z []</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>1520530962750148976</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.999931</td>\n",
       "      <td>0.005551</td>\n",
       "      <td>-0.010374</td>\n",
       "      <td>0.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>1520530962800150976</td>\n",
       "      <td>-0.000226</td>\n",
       "      <td>-0.000111</td>\n",
       "      <td>-0.000192</td>\n",
       "      <td>0.999960</td>\n",
       "      <td>0.001061</td>\n",
       "      <td>-0.008412</td>\n",
       "      <td>0.002790</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>1520530962850151976</td>\n",
       "      <td>-0.000734</td>\n",
       "      <td>-0.000509</td>\n",
       "      <td>-0.000107</td>\n",
       "      <td>0.999943</td>\n",
       "      <td>-0.001249</td>\n",
       "      <td>-0.008413</td>\n",
       "      <td>0.006502</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>1520530962900153976</td>\n",
       "      <td>-0.001732</td>\n",
       "      <td>-0.001568</td>\n",
       "      <td>0.000353</td>\n",
       "      <td>0.999914</td>\n",
       "      <td>-0.000781</td>\n",
       "      <td>-0.009907</td>\n",
       "      <td>0.008531</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>1520530962950155976</td>\n",
       "      <td>0.004681</td>\n",
       "      <td>-0.001322</td>\n",
       "      <td>0.001064</td>\n",
       "      <td>0.999914</td>\n",
       "      <td>-0.008977</td>\n",
       "      <td>0.000322</td>\n",
       "      <td>0.009544</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>...</th>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2816</th>\n",
       "      <td>1520531103554976976</td>\n",
       "      <td>-0.389726</td>\n",
       "      <td>0.840284</td>\n",
       "      <td>0.106658</td>\n",
       "      <td>-0.089800</td>\n",
       "      <td>-0.011144</td>\n",
       "      <td>-0.030912</td>\n",
       "      <td>-0.995418</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2817</th>\n",
       "      <td>1520531103604978976</td>\n",
       "      <td>-0.391111</td>\n",
       "      <td>0.840745</td>\n",
       "      <td>0.106293</td>\n",
       "      <td>-0.088027</td>\n",
       "      <td>-0.011046</td>\n",
       "      <td>-0.031439</td>\n",
       "      <td>-0.995561</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2818</th>\n",
       "      <td>1520531103654992888</td>\n",
       "      <td>-0.392485</td>\n",
       "      <td>0.841127</td>\n",
       "      <td>0.105756</td>\n",
       "      <td>-0.086472</td>\n",
       "      <td>-0.011334</td>\n",
       "      <td>-0.033143</td>\n",
       "      <td>-0.995638</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2819</th>\n",
       "      <td>1520531103704994888</td>\n",
       "      <td>-0.393789</td>\n",
       "      <td>0.841174</td>\n",
       "      <td>0.104638</td>\n",
       "      <td>-0.084300</td>\n",
       "      <td>-0.011306</td>\n",
       "      <td>-0.035558</td>\n",
       "      <td>-0.995742</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2820</th>\n",
       "      <td>1520531103754995888</td>\n",
       "      <td>-0.395145</td>\n",
       "      <td>0.841273</td>\n",
       "      <td>0.103519</td>\n",
       "      <td>-0.082643</td>\n",
       "      <td>-0.010543</td>\n",
       "      <td>-0.037899</td>\n",
       "      <td>-0.995802</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>2821 rows × 8 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "          #timestamp [ns]  p_RS_R_x [m]  p_RS_R_y [m]  p_RS_R_z [m]  \\\n",
       "0     1520530962750148976      0.000000      0.000000      0.000000   \n",
       "1     1520530962800150976     -0.000226     -0.000111     -0.000192   \n",
       "2     1520530962850151976     -0.000734     -0.000509     -0.000107   \n",
       "3     1520530962900153976     -0.001732     -0.001568      0.000353   \n",
       "4     1520530962950155976      0.004681     -0.001322      0.001064   \n",
       "...                   ...           ...           ...           ...   \n",
       "2816  1520531103554976976     -0.389726      0.840284      0.106658   \n",
       "2817  1520531103604978976     -0.391111      0.840745      0.106293   \n",
       "2818  1520531103654992888     -0.392485      0.841127      0.105756   \n",
       "2819  1520531103704994888     -0.393789      0.841174      0.104638   \n",
       "2820  1520531103754995888     -0.395145      0.841273      0.103519   \n",
       "\n",
       "      q_RS_w []  q_RS_x []  q_RS_y []  q_RS_z []  \n",
       "0      0.999931   0.005551  -0.010374   0.000000  \n",
       "1      0.999960   0.001061  -0.008412   0.002790  \n",
       "2      0.999943  -0.001249  -0.008413   0.006502  \n",
       "3      0.999914  -0.000781  -0.009907   0.008531  \n",
       "4      0.999914  -0.008977   0.000322   0.009544  \n",
       "...         ...        ...        ...        ...  \n",
       "2816  -0.089800  -0.011144  -0.030912  -0.995418  \n",
       "2817  -0.088027  -0.011046  -0.031439  -0.995561  \n",
       "2818  -0.086472  -0.011334  -0.033143  -0.995638  \n",
       "2819  -0.084300  -0.011306  -0.035558  -0.995742  \n",
       "2820  -0.082643  -0.010543  -0.037899  -0.995802  \n",
       "\n",
       "[2821 rows x 8 columns]"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "id": "88df5193",
   "metadata": {},
   "outputs": [],
   "source": [
    "df = df.rename(columns={\"#timestamp [ns]\":\"time[s]\", \"p_RS_R_x [m]\":\"x\", \"p_RS_R_y [m]\":\"y\", \"p_RS_R_z [m]\":\"z\", \\\n",
    "                   \"q_RS_w []\":\"qw\", \"q_RS_x []\":\"qx\", \"q_RS_y []\":\"qy\", \"q_RS_z []\":\"qz\"})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "id": "c334bf14",
   "metadata": {},
   "outputs": [],
   "source": [
    "tmp = df[\"qw\"]\n",
    "df = df.drop(\"qw\", axis=1)\n",
    "df[\"qw\"] = tmp.to_numpy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "id": "22886efc",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>time[s]</th>\n",
       "      <th>x</th>\n",
       "      <th>y</th>\n",
       "      <th>z</th>\n",
       "      <th>qx</th>\n",
       "      <th>qy</th>\n",
       "      <th>qz</th>\n",
       "      <th>qw</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>1520530962750148976</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.005551</td>\n",
       "      <td>-0.010374</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.999931</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>1520530962800150976</td>\n",
       "      <td>-0.000226</td>\n",
       "      <td>-0.000111</td>\n",
       "      <td>-0.000192</td>\n",
       "      <td>0.001061</td>\n",
       "      <td>-0.008412</td>\n",
       "      <td>0.002790</td>\n",
       "      <td>0.999960</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>1520530962850151976</td>\n",
       "      <td>-0.000734</td>\n",
       "      <td>-0.000509</td>\n",
       "      <td>-0.000107</td>\n",
       "      <td>-0.001249</td>\n",
       "      <td>-0.008413</td>\n",
       "      <td>0.006502</td>\n",
       "      <td>0.999943</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>1520530962900153976</td>\n",
       "      <td>-0.001732</td>\n",
       "      <td>-0.001568</td>\n",
       "      <td>0.000353</td>\n",
       "      <td>-0.000781</td>\n",
       "      <td>-0.009907</td>\n",
       "      <td>0.008531</td>\n",
       "      <td>0.999914</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>1520530962950155976</td>\n",
       "      <td>0.004681</td>\n",
       "      <td>-0.001322</td>\n",
       "      <td>0.001064</td>\n",
       "      <td>-0.008977</td>\n",
       "      <td>0.000322</td>\n",
       "      <td>0.009544</td>\n",
       "      <td>0.999914</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>...</th>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2816</th>\n",
       "      <td>1520531103554976976</td>\n",
       "      <td>-0.389726</td>\n",
       "      <td>0.840284</td>\n",
       "      <td>0.106658</td>\n",
       "      <td>-0.011144</td>\n",
       "      <td>-0.030912</td>\n",
       "      <td>-0.995418</td>\n",
       "      <td>-0.089800</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2817</th>\n",
       "      <td>1520531103604978976</td>\n",
       "      <td>-0.391111</td>\n",
       "      <td>0.840745</td>\n",
       "      <td>0.106293</td>\n",
       "      <td>-0.011046</td>\n",
       "      <td>-0.031439</td>\n",
       "      <td>-0.995561</td>\n",
       "      <td>-0.088027</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2818</th>\n",
       "      <td>1520531103654992888</td>\n",
       "      <td>-0.392485</td>\n",
       "      <td>0.841127</td>\n",
       "      <td>0.105756</td>\n",
       "      <td>-0.011334</td>\n",
       "      <td>-0.033143</td>\n",
       "      <td>-0.995638</td>\n",
       "      <td>-0.086472</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2819</th>\n",
       "      <td>1520531103704994888</td>\n",
       "      <td>-0.393789</td>\n",
       "      <td>0.841174</td>\n",
       "      <td>0.104638</td>\n",
       "      <td>-0.011306</td>\n",
       "      <td>-0.035558</td>\n",
       "      <td>-0.995742</td>\n",
       "      <td>-0.084300</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2820</th>\n",
       "      <td>1520531103754995888</td>\n",
       "      <td>-0.395145</td>\n",
       "      <td>0.841273</td>\n",
       "      <td>0.103519</td>\n",
       "      <td>-0.010543</td>\n",
       "      <td>-0.037899</td>\n",
       "      <td>-0.995802</td>\n",
       "      <td>-0.082643</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>2821 rows × 8 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "                  time[s]         x         y         z        qx        qy  \\\n",
       "0     1520530962750148976  0.000000  0.000000  0.000000  0.005551 -0.010374   \n",
       "1     1520530962800150976 -0.000226 -0.000111 -0.000192  0.001061 -0.008412   \n",
       "2     1520530962850151976 -0.000734 -0.000509 -0.000107 -0.001249 -0.008413   \n",
       "3     1520530962900153976 -0.001732 -0.001568  0.000353 -0.000781 -0.009907   \n",
       "4     1520530962950155976  0.004681 -0.001322  0.001064 -0.008977  0.000322   \n",
       "...                   ...       ...       ...       ...       ...       ...   \n",
       "2816  1520531103554976976 -0.389726  0.840284  0.106658 -0.011144 -0.030912   \n",
       "2817  1520531103604978976 -0.391111  0.840745  0.106293 -0.011046 -0.031439   \n",
       "2818  1520531103654992888 -0.392485  0.841127  0.105756 -0.011334 -0.033143   \n",
       "2819  1520531103704994888 -0.393789  0.841174  0.104638 -0.011306 -0.035558   \n",
       "2820  1520531103754995888 -0.395145  0.841273  0.103519 -0.010543 -0.037899   \n",
       "\n",
       "            qz        qw  \n",
       "0     0.000000  0.999931  \n",
       "1     0.002790  0.999960  \n",
       "2     0.006502  0.999943  \n",
       "3     0.008531  0.999914  \n",
       "4     0.009544  0.999914  \n",
       "...        ...       ...  \n",
       "2816 -0.995418 -0.089800  \n",
       "2817 -0.995561 -0.088027  \n",
       "2818 -0.995638 -0.086472  \n",
       "2819 -0.995742 -0.084300  \n",
       "2820 -0.995802 -0.082643  \n",
       "\n",
       "[2821 rows x 8 columns]"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "id": "32db6131",
   "metadata": {},
   "outputs": [],
   "source": [
    "df.to_csv(folder + \"/f_dataset-room3_512_16_stereoi_basalt.txt\", sep=\" \", index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f14c889d",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}