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

535 lines
17 KiB
Plaintext
Raw Permalink 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": "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
}
Reference in New Issue View Git Blame Copy Permalink