{
 "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
}