pi/slam_systems_research
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
b6256d47b820648d81e7f252b32bac46442f26ca
slam_systems_research/tum_vi/testing_basalt_converter.ipynb
Ivan 281a8818a3 v02
2022-04-05 11:45:47 +03:00

535 lines
17 KiB
Plaintext
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
{
"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
}
Reference in New Issue View Git Blame Copy Permalink