{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "a0f4e1a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "import pandas as pd\n",
    "import argparse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "eb58c409",
   "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": 49,
   "id": "2f0e5686",
   "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": 50,
   "id": "c64ed64a",
   "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>1.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>1520530962800150976</td>\n",
       "      <td>0.001154</td>\n",
       "      <td>-0.000453</td>\n",
       "      <td>-0.000584</td>\n",
       "      <td>0.999984</td>\n",
       "      <td>-0.004628</td>\n",
       "      <td>0.002044</td>\n",
       "      <td>0.002671</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>1520530962850151976</td>\n",
       "      <td>0.002451</td>\n",
       "      <td>-0.000723</td>\n",
       "      <td>-0.000083</td>\n",
       "      <td>0.999953</td>\n",
       "      <td>-0.006943</td>\n",
       "      <td>0.002162</td>\n",
       "      <td>0.006434</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>1520530962900153976</td>\n",
       "      <td>0.003486</td>\n",
       "      <td>-0.000895</td>\n",
       "      <td>0.000221</td>\n",
       "      <td>0.999941</td>\n",
       "      <td>-0.006661</td>\n",
       "      <td>0.000697</td>\n",
       "      <td>0.008585</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>1520530962950155976</td>\n",
       "      <td>0.004718</td>\n",
       "      <td>-0.001340</td>\n",
       "      <td>0.001000</td>\n",
       "      <td>0.999948</td>\n",
       "      <td>-0.003669</td>\n",
       "      <td>-0.001251</td>\n",
       "      <td>0.009422</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.388835</td>\n",
       "      <td>0.679493</td>\n",
       "      <td>0.048913</td>\n",
       "      <td>-0.067123</td>\n",
       "      <td>-0.028861</td>\n",
       "      <td>-0.024538</td>\n",
       "      <td>-0.997025</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2817</th>\n",
       "      <td>1520531103604978976</td>\n",
       "      <td>-0.390200</td>\n",
       "      <td>0.679882</td>\n",
       "      <td>0.048612</td>\n",
       "      <td>-0.065333</td>\n",
       "      <td>-0.028774</td>\n",
       "      <td>-0.025024</td>\n",
       "      <td>-0.997135</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2818</th>\n",
       "      <td>1520531103654992888</td>\n",
       "      <td>-0.391548</td>\n",
       "      <td>0.680447</td>\n",
       "      <td>0.047928</td>\n",
       "      <td>-0.063690</td>\n",
       "      <td>-0.029049</td>\n",
       "      <td>-0.026683</td>\n",
       "      <td>-0.997190</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2819</th>\n",
       "      <td>1520531103704994888</td>\n",
       "      <td>-0.392822</td>\n",
       "      <td>0.680516</td>\n",
       "      <td>0.047170</td>\n",
       "      <td>-0.061396</td>\n",
       "      <td>-0.028912</td>\n",
       "      <td>-0.029105</td>\n",
       "      <td>-0.997270</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2820</th>\n",
       "      <td>1520531103754995888</td>\n",
       "      <td>-0.394133</td>\n",
       "      <td>0.680577</td>\n",
       "      <td>0.046144</td>\n",
       "      <td>-0.059644</td>\n",
       "      <td>-0.028037</td>\n",
       "      <td>-0.031506</td>\n",
       "      <td>-0.997328</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.001154     -0.000453     -0.000584   \n",
       "2     1520530962850151976      0.002451     -0.000723     -0.000083   \n",
       "3     1520530962900153976      0.003486     -0.000895      0.000221   \n",
       "4     1520530962950155976      0.004718     -0.001340      0.001000   \n",
       "...                   ...           ...           ...           ...   \n",
       "2816  1520531103554976976     -0.388835      0.679493      0.048913   \n",
       "2817  1520531103604978976     -0.390200      0.679882      0.048612   \n",
       "2818  1520531103654992888     -0.391548      0.680447      0.047928   \n",
       "2819  1520531103704994888     -0.392822      0.680516      0.047170   \n",
       "2820  1520531103754995888     -0.394133      0.680577      0.046144   \n",
       "\n",
       "      q_RS_w []  q_RS_x []  q_RS_y []  q_RS_z []  \n",
       "0      1.000000   0.000000   0.000000   0.000000  \n",
       "1      0.999984  -0.004628   0.002044   0.002671  \n",
       "2      0.999953  -0.006943   0.002162   0.006434  \n",
       "3      0.999941  -0.006661   0.000697   0.008585  \n",
       "4      0.999948  -0.003669  -0.001251   0.009422  \n",
       "...         ...        ...        ...        ...  \n",
       "2816  -0.067123  -0.028861  -0.024538  -0.997025  \n",
       "2817  -0.065333  -0.028774  -0.025024  -0.997135  \n",
       "2818  -0.063690  -0.029049  -0.026683  -0.997190  \n",
       "2819  -0.061396  -0.028912  -0.029105  -0.997270  \n",
       "2820  -0.059644  -0.028037  -0.031506  -0.997328  \n",
       "\n",
       "[2821 rows x 8 columns]"
      ]
     },
     "execution_count": 50,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "id": "fce211b8",
   "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": 52,
   "id": "1457f45a",
   "metadata": {},
   "outputs": [],
   "source": [
    "tmp = df[\"qw\"]\n",
    "df = df.drop(\"qw\", axis=1)\n",
    "df[\"qw\"] = tmp.to_numpy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "id": "9b75e536",
   "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.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>0.000000</td>\n",
       "      <td>1.000000</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>1520530962800150976</td>\n",
       "      <td>0.001154</td>\n",
       "      <td>-0.000453</td>\n",
       "      <td>-0.000584</td>\n",
       "      <td>-0.004628</td>\n",
       "      <td>0.002044</td>\n",
       "      <td>0.002671</td>\n",
       "      <td>0.999984</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>1520530962850151976</td>\n",
       "      <td>0.002451</td>\n",
       "      <td>-0.000723</td>\n",
       "      <td>-0.000083</td>\n",
       "      <td>-0.006943</td>\n",
       "      <td>0.002162</td>\n",
       "      <td>0.006434</td>\n",
       "      <td>0.999953</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>1520530962900153976</td>\n",
       "      <td>0.003486</td>\n",
       "      <td>-0.000895</td>\n",
       "      <td>0.000221</td>\n",
       "      <td>-0.006661</td>\n",
       "      <td>0.000697</td>\n",
       "      <td>0.008585</td>\n",
       "      <td>0.999941</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>1520530962950155976</td>\n",
       "      <td>0.004718</td>\n",
       "      <td>-0.001340</td>\n",
       "      <td>0.001000</td>\n",
       "      <td>-0.003669</td>\n",
       "      <td>-0.001251</td>\n",
       "      <td>0.009422</td>\n",
       "      <td>0.999948</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.388835</td>\n",
       "      <td>0.679493</td>\n",
       "      <td>0.048913</td>\n",
       "      <td>-0.028861</td>\n",
       "      <td>-0.024538</td>\n",
       "      <td>-0.997025</td>\n",
       "      <td>-0.067123</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2817</th>\n",
       "      <td>1520531103604978976</td>\n",
       "      <td>-0.390200</td>\n",
       "      <td>0.679882</td>\n",
       "      <td>0.048612</td>\n",
       "      <td>-0.028774</td>\n",
       "      <td>-0.025024</td>\n",
       "      <td>-0.997135</td>\n",
       "      <td>-0.065333</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2818</th>\n",
       "      <td>1520531103654992888</td>\n",
       "      <td>-0.391548</td>\n",
       "      <td>0.680447</td>\n",
       "      <td>0.047928</td>\n",
       "      <td>-0.029049</td>\n",
       "      <td>-0.026683</td>\n",
       "      <td>-0.997190</td>\n",
       "      <td>-0.063690</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2819</th>\n",
       "      <td>1520531103704994888</td>\n",
       "      <td>-0.392822</td>\n",
       "      <td>0.680516</td>\n",
       "      <td>0.047170</td>\n",
       "      <td>-0.028912</td>\n",
       "      <td>-0.029105</td>\n",
       "      <td>-0.997270</td>\n",
       "      <td>-0.061396</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2820</th>\n",
       "      <td>1520531103754995888</td>\n",
       "      <td>-0.394133</td>\n",
       "      <td>0.680577</td>\n",
       "      <td>0.046144</td>\n",
       "      <td>-0.028037</td>\n",
       "      <td>-0.031506</td>\n",
       "      <td>-0.997328</td>\n",
       "      <td>-0.059644</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.000000  0.000000   \n",
       "1     1520530962800150976  0.001154 -0.000453 -0.000584 -0.004628  0.002044   \n",
       "2     1520530962850151976  0.002451 -0.000723 -0.000083 -0.006943  0.002162   \n",
       "3     1520530962900153976  0.003486 -0.000895  0.000221 -0.006661  0.000697   \n",
       "4     1520530962950155976  0.004718 -0.001340  0.001000 -0.003669 -0.001251   \n",
       "...                   ...       ...       ...       ...       ...       ...   \n",
       "2816  1520531103554976976 -0.388835  0.679493  0.048913 -0.028861 -0.024538   \n",
       "2817  1520531103604978976 -0.390200  0.679882  0.048612 -0.028774 -0.025024   \n",
       "2818  1520531103654992888 -0.391548  0.680447  0.047928 -0.029049 -0.026683   \n",
       "2819  1520531103704994888 -0.392822  0.680516  0.047170 -0.028912 -0.029105   \n",
       "2820  1520531103754995888 -0.394133  0.680577  0.046144 -0.028037 -0.031506   \n",
       "\n",
       "            qz        qw  \n",
       "0     0.000000  1.000000  \n",
       "1     0.002671  0.999984  \n",
       "2     0.006434  0.999953  \n",
       "3     0.008585  0.999941  \n",
       "4     0.009422  0.999948  \n",
       "...        ...       ...  \n",
       "2816 -0.997025 -0.067123  \n",
       "2817 -0.997135 -0.065333  \n",
       "2818 -0.997190 -0.063690  \n",
       "2819 -0.997270 -0.061396  \n",
       "2820 -0.997328 -0.059644  \n",
       "\n",
       "[2821 rows x 8 columns]"
      ]
     },
     "execution_count": 53,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "id": "0f988fae",
   "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": "f7d223fe",
   "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
}