504 lines
17 KiB
Plaintext
504 lines
17 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 10,
|
||
"id": "805fb727",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"import json\n",
|
||
"import pandas as pd\n",
|
||
"import numpy as np\n",
|
||
"from scipy.spatial.transform import Rotation as R"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 11,
|
||
"id": "456ca3a7",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"path = \"/home/ivan/ivan/git/work_drivecast2/SLAM/ORB_SLAM3-1.0-release/Examples/Stereo/CameraTrajectory.txt\"\n",
|
||
"traj = pd.read_csv(path, sep=\" \")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 12,
|
||
"id": "95b5c6ad",
|
||
"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>rwc(0,0)</th>\n",
|
||
" <th>rwc(0,1)</th>\n",
|
||
" <th>rwc(0,2)</th>\n",
|
||
" <th>twc(0)</th>\n",
|
||
" <th>rwc(1,0)</th>\n",
|
||
" <th>rwc(1,1)</th>\n",
|
||
" <th>rwc(1,2)</th>\n",
|
||
" <th>twc(1)</th>\n",
|
||
" <th>rwc(2,0)</th>\n",
|
||
" <th>rwc(2,1)</th>\n",
|
||
" <th>rwc(2,2)</th>\n",
|
||
" <th>twc(2)</th>\n",
|
||
" </tr>\n",
|
||
" </thead>\n",
|
||
" <tbody>\n",
|
||
" <tr>\n",
|
||
" <th>0</th>\n",
|
||
" <td>1.000000</td>\n",
|
||
" <td>-0.000096</td>\n",
|
||
" <td>-0.000134</td>\n",
|
||
" <td>0.002967</td>\n",
|
||
" <td>0.000096</td>\n",
|
||
" <td>1.000000</td>\n",
|
||
" <td>-0.000257</td>\n",
|
||
" <td>0.003075</td>\n",
|
||
" <td>0.000134</td>\n",
|
||
" <td>0.000257</td>\n",
|
||
" <td>1.000000</td>\n",
|
||
" <td>-0.002494</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1</th>\n",
|
||
" <td>0.999998</td>\n",
|
||
" <td>0.000914</td>\n",
|
||
" <td>0.001881</td>\n",
|
||
" <td>-0.006111</td>\n",
|
||
" <td>-0.000911</td>\n",
|
||
" <td>0.999999</td>\n",
|
||
" <td>-0.001395</td>\n",
|
||
" <td>-0.013490</td>\n",
|
||
" <td>-0.001882</td>\n",
|
||
" <td>0.001394</td>\n",
|
||
" <td>0.999997</td>\n",
|
||
" <td>1.208325</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>2</th>\n",
|
||
" <td>0.999995</td>\n",
|
||
" <td>0.001234</td>\n",
|
||
" <td>0.002927</td>\n",
|
||
" <td>-0.004074</td>\n",
|
||
" <td>-0.001232</td>\n",
|
||
" <td>0.999999</td>\n",
|
||
" <td>-0.000782</td>\n",
|
||
" <td>-0.036465</td>\n",
|
||
" <td>-0.002928</td>\n",
|
||
" <td>0.000779</td>\n",
|
||
" <td>0.999995</td>\n",
|
||
" <td>2.407830</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>3</th>\n",
|
||
" <td>0.999991</td>\n",
|
||
" <td>0.000727</td>\n",
|
||
" <td>0.004202</td>\n",
|
||
" <td>-0.006533</td>\n",
|
||
" <td>-0.000725</td>\n",
|
||
" <td>1.000000</td>\n",
|
||
" <td>-0.000472</td>\n",
|
||
" <td>-0.056512</td>\n",
|
||
" <td>-0.004202</td>\n",
|
||
" <td>0.000469</td>\n",
|
||
" <td>0.999991</td>\n",
|
||
" <td>3.606258</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>4</th>\n",
|
||
" <td>0.999987</td>\n",
|
||
" <td>0.000512</td>\n",
|
||
" <td>0.005128</td>\n",
|
||
" <td>-0.006541</td>\n",
|
||
" <td>-0.000510</td>\n",
|
||
" <td>1.000000</td>\n",
|
||
" <td>-0.000432</td>\n",
|
||
" <td>-0.077331</td>\n",
|
||
" <td>-0.005129</td>\n",
|
||
" <td>0.000430</td>\n",
|
||
" <td>0.999987</td>\n",
|
||
" <td>4.806717</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",
|
||
" <td>...</td>\n",
|
||
" <td>...</td>\n",
|
||
" <td>...</td>\n",
|
||
" <td>...</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1096</th>\n",
|
||
" <td>0.999676</td>\n",
|
||
" <td>0.018372</td>\n",
|
||
" <td>0.017593</td>\n",
|
||
" <td>0.315591</td>\n",
|
||
" <td>-0.018334</td>\n",
|
||
" <td>0.999829</td>\n",
|
||
" <td>-0.002316</td>\n",
|
||
" <td>-4.811160</td>\n",
|
||
" <td>-0.017633</td>\n",
|
||
" <td>0.001993</td>\n",
|
||
" <td>0.999843</td>\n",
|
||
" <td>298.391571</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1097</th>\n",
|
||
" <td>0.999719</td>\n",
|
||
" <td>0.017335</td>\n",
|
||
" <td>0.016196</td>\n",
|
||
" <td>0.324796</td>\n",
|
||
" <td>-0.017322</td>\n",
|
||
" <td>0.999850</td>\n",
|
||
" <td>-0.000900</td>\n",
|
||
" <td>-4.826771</td>\n",
|
||
" <td>-0.016209</td>\n",
|
||
" <td>0.000619</td>\n",
|
||
" <td>0.999868</td>\n",
|
||
" <td>299.177399</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1098</th>\n",
|
||
" <td>0.999756</td>\n",
|
||
" <td>0.016880</td>\n",
|
||
" <td>0.014275</td>\n",
|
||
" <td>0.339924</td>\n",
|
||
" <td>-0.016880</td>\n",
|
||
" <td>0.999858</td>\n",
|
||
" <td>-0.000125</td>\n",
|
||
" <td>-4.839842</td>\n",
|
||
" <td>-0.014275</td>\n",
|
||
" <td>-0.000116</td>\n",
|
||
" <td>0.999898</td>\n",
|
||
" <td>299.979767</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1099</th>\n",
|
||
" <td>0.999804</td>\n",
|
||
" <td>0.014992</td>\n",
|
||
" <td>0.012966</td>\n",
|
||
" <td>0.347605</td>\n",
|
||
" <td>-0.015007</td>\n",
|
||
" <td>0.999887</td>\n",
|
||
" <td>0.001013</td>\n",
|
||
" <td>-4.852088</td>\n",
|
||
" <td>-0.012949</td>\n",
|
||
" <td>-0.001207</td>\n",
|
||
" <td>0.999915</td>\n",
|
||
" <td>300.784546</td>\n",
|
||
" </tr>\n",
|
||
" <tr>\n",
|
||
" <th>1100</th>\n",
|
||
" <td>0.999858</td>\n",
|
||
" <td>0.012979</td>\n",
|
||
" <td>0.010749</td>\n",
|
||
" <td>0.356986</td>\n",
|
||
" <td>-0.013000</td>\n",
|
||
" <td>0.999914</td>\n",
|
||
" <td>0.001834</td>\n",
|
||
" <td>-4.867191</td>\n",
|
||
" <td>-0.010724</td>\n",
|
||
" <td>-0.001974</td>\n",
|
||
" <td>0.999941</td>\n",
|
||
" <td>301.585327</td>\n",
|
||
" </tr>\n",
|
||
" </tbody>\n",
|
||
"</table>\n",
|
||
"<p>1101 rows × 12 columns</p>\n",
|
||
"</div>"
|
||
],
|
||
"text/plain": [
|
||
" rwc(0,0) rwc(0,1) rwc(0,2) twc(0) rwc(1,0) rwc(1,1) rwc(1,2) \\\n",
|
||
"0 1.000000 -0.000096 -0.000134 0.002967 0.000096 1.000000 -0.000257 \n",
|
||
"1 0.999998 0.000914 0.001881 -0.006111 -0.000911 0.999999 -0.001395 \n",
|
||
"2 0.999995 0.001234 0.002927 -0.004074 -0.001232 0.999999 -0.000782 \n",
|
||
"3 0.999991 0.000727 0.004202 -0.006533 -0.000725 1.000000 -0.000472 \n",
|
||
"4 0.999987 0.000512 0.005128 -0.006541 -0.000510 1.000000 -0.000432 \n",
|
||
"... ... ... ... ... ... ... ... \n",
|
||
"1096 0.999676 0.018372 0.017593 0.315591 -0.018334 0.999829 -0.002316 \n",
|
||
"1097 0.999719 0.017335 0.016196 0.324796 -0.017322 0.999850 -0.000900 \n",
|
||
"1098 0.999756 0.016880 0.014275 0.339924 -0.016880 0.999858 -0.000125 \n",
|
||
"1099 0.999804 0.014992 0.012966 0.347605 -0.015007 0.999887 0.001013 \n",
|
||
"1100 0.999858 0.012979 0.010749 0.356986 -0.013000 0.999914 0.001834 \n",
|
||
"\n",
|
||
" twc(1) rwc(2,0) rwc(2,1) rwc(2,2) twc(2) \n",
|
||
"0 0.003075 0.000134 0.000257 1.000000 -0.002494 \n",
|
||
"1 -0.013490 -0.001882 0.001394 0.999997 1.208325 \n",
|
||
"2 -0.036465 -0.002928 0.000779 0.999995 2.407830 \n",
|
||
"3 -0.056512 -0.004202 0.000469 0.999991 3.606258 \n",
|
||
"4 -0.077331 -0.005129 0.000430 0.999987 4.806717 \n",
|
||
"... ... ... ... ... ... \n",
|
||
"1096 -4.811160 -0.017633 0.001993 0.999843 298.391571 \n",
|
||
"1097 -4.826771 -0.016209 0.000619 0.999868 299.177399 \n",
|
||
"1098 -4.839842 -0.014275 -0.000116 0.999898 299.979767 \n",
|
||
"1099 -4.852088 -0.012949 -0.001207 0.999915 300.784546 \n",
|
||
"1100 -4.867191 -0.010724 -0.001974 0.999941 301.585327 \n",
|
||
"\n",
|
||
"[1101 rows x 12 columns]"
|
||
]
|
||
},
|
||
"execution_count": 12,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"traj"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 13,
|
||
"id": "77d852aa",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"with open(\"/home/ivan/Downloads/Telegram Desktop/test_data.json\") as f:\n",
|
||
" traj_example = json.load(f)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 14,
|
||
"id": "2eb4a8c7",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"{'frame': 2,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'Euler angles': [179.0592276314431, 178.71021095455262, 12.371121058785771],\n",
|
||
" 'xyz coords': [1598.4821251839471, 294.28354659765137, -65.4766852385318]},\n",
|
||
" {'object_id': 2,\n",
|
||
" 'Euler angles': [179.0592276314431, 178.71021095455262, 12.371121058785771],\n",
|
||
" 'xyz coords': [1598.4821251839471, 294.28354659765137, -65.4766852385318]}]}"
|
||
]
|
||
},
|
||
"execution_count": 14,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"traj_example[\"data\"][1]"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 15,
|
||
"id": "3f29fcb8",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"json_traj_example = {\"data\": [\n",
|
||
" {\"frame\":0, \n",
|
||
" \"poses\":{\n",
|
||
" \"euler_angles\":[0.0,0.0,0.0],\n",
|
||
" \"xyz_coords\":[0.0,0.0,0.0]\n",
|
||
" }\n",
|
||
" }\n",
|
||
" ]\n",
|
||
" }\n",
|
||
"\n",
|
||
"json_traj = {\"data\": [\n",
|
||
" ]\n",
|
||
" }"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 16,
|
||
"id": "b30e79c9",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Fill the first line\n",
|
||
"Twc = traj.loc[0]\n",
|
||
"Rot = R.from_matrix([[Twc[0], Twc[1], Twc[2]], \n",
|
||
" [Twc[4], Twc[5], Twc[6]], \n",
|
||
" [Twc[8], Twc[9], Twc[10]]])\n",
|
||
"euler = Rot.as_euler('xyz', degrees=True)\n",
|
||
"# Coordinate system in ORB-SLAM3: ZXY. Coordinate system in UE4: XYZ \n",
|
||
"entry = {\"frame\":0, \"poses\":[{\"camera_id\":0, \"euler_angles\":[-euler[1], -euler[2], -euler[0]], \n",
|
||
" \"xyz_coords\":[-Twc[7], Twc[11], Twc[3]]},\n",
|
||
" {\"cube_id\":0, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-33.81, 5.15, 0.82]},\n",
|
||
" {\"cube_id\":1, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-11.5, 0.7, 0.56]},\n",
|
||
" {\"cube_id\":2, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-95.0, 5.15, 0.8]},\n",
|
||
" {\"cube_id\":3, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-130.3, 4.6, 2.1]},\n",
|
||
" {\"cube_id\":4, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-346.5, 4.1, 5.9]},\n",
|
||
" {\"cube_id\":5, \"euler_angles\":[0.0, 0.0, 0.0], \n",
|
||
" \"xyz_coords\":[-346.5, 25.7, 4.8]},\n",
|
||
" ]\n",
|
||
" }\n",
|
||
"json_traj[\"data\"].append(entry)\n",
|
||
"\n",
|
||
"\n",
|
||
"for i in range(1, len(traj.index)):\n",
|
||
" Twc = traj.loc[i]\n",
|
||
" Rot = R.from_matrix([[Twc[0], Twc[1], Twc[2]], \n",
|
||
" [Twc[4], Twc[5], Twc[6]], \n",
|
||
" [Twc[8], Twc[9], Twc[10]]])\n",
|
||
" euler = Rot.as_euler('xyz', degrees=True)\n",
|
||
" entry = {\"frame\":i, \"poses\":\n",
|
||
" [\n",
|
||
" {\"camera_id\":0, \"euler_angles\":[-euler[1], -euler[2], -euler[0]], \n",
|
||
" \"xyz_coords\":[Twc[7], Twc[11], Twc[3]]}\n",
|
||
" ]\n",
|
||
" }\n",
|
||
" json_traj[\"data\"].append(entry)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 17,
|
||
"id": "e7426f0a",
|
||
"metadata": {},
|
||
"outputs": [
|
||
{
|
||
"data": {
|
||
"text/plain": [
|
||
"[{'frame': 0,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'euler_angles': [0.007671722302035774,\n",
|
||
" -0.005487577406769043,\n",
|
||
" -0.014727885522586002],\n",
|
||
" 'xyz_coords': [-0.003075266, -0.002493523, 0.002967205]},\n",
|
||
" {'cube_id': 0,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-33.81, 5.15, 0.82]},\n",
|
||
" {'cube_id': 1,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-11.5, 0.7, 0.56]},\n",
|
||
" {'cube_id': 2,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-95.0, 5.15, 0.8]},\n",
|
||
" {'cube_id': 3,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-130.3, 4.6, 2.1]},\n",
|
||
" {'cube_id': 4,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-346.5, 4.1, 5.9]},\n",
|
||
" {'cube_id': 5,\n",
|
||
" 'euler_angles': [0.0, 0.0, 0.0],\n",
|
||
" 'xyz_coords': [-346.5, 25.7, 4.8]}]},\n",
|
||
" {'frame': 1,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'euler_angles': [-0.10783204884531329,\n",
|
||
" 0.05220453371332025,\n",
|
||
" -0.07984648626708363],\n",
|
||
" 'xyz_coords': [-0.013490011, 1.208325267, -0.006111192]}]},\n",
|
||
" {'frame': 2,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'euler_angles': [-0.16774521663570027,\n",
|
||
" 0.07056213422363951,\n",
|
||
" -0.04462317100084628],\n",
|
||
" 'xyz_coords': [-0.036465403, 2.407830477, -0.004074253]}]},\n",
|
||
" {'frame': 3,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'euler_angles': [-0.24075229907512063,\n",
|
||
" 0.04155419931766713,\n",
|
||
" -0.026881656776181453],\n",
|
||
" 'xyz_coords': [-0.056511778, 3.606258154, -0.006532952]}]},\n",
|
||
" {'frame': 4,\n",
|
||
" 'poses': [{'camera_id': 0,\n",
|
||
" 'euler_angles': [-0.29384561979999074,\n",
|
||
" 0.02919373907146133,\n",
|
||
" -0.02462359471720489],\n",
|
||
" 'xyz_coords': [-0.077330925, 4.806717396, -0.006540779]}]}]"
|
||
]
|
||
},
|
||
"execution_count": 17,
|
||
"metadata": {},
|
||
"output_type": "execute_result"
|
||
}
|
||
],
|
||
"source": [
|
||
"json_traj[\"data\"][:5]"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": 18,
|
||
"id": "ec96c736",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"with open('cameraTraj_UE4_format.json', 'w') as f:\n",
|
||
" json.dump(json_traj, f, indent=2)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "13093b0d",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": []
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "e9095c85",
|
||
"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
|
||
}
|