{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Function to convert rotation matrix to Euler angles\n",
    "def rotation_matrix_to_euler_angles(R):\n",
    "    sy = math.sqrt(R[0][0] * R[0][0] +  R[1][0] * R[1][0])\n",
    "    print(\"sy value : \", sy)\n",
    "    singular = sy < 1e-6\n",
    "    if not singular:\n",
    "        x = math.atan2(R[2][1], R[2][2])\n",
    "        y = math.atan2(R[2][0], sy)\n",
    "        z = math.atan2(R[1][0], R[0][0])\n",
    "    else:\n",
    "        x = math.atan2(R[1][2], R[1][1])\n",
    "        y = math.atan2(R[2][0], sy)\n",
    "        z = 180\n",
    "    return [math.degrees(x), math.degrees(y), math.degrees(z)]\n",
    "\n",
    "# Normalize angles to be within 0 to 360 degrees\n",
    "def normalize_angles(angles):\n",
    "    return [\n",
    "        angles[0] % 360,\n",
    "        angles[1] % 360,\n",
    "        angles[2] % 360\n",
    "    ]\n",
    "\n",
    "# Function to construct a rotation matrix from Euler angles\n",
    "def euler_angles_to_rotation_matrix(angles):\n",
    "    x, y, z = map(math.radians, angles)\n",
    "    Rx = np.array([\n",
    "        [1, 0, 0],\n",
    "        [0, math.cos(x), -math.sin(x)],\n",
    "        [0, math.sin(x), math.cos(x)]\n",
    "    ])\n",
    "    Ry = np.array([\n",
    "        [math.cos(y), 0, math.sin(y)],\n",
    "        [0, 1, 0],\n",
    "        [-math.sin(y), 0, math.cos(y)]\n",
    "    ])\n",
    "    Rz = np.array([\n",
    "        [math.cos(z), -math.sin(z), 0],\n",
    "        [math.sin(z), math.cos(z), 0],\n",
    "        [0, 0, 1]\n",
    "    ])\n",
    "    R = np.dot(Rz, np.dot(Ry, Rx))\n",
    "    return R"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Find sy value"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sy value :  8.098157056580103\n",
      "Euler Angles (degrees): [92.42400882034067, 0.8665358410071258, 90.09314479351303]\n",
      "Translation Vector: [-1109.96346502  -462.780196      91.38837728]\n"
     ]
    }
   ],
   "source": [
    "# input extrinsic matrix from nerfstudio generated path\n",
    "extrinsic_matrix = np.array([\n",
    "    [0.013165033114679656, -0.34274364470946234, -8.091817614914053, -1109.9634650183796],\n",
    "    [-8.098146355490323, 0.1218192033841708, -0.018334649660315703, -462.7801960025279],\n",
    "    [0.12248508486354542, 8.090911886719471, -0.3425060556451678, 91.3883772799453],\n",
    "    [0.0, 0.0, 0.0, 1.0]\n",
    "])\n",
    "\n",
    "# Extract rotation matrix (R) and translation vector (T)\n",
    "R_matrix = extrinsic_matrix[:3, :3]\n",
    "T_vector = extrinsic_matrix[:3, 3]\n",
    "\n",
    "# Convert rotation matrix to Euler angles with fixed sy\n",
    "euler_angles_deg = rotation_matrix_to_euler_angles(R_matrix)\n",
    "\n",
    "# Normalize the angles to be within 0 to 360 degrees\n",
    "euler_angles_deg = normalize_angles(euler_angles_deg)\n",
    "\n",
    "# Adjust the Z rotation angle to align the zero angle with the -Z direction\n",
    "euler_angles_deg[2] = (euler_angles_deg[2] + 180) % 360\n",
    "\n",
    "# Print the Euler angles and translation vector\n",
    "print(\"Euler Angles (degrees):\", euler_angles_deg)\n",
    "print(\"Translation Vector:\", T_vector)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Fix fy & Generate camera path "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "New Extrinsic Matrix:\n",
      "[[ 4.95865612e-16  5.72622142e+00  5.72622142e+00 -1.10996347e+03]\n",
      " [ 8.09810000e+00 -3.50629937e-16 -3.50629937e-16 -4.62780196e+02]\n",
      " [ 0.00000000e+00  5.72622142e+00 -5.72622142e+00  9.13883773e+01]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "# input fixed sy value\n",
    "sy_fixed = 8.0981\n",
    "\n",
    "# Construct the rotation matrix from the normalized Euler angles\n",
    "euler_angles_deg = [135.0, 0.0, 90.0]\n",
    "R_new = euler_angles_to_rotation_matrix(euler_angles_deg)\n",
    "\n",
    "\n",
    "# Scale the rotation matrix to achieve the desired sy value\n",
    "scaling_factor = sy_fixed / math.sqrt(R_new[0, 0] * R_new[0, 0] + R_new[1, 0] * R_new[1, 0])\n",
    "R_new_scaled = R_new * scaling_factor\n",
    "\n",
    "# Construct the new transformation matrix\n",
    "new_extrinsic_matrix = np.eye(4)\n",
    "new_extrinsic_matrix[:3, :3] = R_new_scaled\n",
    "new_extrinsic_matrix[:3, 3] = T_vector\n",
    "\n",
    "print(\"New Extrinsic Matrix:\")\n",
    "print(new_extrinsic_matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 4.95865612e-16,  5.72622142e+00,  5.72622142e+00],\n",
       "       [ 8.09810000e+00, -3.50629937e-16, -3.50629937e-16],\n",
       "       [ 0.00000000e+00,  5.72622142e+00, -5.72622142e+00]])"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "new_extrinsic_matrix[:3,:3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "nerfstudio2",
   "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.19"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}