{ "cells": [ { "cell_type": "code", "execution_count": 95, "metadata": {}, "outputs": [], "source": [ "from nerfstudio.cameras import camera_utils\n", "\n", "import torch\n", "import numpy as np\n", "import json\n", "import copy\n", "import math" ] }, { "cell_type": "code", "execution_count": 121, "metadata": {}, "outputs": [], "source": [ "transform_matrix = './site1_large/transform_matrix.json'\n", "with open(transform_matrix, 'r') as file:\n", " t_matrix = json.load(file)\n", "\n", "\n", "# bim_camera = './site1_large/input_bim_camera.json'\n", "# with open(bim_camera, 'r') as file:\n", "# bim_path = json.load(file)\n", "\n", "# bim_path_raw = copy.deepcopy(bim_path)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# BIM to Camera" ] }, { "cell_type": "code", "execution_count": 291, "metadata": {}, "outputs": [], "source": [ "camera_world_transforms = []\n", "\n", "# for frame in bim_path['frames']:\n", "# bim_transform_4x4 = frame['transform_matrix']\n", "\n", "# matrix_1 = np.array(bim_transform_4x4)\n", "# matrix_2 = np.array(t_matrix['b2c_transform'])\n", "\n", "# camera_world_transform_4x4 = np.dot(matrix_2, matrix_1)\n", "# camera_world_transforms.append(list(list(row) for row in camera_world_transform_4x4))\n", "\n", "for matrix in bim_path_list:\n", " bim_transform_4x4 = matrix\n", "\n", " matrix_1 = np.array(bim_transform_4x4)\n", " matrix_2 = np.array(t_matrix['b2c_transform'])\n", "\n", " # Transform the extrinsic matrix\n", " camera_world_transform_4x4 = np.dot(matrix_2, matrix_1)\n", "\n", " R = camera_world_transform_4x4[:3, :3]\n", " t = camera_world_transform_4x4[:3, 3]\n", "\n", " # Construct the 4x4 transformation matrix\n", " transform_matrix = np.eye(4)\n", " transform_matrix[:3, :3] = R.T\n", "\n", " transform_matrix[0, 0] *= -1\n", " transform_matrix[1, 0] *= -1\n", " transform_matrix[2, 1] *= -1\n", " transform_matrix[2, 2] *= -1\n", "\n", " transform_matrix[0, 3] = float(-t[0])\n", " transform_matrix[1, 3] = float(-t[1])\n", " transform_matrix[2, 3] = float(t[2])\n", "\n", " camera_world_transforms.append(list(list(row) for row in transform_matrix))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Camera to Norm" ] }, { "cell_type": "code", "execution_count": 292, "metadata": {}, "outputs": [], "source": [ "normed_camera_world_transforms = []\n", "\n", "for transform in camera_world_transforms:\n", "\n", " matrix_1 = np.array(transform)\n", " matrix_2 = np.array(t_matrix['c2n_transform'])\n", "\n", " normed_camera_world_transform_4x4 = np.dot(matrix_2, matrix_1)\n", " normed_camera_world_transform_4x4[:3, 3] *= t_matrix['norm_scale']\n", "\n", " normed_camera_world_transforms.append(list(list(row) for row in normed_camera_world_transform_4x4))" ] }, { "cell_type": "code", "execution_count": 293, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "[[[-0.018663635203528976,\n", " 0.01978361252148456,\n", " 0.120437957889028,\n", " 0.9215669200457903],\n", " [-0.013450979278442571,\n", " -0.1214291915929607,\n", " 0.01786201445354012,\n", " 0.2688063339987272],\n", " [0.12130856765245047,\n", " -0.010420604923397365,\n", " 0.020510275709091662,\n", " -0.0383849907185625],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.018663635203528976,\n", " 0.01978361252148456,\n", " 0.120437957889028,\n", " 0.9197317157416658],\n", " [-0.013450979278442571,\n", " -0.1214291915929607,\n", " 0.01786201445354012,\n", " 0.27242520666418885],\n", " [0.12130856765245047,\n", " -0.010420604923397365,\n", " 0.020510275709091662,\n", " 0.16926187493976144],\n", " [0.0, 0.0, 0.0, 1.0]]]" ] }, "execution_count": 293, "metadata": {}, "output_type": "execute_result" } ], "source": [ "normed_camera_world_transforms" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Camera Pose for NeRF Render" ] }, { "cell_type": "code", "execution_count": 294, "metadata": {}, "outputs": [], "source": [ "nerf_template = './nerf_template.json'\n", "with open(nerf_template, 'r') as file:\n", " tmp = json.load(file)\n", "\n", "# Write the updated dictionary back to the JSON file\n", "with open(nerf_template, 'w') as f:\n", " json.dump(tmp, f, indent=4)" ] }, { "cell_type": "code", "execution_count": 295, "metadata": {}, "outputs": [], "source": [ "nerf_template = './nerf_template.json'\n", "with open(nerf_template, 'r') as file:\n", " tmp = json.load(file)" ] }, { "cell_type": "code", "execution_count": 296, "metadata": {}, "outputs": [], "source": [ "stretched_matrix = np.array(normed_camera_world_transforms).reshape(-1,16)" ] }, { "cell_type": "code", "execution_count": 297, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[-0.01866364, 0.01978361, 0.12043796, 0.92156692, -0.01345098,\n", " -0.12142919, 0.01786201, 0.26880633, 0.12130857, -0.0104206 ,\n", " 0.02051028, -0.03838499, 0. , 0. , 0. ,\n", " 1. ],\n", " [-0.01866364, 0.01978361, 0.12043796, 0.91973172, -0.01345098,\n", " -0.12142919, 0.01786201, 0.27242521, 0.12130857, -0.0104206 ,\n", " 0.02051028, 0.16926187, 0. , 0. , 0. ,\n", " 1. ]])" ] }, "execution_count": 297, "metadata": {}, "output_type": "execute_result" } ], "source": [ "stretched_matrix" ] }, { "cell_type": "code", "execution_count": 298, "metadata": {}, "outputs": [], "source": [ "fps = 1.0\n", "seconds = math.ceil(len(stretched_matrix) / fps)\n", "\n", "\n", "keyframes = []\n", "for matrix in stretched_matrix:\n", " temp_dic = {}\n", " temp_dic[\"matrix\"] = list(matrix)\n", " temp_dic[\"fov\"] = 75.0\n", " temp_dic[\"aspect\"] = 0.5625\n", " temp_dic[\"override_transition_enabled\"] = False\n", " temp_dic[\"override_transition_sec\"] = None\n", "\n", " keyframes.append(temp_dic)\n", "\n", "camera_path = []\n", "for matrix in stretched_matrix:\n", " temp_dic = {}\n", " temp_dic[\"camera_to_world\"] = list(matrix)\n", " temp_dic[\"fov\"] = 75.0\n", " temp_dic[\"aspect\"] = 0.5625\n", "\n", " camera_path.append(temp_dic)\n", "\n", "tmp[\"fps\"] = fps\n", "tmp[\"seconds\"] = seconds\n", "tmp[\"keyframes\"] = keyframes\n", "tmp[\"camera_path\"] = camera_path\n", "\n", "tmp[\"render_height\"] = 1920\n", "tmp[\"render_width\"] = 1080" ] }, { "cell_type": "code", "execution_count": 299, "metadata": {}, "outputs": [], "source": [ "# Write the updated dictionary back to the JSON file\n", "nerf_template = './site1_large/nerf_camera_path.json'\n", "with open(nerf_template, 'w') as f:\n", " json.dump(tmp, f, indent=4)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Reverse" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### NeRF Render(Normed) to Camera-World " ] }, { "cell_type": "code", "execution_count": 243, "metadata": {}, "outputs": [], "source": [ "# Load transformation matrices\n", "transform_matrix = './site1_large/transform_matrix.json'\n", "with open(transform_matrix, 'r') as file:\n", " t_matrix = json.load(file)\n", "\n", "# Load NeRF Render\n", "normed_camera_world_transforms = '/home/worker/yt/autocon/dataset/site1_large/camera_paths/final_col.json'\n", "with open(normed_camera_world_transforms, 'r') as file:\n", " normed_path = json.load(file)\n", "\n", "normed_path_raw = copy.deepcopy(normed_path)" ] }, { "cell_type": "code", "execution_count": 244, "metadata": {}, "outputs": [], "source": [ "# Norm to Camera\n", "camera_world_transforms = []\n", "\n", "for frame in normed_path['camera_path']:\n", " normed_transform_4x4 = np.array(frame['camera_to_world']).reshape(4,4)\n", " normed_transform_4x4[:3, 3] /= t_matrix['norm_scale']\n", "\n", " # normed_transform_4x4 = np.vstack([normed_transform_3x4, [0., 0., 0., 1.0]])\n", "\n", " matrix_1 = np.linalg.inv(np.array(t_matrix['c2n_transform']))\n", " matrix_2 = normed_transform_4x4\n", "\n", " camera_world_transform_4x4 = np.dot(matrix_1, matrix_2)\n", " camera_world_transforms.append(list(list(row) for row in camera_world_transform_4x4))" ] }, { "cell_type": "code", "execution_count": 245, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "[[[-0.14323084465315403,\n", " 0.15568792529783063,\n", " -0.9773669947000363,\n", " 109.37991865094716],\n", " [-0.9896683659049774,\n", " -0.02895193798366831,\n", " 0.14042173468922403,\n", " 31.982859718923518],\n", " [-0.006434699593758372,\n", " 0.9873819201760229,\n", " 0.15822622530979755,\n", " -4.89083185328284],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432308387710771,\n", " 0.15568792540785717,\n", " -0.9773669955445147,\n", " 109.39048695915534],\n", " [-0.9896683667551337,\n", " -0.028951937223428592,\n", " 0.1404217288542147,\n", " 31.978886636074524],\n", " [-0.006434699768164712,\n", " 0.9873819201809662,\n", " 0.1582262252718588,\n", " -4.515034715481193],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323083288899963,\n", " 0.15568792551788357,\n", " -0.9773669963889926,\n", " 109.40105526736352],\n", " [-0.9896683676052895,\n", " -0.028951936463188197,\n", " 0.14042172301920536,\n", " 31.97491355322553],\n", " [-0.006434699942570719,\n", " 0.987381920185909,\n", " 0.15822622523392024,\n", " -4.139237577679544],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323082700692255,\n", " 0.1556879256279106,\n", " -0.9773669972334705,\n", " 109.41162357557171],\n", " [-0.9896683684554454,\n", " -0.028951935702948243,\n", " 0.14042171718419622,\n", " 31.970940470376537],\n", " [-0.006434700116976941,\n", " 0.9873819201908517,\n", " 0.15822622519598226,\n", " -3.7634404398778973],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323082112484584,\n", " 0.15568792573793697,\n", " -0.9773669980779489,\n", " 109.42219188377989],\n", " [-0.9896683693056016,\n", " -0.028951934942708635,\n", " 0.14042171134918702,\n", " 31.966967387527543],\n", " [-0.0064347002913832775,\n", " 0.9873819201957951,\n", " 0.1582262251580433,\n", " -3.387643302076249],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432308152427685,\n", " 0.1556879258479638,\n", " -0.9773669989224262,\n", " 109.43276019198808],\n", " [-0.9896683701557569,\n", " -0.028951934182468234,\n", " 0.14042170551417799,\n", " 31.96299430467855],\n", " [-0.006434700465789271,\n", " 0.9873819202007376,\n", " 0.1582262251201055,\n", " -3.0118461642746013],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323080936069132,\n", " 0.15568792595798986,\n", " -0.9773669997669047,\n", " 109.44332850019627],\n", " [-0.9896683710059131,\n", " -0.0289519334222284,\n", " 0.14042169967916845,\n", " 31.959021221829556],\n", " [-0.006434700640195559,\n", " 0.9873819202056807,\n", " 0.15822622508216638,\n", " -2.636049026472954],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323080347861403,\n", " 0.1556879260680165,\n", " -0.9773670006113824,\n", " 109.45389680840445],\n", " [-0.9896683718560688,\n", " -0.028951932661988453,\n", " 0.14042169384415903,\n", " 31.955048138980562],\n", " [-0.006434700814601777,\n", " 0.9873819202106237,\n", " 0.15822622504422812,\n", " -2.2602518886713057],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323079759653728,\n", " 0.15568792617804297,\n", " -0.9773670014558603,\n", " 109.46446511661262],\n", " [-0.9896683727062245,\n", " -0.028951931901748173,\n", " 0.14042168800915028,\n", " 31.951075056131568],\n", " [-0.006434700989007662,\n", " 0.9873819202155668,\n", " 0.15822622500628944,\n", " -1.8844547508696585],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323079171445996,\n", " 0.15568792628806988,\n", " -0.977367002300338,\n", " 109.47503342482081],\n", " [-0.9896683735563803,\n", " -0.028951931141508444,\n", " 0.14042168217414064,\n", " 31.947101973282575],\n", " [-0.006434701163414162,\n", " 0.9873819202205096,\n", " 0.15822622496835145,\n", " -1.508657613068011],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323078583238302,\n", " 0.1556879263980963,\n", " -0.9773670031448162,\n", " 109.485601733029],\n", " [-0.9896683744065363,\n", " -0.02895193038126861,\n", " 0.14042167633913133,\n", " 31.94312889043358],\n", " [-0.006434701337820165,\n", " 0.9873819202254528,\n", " 0.15822622493041255,\n", " -1.1328604752663631],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432307799503055,\n", " 0.1556879265081227,\n", " -0.9773670039892938,\n", " 109.49617004123718],\n", " [-0.9896683752566919,\n", " -0.02895192962102811,\n", " 0.14042167050412194,\n", " 31.939155807584594],\n", " [-0.006434701512225999,\n", " 0.9873819202303957,\n", " 0.15822622489247393,\n", " -0.7570633374647153],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432307740682284,\n", " 0.15568792661814904,\n", " -0.9773670048337715,\n", " 109.50673834944537],\n", " [-0.9896683761068474,\n", " -0.02895192886078805,\n", " 0.14042166466911277,\n", " 31.9351827247356],\n", " [-0.006434701686632162,\n", " 0.9873819202353388,\n", " 0.15822622485453533,\n", " -0.38126619966306724],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432307681861514,\n", " 0.15568792672817564,\n", " -0.9773670056782493,\n", " 109.51730665765356],\n", " [-0.989668376957003,\n", " -0.02895192810054821,\n", " 0.14042165883410354,\n", " 31.931209641886607],\n", " [-0.0064347018610384325,\n", " 0.9873819202402817,\n", " 0.15822622481659687,\n", " -0.0054690618614197245],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323076230407403,\n", " 0.15568792683820193,\n", " -0.9773670065227268,\n", " 109.52787496586174],\n", " [-0.9896683778071586,\n", " -0.028951927340308038,\n", " 0.1404216529990941,\n", " 31.92723655903761],\n", " [-0.006434702035444596,\n", " 0.9873819202452245,\n", " 0.15822622477865828,\n", " 0.3703280759402277],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323075642199695,\n", " 0.15568792694822878,\n", " -0.9773670073672047,\n", " 109.53844327406992],\n", " [-0.9896683786573143,\n", " -0.028951926580068427,\n", " 0.1404216471640847,\n", " 31.92326347618862],\n", " [-0.006434702209850818,\n", " 0.9873819202501678,\n", " 0.15822622474071987,\n", " 0.7461252137418755],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323075053991963,\n", " 0.15568792705825493,\n", " -0.9773670082116821,\n", " 109.54901158227811],\n", " [-0.9896683795074697,\n", " -0.028951925819828143,\n", " 0.14042164132907542,\n", " 31.91929039333963],\n", " [-0.006434702384257033,\n", " 0.9873819202551106,\n", " 0.1582262247027811,\n", " 1.1219223515435233],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323074465784247,\n", " 0.15568792716828178,\n", " -0.9773670090561597,\n", " 109.55957989048629],\n", " [-0.9896683803576252,\n", " -0.028951925059588306,\n", " 0.14042163549406614,\n", " 31.915317310490632],\n", " [-0.006434702558663304,\n", " 0.9873819202600537,\n", " 0.1582262246648431,\n", " 1.4977194893451706],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432307387757658,\n", " 0.1556879272783082,\n", " -0.9773670099006372,\n", " 109.57014819869445],\n", " [-0.9896683812077804,\n", " -0.02895192429934858,\n", " 0.1404216296590572,\n", " 31.911344227641635],\n", " [-0.006434702733069514,\n", " 0.9873819202649966,\n", " 0.1582262246269044,\n", " 1.8735166271468184],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432307328936884,\n", " 0.15568792738833465,\n", " -0.9773670107451147,\n", " 109.58071650690266],\n", " [-0.989668382057936,\n", " -0.028951923539108297,\n", " 0.1404216238240478,\n", " 31.907371144792645],\n", " [-0.006434702907475563,\n", " 0.9873819202699394,\n", " 0.15822622458896593,\n", " 2.2493137649484667],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323072701161133,\n", " 0.155687927498361,\n", " -0.9773670115895923,\n", " 109.59128481511085],\n", " [-0.9896683829080913,\n", " -0.028951922778868464,\n", " 0.1404216179890384,\n", " 31.903398061943648],\n", " [-0.006434703081881834,\n", " 0.9873819202748826,\n", " 0.15822622455102717,\n", " 2.625110902750114],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323072112953392,\n", " 0.15568792760838773,\n", " -0.9773670124340695,\n", " 109.601853123319],\n", " [-0.9896683837582465,\n", " -0.02895192201862818,\n", " 0.14042161215402912,\n", " 31.899424979094658],\n", " [-0.006434703256287827,\n", " 0.9873819202798255,\n", " 0.1582262245130891,\n", " 3.0009080405517614],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323071524745706,\n", " 0.15568792771841442,\n", " -0.9773670132785468,\n", " 109.6124214315272],\n", " [-0.9896683846084018,\n", " -0.028951921258388677,\n", " 0.1404216063190199,\n", " 31.895451896245667],\n", " [-0.006434703430694368,\n", " 0.9873819202847683,\n", " 0.15822622447515083,\n", " 3.376705178353409],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323070936537954,\n", " 0.15568792782844076,\n", " -0.9773670141230241,\n", " 109.62298973973542],\n", " [-0.9896683854585571,\n", " -0.02895192049814839,\n", " 0.14042160048401045,\n", " 31.89147881339667],\n", " [-0.006434703605100361,\n", " 0.9873819202897113,\n", " 0.1582262244372122,\n", " 3.752502316155059],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323070348330266,\n", " 0.1556879279384673,\n", " -0.9773670149675017,\n", " 109.63355804794358],\n", " [-0.9896683863087123,\n", " -0.02895191973790856,\n", " 0.1404215946490013,\n", " 31.887505730547673],\n", " [-0.006434703779506462,\n", " 0.9873819202946547,\n", " 0.15822622439927345,\n", " 4.128299453956705],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323069760122514,\n", " 0.15568792804849346,\n", " -0.977367015811979,\n", " 109.64412635615176],\n", " [-0.9896683871588675,\n", " -0.02895191897766828,\n", " 0.1404215888139916,\n", " 31.883532647698676],\n", " [-0.006434703953912452,\n", " 0.9873819202995975,\n", " 0.15822622436133463,\n", " 4.504096591758353],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323069171914787,\n", " 0.1556879281585201,\n", " -0.9773670166564563,\n", " 109.65469466435995],\n", " [-0.9896683880090229,\n", " -0.02895191821742867,\n", " 0.14042158297898202,\n", " 31.879559564849693],\n", " [-0.006434704128318833,\n", " 0.9873819203045406,\n", " 0.15822622432339623,\n", " 4.87989372956],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323068583707088,\n", " 0.15568792826854666,\n", " -0.9773670175009336,\n", " 109.66526297256813],\n", " [-0.989668388859178,\n", " -0.028951917457188718,\n", " 0.14042157714397294,\n", " 31.875586482000696],\n", " [-0.006434704302725037,\n", " 0.9873819203094836,\n", " 0.1582262242854579,\n", " 5.255690867361648],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432306799549938,\n", " 0.15568792837857298,\n", " -0.9773670183454105,\n", " 109.67583128077632],\n", " [-0.9896683897093328,\n", " -0.028951916696948545,\n", " 0.14042157130896377,\n", " 31.8716133991517],\n", " [-0.006434704477131245,\n", " 0.9873819203144265,\n", " 0.15822622424751945,\n", " 5.631488005163296],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323067407291618,\n", " 0.15568792848859922,\n", " -0.9773670191898879,\n", " 109.6863995889845],\n", " [-0.9896683905594882,\n", " -0.028951915936708712,\n", " 0.14042156547395399,\n", " 31.867640316302708],\n", " [-0.006434704651537516,\n", " 0.9873819203193696,\n", " 0.15822622420958052,\n", " 6.0072851429649425],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323066819083943,\n", " 0.1556879285986259,\n", " -0.977367020034365,\n", " 109.69696789719268],\n", " [-0.9896683914096432,\n", " -0.0289519151764691,\n", " 0.14042155963894484,\n", " 31.86366723345371],\n", " [-0.006434704825943832,\n", " 0.9873819203243127,\n", " 0.1582262241716421,\n", " 6.383082280766591],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323066230876177,\n", " 0.15568792870865208,\n", " -0.9773670208788421,\n", " 109.70753620540087],\n", " [-0.9896683922597981,\n", " -0.028951914416228595,\n", " 0.1404215538039354,\n", " 31.85969415060472],\n", " [-0.0064347050003496515,\n", " 0.9873819203292556,\n", " 0.1582262241337035,\n", " 6.758879418568241],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432306564266848,\n", " 0.15568792881867877,\n", " -0.977367021723319,\n", " 109.71810451360905],\n", " [-0.9896683931099529,\n", " -0.028951913655988645,\n", " 0.14042154796892622,\n", " 31.85572106775573],\n", " [-0.006434705174755801,\n", " 0.9873819203341986,\n", " 0.15822622409576514,\n", " 7.134676556369885],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323065054460699,\n", " 0.15568792892870495,\n", " -0.9773670225677958,\n", " 109.72867282181724],\n", " [-0.9896683939601076,\n", " -0.02895191289574836,\n", " 0.14042154213391655,\n", " 31.85174798490674],\n", " [-0.0064347053491620645,\n", " 0.9873819203391414,\n", " 0.15822622405782666,\n", " 7.5104736941715355],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323064466253033,\n", " 0.15568792903873155,\n", " -0.9773670234122733,\n", " 109.73924113002543],\n", " [-0.9896683948102629,\n", " -0.02895191213550886,\n", " 0.14042153629890722,\n", " 31.847774902057743],\n", " [-0.006434705523568162,\n", " 0.9873819203440846,\n", " 0.15822622401988784,\n", " 7.886270831973182],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.143230638780452,\n", " 0.1556879291487578,\n", " -0.9773670242567498,\n", " 109.74980943823361],\n", " [-0.9896683956604173,\n", " -0.02895191137526813,\n", " 0.14042153046389738,\n", " 31.843801819208746],\n", " [-0.0064347056979743664,\n", " 0.9873819203490273,\n", " 0.1582262239819497,\n", " 8.262067969774831],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323063289837595,\n", " 0.1556879292587843,\n", " -0.977367025101227,\n", " 109.76037774644178],\n", " [-0.9896683965105724,\n", " -0.028951910615028744,\n", " 0.14042152462888866,\n", " 31.839828736359756],\n", " [-0.006434705872380563,\n", " 0.9873819203539707,\n", " 0.1582262239440109,\n", " 8.637865107576479],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323062701629843,\n", " 0.15568792936881082,\n", " -0.9773670259457038,\n", " 109.77094605464998],\n", " [-0.9896683973607268,\n", " -0.02895190985478879,\n", " 0.14042151879387932,\n", " 31.835855653510766],\n", " [-0.006434706046786987,\n", " 0.9873819203589133,\n", " 0.15822622390607274,\n", " 9.013662245378127],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.143230621134221,\n", " 0.1556879294788371,\n", " -0.9773670267901806,\n", " 109.78151436285815],\n", " [-0.9896683982108817,\n", " -0.028951909094548735,\n", " 0.1404215129588697,\n", " 31.83188257066177],\n", " [-0.006434706221193028,\n", " 0.9873819203638564,\n", " 0.15822622386813404,\n", " 9.389459383179773],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323061525214362,\n", " 0.15568792958886357,\n", " -0.9773670276346572,\n", " 109.79208267106635],\n", " [-0.9896683990610363,\n", " -0.02895190833430834,\n", " 0.14042150712386037,\n", " 31.82790948781277],\n", " [-0.006434706395599008,\n", " 0.9873819203687992,\n", " 0.15822622383019566,\n", " 9.76525652098142],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323060937006618,\n", " 0.15568792969888978,\n", " -0.9773670284791341,\n", " 109.80265097927455],\n", " [-0.989668399911191,\n", " -0.028951907574068392,\n", " 0.14042150128885064,\n", " 31.82393640496378],\n", " [-0.00643470657000532,\n", " 0.9873819203737423,\n", " 0.15822622379225695,\n", " 10.141053658783068],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323060348798888,\n", " 0.1556879298089162,\n", " -0.977367029323611,\n", " 109.8132192874827],\n", " [-0.9896684007613455,\n", " -0.02895190681382833,\n", " 0.14042149545384108,\n", " 31.819963322114784],\n", " [-0.006434706744411299,\n", " 0.9873819203786851,\n", " 0.15822622375431836,\n", " 10.516850796584718],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323059760591192,\n", " 0.15568792991894248,\n", " -0.9773670301680878,\n", " 109.82378759569087],\n", " [-0.9896684016115005,\n", " -0.028951906053588612,\n", " 0.14042148961883189,\n", " 31.815990239265794],\n", " [-0.006434706918817504,\n", " 0.9873819203836285,\n", " 0.15822622371637948,\n", " 10.892647934386362],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323059172383418,\n", " 0.15568793002896908,\n", " -0.977367031012564,\n", " 109.83435590389907],\n", " [-0.9896684024616547,\n", " -0.028951905293348103,\n", " 0.14042148378382246,\n", " 31.81201715641681],\n", " [-0.006434707093223591,\n", " 0.9873819203885712,\n", " 0.1582262236784415,\n", " 11.268445072188014],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323058584175744,\n", " 0.15568793013899548,\n", " -0.9773670318570411,\n", " 109.84492421210726],\n", " [-0.9896684033118096,\n", " -0.028951904533108606,\n", " 0.1404214779488131,\n", " 31.808044073567814],\n", " [-0.006434707267629681,\n", " 0.9873819203935146,\n", " 0.15822622364050248,\n", " 11.644242209989661],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323057995968003,\n", " 0.1556879302490218,\n", " -0.9773670327015175,\n", " 109.85549252031544],\n", " [-0.9896684041619639,\n", " -0.028951903772868545,\n", " 0.14042147211380368,\n", " 31.804070990718817],\n", " [-0.006434707442036152,\n", " 0.9873819203984573,\n", " 0.15822622360256428,\n", " 12.020039347791307],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432305740776026,\n", " 0.1556879303590483,\n", " -0.9773670335459939,\n", " 109.86606082852362],\n", " [-0.9896684050121183,\n", " -0.02895190301262837,\n", " 0.14042146627879418,\n", " 31.800097907869826],\n", " [-0.006434707616442128,\n", " 0.9873819204034003,\n", " 0.1582262235646259,\n", " 12.395836485592955],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432305681955253,\n", " 0.15568793046907467,\n", " -0.9773670343904707,\n", " 109.87662913673181],\n", " [-0.9896684058622727,\n", " -0.028951902252388535,\n", " 0.14042146044378462,\n", " 31.79612482502083],\n", " [-0.006434707790848492,\n", " 0.9873819204083433,\n", " 0.1582262235266873,\n", " 12.771633623394601],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323056231344747,\n", " 0.15568793057910063,\n", " -0.9773670352349471,\n", " 109.88719744493999],\n", " [-0.9896684067124271,\n", " -0.02895190149214825,\n", " 0.14042145460877475,\n", " 31.79215174217184],\n", " [-0.0064347079652545824,\n", " 0.9873819204132862,\n", " 0.1582262234887484,\n", " 13.14743076119625],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432305564313708,\n", " 0.15568793068912729,\n", " -0.9773670360794235,\n", " 109.89776575314816],\n", " [-0.9896684075625815,\n", " -0.028951900731908863,\n", " 0.14042144877376578,\n", " 31.788178659322842],\n", " [-0.006434708139660995,\n", " 0.9873819204182293,\n", " 0.15822622345081017,\n", " 13.523227898997899],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323055054929307,\n", " 0.15568793079915352,\n", " -0.9773670369238998,\n", " 109.90833406135637],\n", " [-0.9896684084127356,\n", " -0.02895189997166836,\n", " 0.14042144293875614,\n", " 31.78420557647386],\n", " [-0.006434708314067027,\n", " 0.9873819204231722,\n", " 0.15822622341287162,\n", " 13.899025036799545],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323054466721621,\n", " 0.1556879309091798,\n", " -0.9773670377683764,\n", " 109.91890236956455],\n", " [-0.9896684092628899,\n", " -0.028951899211428968,\n", " 0.14042143710374674,\n", " 31.78023249362486],\n", " [-0.006434708488473495,\n", " 0.9873819204281153,\n", " 0.15822622337493286,\n", " 14.274822174601193],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323053878513858,\n", " 0.15568793101920647,\n", " -0.9773670386128525,\n", " 109.92947067777271],\n", " [-0.9896684101130443,\n", " -0.02895189845118835,\n", " 0.14042143126873732,\n", " 31.776259410775864],\n", " [-0.006434708662879249,\n", " 0.9873819204330582,\n", " 0.15822622333699465,\n", " 14.650619312402844],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323053290306137,\n", " 0.15568793112923263,\n", " -0.9773670394573291,\n", " 109.94003898598089],\n", " [-0.9896684109631986,\n", " -0.02895189769094885,\n", " 0.14042142543372774,\n", " 31.772286327926874],\n", " [-0.00643470883728561,\n", " 0.9873819204380013,\n", " 0.15822622329905578,\n", " 15.02641645020449],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432305270209843,\n", " 0.1556879312392592,\n", " -0.9773670403018053,\n", " 109.9506072941891],\n", " [-0.9896684118133527,\n", " -0.028951896930708567,\n", " 0.1404214195987185,\n", " 31.768313245077884],\n", " [-0.0064347090116914675,\n", " 0.9873819204429443,\n", " 0.1582262232611173,\n", " 15.40221358800614],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432305211389068,\n", " 0.15568793134928555,\n", " -0.9773670411462816,\n", " 109.96117560239729],\n", " [-0.9896684126635068,\n", " -0.028951896170468727,\n", " 0.14042141376370895,\n", " 31.764340162228887],\n", " [-0.006434709186097991,\n", " 0.9873819204478872,\n", " 0.15822622322317909,\n", " 15.778010725807786],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323051525682903,\n", " 0.15568793145931153,\n", " -0.9773670419907577,\n", " 109.97174391060545],\n", " [-0.9896684135136607,\n", " -0.028951895410228225,\n", " 0.14042140792869923,\n", " 31.76036707937989],\n", " [-0.006434709360503796,\n", " 0.9873819204528301,\n", " 0.15822622318524024,\n", " 16.15380786360943],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323050937475196,\n", " 0.1556879315693376,\n", " -0.9773670428352338,\n", " 109.98231221881363],\n", " [-0.9896684143638148,\n", " -0.028951894649988614,\n", " 0.14042140209368995,\n", " 31.7563939965309],\n", " [-0.006434709534910261,\n", " 0.9873819204577732,\n", " 0.15822622314730142,\n", " 16.52960500141108],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.14323050349267422,\n", " 0.1556879316793642,\n", " -0.9773670436797101,\n", " 109.99288052702184],\n", " [-0.9896684152139689,\n", " -0.028951893889748108,\n", " 0.1404213962586801,\n", " 31.752420913681902],\n", " [-0.006434709709316067,\n", " 0.9873819204627161,\n", " 0.1582262231093631,\n", " 16.90540213921273],\n", " [0.0, 0.0, 0.0, 1.0]],\n", " [[-0.1432304976105968,\n", " 0.15568793178939014,\n", " -0.9773670445241862,\n", " 110.00344883523002],\n", " [-0.989668416064123,\n", " -0.028951893129508164,\n", " 0.14042139042367066,\n", " 31.748447830832912],\n", " [-0.006434709883722317,\n", " 0.9873819204676593,\n", " 0.15822622307142417,\n", " 17.281199277014377],\n", " [0.0, 0.0, 0.0, 1.0]]]" ] }, "execution_count": 245, "metadata": {}, "output_type": "execute_result" } ], "source": [ "camera_world_transforms" ] }, { "cell_type": "code", "execution_count": 246, "metadata": {}, "outputs": [], "source": [ "# camera_world_transforms = []\n", "\n", "# train_transform = './site1_large/transforms.json'\n", "# with open(train_transform, 'r') as file:\n", "# nerf_path = json.load(file)\n", "\n", "# for frame in nerf_path['frames']:\n", "# nerf_transform_4x4 = frame['transform_matrix']\n", "\n", "# camera_world_transforms.append(list(list(row) for row in nerf_transform_4x4))" ] }, { "cell_type": "code", "execution_count": 247, "metadata": {}, "outputs": [], "source": [ "\n", "# Camera to BIM\n", "bim_transforms = []\n", "\n", "for transform in camera_world_transforms:\n", " camera_world_transform_4x4 = np.array(transform)\n", "\n", " # Extract rotation and translation components\n", " reversed_transform_matrix = np.eye(4)\n", " t = camera_world_transform_4x4[:3, 3]\n", "\n", " reversed_transform_matrix[0, 3] = float(-t[0])\n", " reversed_transform_matrix[1, 3] = float(-t[1])\n", " reversed_transform_matrix[2, 3] = float(t[2])\n", "\n", " camera_world_transform_4x4[0, 0] *= -1\n", " camera_world_transform_4x4[1, 0] *= -1\n", " camera_world_transform_4x4[2, 1] *= -1\n", " camera_world_transform_4x4[2, 2] *= -1\n", "\n", " reversed_transform_matrix[:3, :3] = camera_world_transform_4x4[:3, :3].T\n", "\n", " # Compute the inverse of b2c_transform\n", " b2c_transform_inverse = np.linalg.inv(np.array(t_matrix['b2c_transform']))\n", "\n", " # Apply the inverse of b2c_transform\n", " bim_transform_4x4_reversed = np.dot(b2c_transform_inverse, reversed_transform_matrix)\n", "\n", " # Append the reversed transformation for verification\n", " bim_transforms.append(list(list(row) for row in bim_transform_4x4_reversed))\n", "\n" ] }, { "cell_type": "code", "execution_count": 248, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[ 8.72607892e-01, 7.89896396e+00, 1.56207457e+00,\n", " -1.08595013e+03],\n", " [ 1.48234497e+00, 1.38677126e+00, -7.84057856e+00,\n", " -4.51663558e+02],\n", " [-7.91431469e+00, 1.13065707e+00, -1.29630578e+00,\n", " 1.20258615e+02],\n", " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", " 1.00000000e+00]])" ] }, "execution_count": 248, "metadata": {}, "output_type": "execute_result" } ], "source": [ "np.array(bim_transforms)[0]" ] }, { "cell_type": "code", "execution_count": 249, "metadata": {}, "outputs": [], "source": [ "# # File path\n", "# file_path = \"output.txt\"\n", "\n", "# # Open the file in write mode\n", "# with open(file_path, 'w') as file:\n", "# # Write each item in the list to the file\n", "# for item in bim_transforms:\n", "# file.write(f\"{item}\\n\")\n", "\n", "# print(f\"List has been saved to {file_path}\")" ] }, { "cell_type": "code", "execution_count": 300, "metadata": {}, "outputs": [], "source": [ "# Define the filename\n", "bim_path_temp_name = 'bim_path_template.json'\n", "\n", "# Write the updated dictionary back to the JSON file\n", "with open(bim_path_temp_name, 'r') as f:\n", " bim_path_temp = json.load(f)" ] }, { "cell_type": "code", "execution_count": 301, "metadata": {}, "outputs": [], "source": [ "new_frames = []\n", "\n", "for matrix in bim_transforms:\n", " temp_dic = {}\n", " \n", " temp_dic[\"transform_matrix\"] = matrix\n", "\n", " new_frames.append(temp_dic)" ] }, { "cell_type": "code", "execution_count": 302, "metadata": {}, "outputs": [], "source": [ "bim_path_temp[\"frames\"] = new_frames" ] }, { "cell_type": "code", "execution_count": 303, "metadata": {}, "outputs": [], "source": [ "# Write the updated dictionary back to the JSON file\n", "bim_path_name = './site1_large/bim_camera_path.json'\n", "with open(bim_path_name, 'w') as f:\n", " json.dump(bim_path_temp, f, indent=4)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Find Angular Relationship" ] }, { "cell_type": "code", "execution_count": 322, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "8.0981\n", "Euler Angles (degrees): [90.0, 359.8999999997664, 270.0]\n", "Translation Vector: [-1109.96347 -462.780196 91.3883773]\n" ] } ], "source": [ "import math\n", "import numpy as np\n", "\n", "# 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", " singular = sy < 1e-6\n", " if not singular:\n", " print(sy)\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", "# Given extrinsic matrix\n", "\n", "extrinsic_matrix = np.array([[ 4.95865612e-16, -4.95000918e-16, 8.09811233e+00, -1.10996347e+03],\n", " [ 8.09810000e+00, 1.41338652e-02, -4.95000918e-16, -4.62780196e+02],\n", " [-1.41338652e-02, 8.09810000e+00, 4.95865612e-16, 9.13883773e+01],\n", " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])\n", "\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\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(\"Euler Angles (degrees):\", euler_angles_deg)\n", "print(\"Translation Vector:\", T_vector)" ] }, { "cell_type": "code", "execution_count": 286, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Extrinsic Matrix:\n", "[[ 1.07741548e-01 3.23598376e-01 9.40040345e-01 -1.08595013e+03]\n", " [ 1.83026241e-01 -9.35839245e-01 3.01174871e-01 -4.51663558e+02]\n", " [ 9.77186346e-01 1.39603003e-01 -1.60055763e-01 1.20258615e+02]\n", " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n" ] } ], "source": [ "import numpy as np\n", "import math\n", "\n", "# Function to convert Euler angles to a rotation matrix\n", "def euler_angles_to_rotation_matrix(angles):\n", " x, y, z = map(math.radians, angles)\n", " \n", " # Rotation matrix for X-axis\n", " Rx = np.array([[1, 0, 0],\n", " [0, math.cos(x), -math.sin(x)],\n", " [0, math.sin(x), math.cos(x)]])\n", " \n", " # Rotation matrix for Y-axis\n", " Ry = np.array([[math.cos(y), 0, math.sin(y)],\n", " [0, 1, 0],\n", " [-math.sin(y), 0, math.cos(y)]])\n", " \n", " # Rotation matrix for Z-axis\n", " Rz = np.array([[math.cos(z), -math.sin(z), 0],\n", " [math.sin(z), math.cos(z), 0],\n", " [0, 0, 1]])\n", " \n", " # Combined rotation matrix\n", " R = Rz @ Ry @ Rx\n", " return R\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", "# Input Euler angles (degrees) and translation vector\n", "euler_angles_deg = [138.9045818099805, 282.2620812069248, 239.51607287019073]\n", "translation_vector = [-1085.95013, -451.663558, 120.258615]\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", "# Normalize the angles to be within 0 to 360 degrees\n", "euler_angles_deg = normalize_angles(euler_angles_deg)\n", "\n", "# Convert Euler angles to rotation matrix\n", "rotation_matrix = euler_angles_to_rotation_matrix(euler_angles_deg)\n", "\n", "# # Construct the 4x4 transformation matrix\n", "transform_matrix = np.eye(4)\n", "transform_matrix[:3, :3] = rotation_matrix\n", "transform_matrix[:3, 3] = translation_vector\n", "\n", "print(\"Extrinsic Matrix:\")\n", "print(transform_matrix)\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] }, { "cell_type": "code", "execution_count": 318, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Euler Angles (degrees): [92.42400882034067, 0.8665419454060129, 90.09314479351303]\n", "Translation Vector: [-1109.96346502 -462.780196 91.38837728]\n", "New sy value: 0.9998856343399276\n", "New Extrinsic Matrix:\n", "[[-1.62549669e-03 4.22696956e-02 9.99104915e-01 -1.10996347e+03]\n", " [ 9.99884313e-01 1.51786381e-02 9.84593522e-04 -4.62780196e+02]\n", " [-1.51234335e-02 9.98990932e-01 -4.22894784e-02 9.13883773e+01]\n", " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n" ] } ], "source": [ "import math\n", "import numpy as np\n", "\n", "# Function to convert rotation matrix to Euler angles\n", "def rotation_matrix_to_euler_angles(R, sy_fixed):\n", " sy = sy_fixed\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", "# Given extrinsic matrix\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", "# Set fixed sy value\n", "sy_fixed = 8.0981\n", "\n", "# Convert rotation matrix to Euler angles with fixed sy\n", "euler_angles_deg = rotation_matrix_to_euler_angles(R_matrix, sy_fixed)\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)\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\n", "\n", "# Construct the rotation matrix from the normalized Euler angles\n", "R_new = euler_angles_to_rotation_matrix(euler_angles_deg)\n", "\n", "# Verify that the sy value is as desired\n", "sy_new = math.sqrt(R_new[0, 0] * R_new[0, 0] + R_new[1, 0] * R_new[1, 0])\n", "print(\"New sy value:\", sy_new)\n", "\n", "# Construct the new transformation matrix\n", "new_extrinsic_matrix = np.eye(4)\n", "new_extrinsic_matrix[:3, :3] = R_new\n", "new_extrinsic_matrix[:3, 3] = T_vector\n", "\n", "print(\"New Extrinsic Matrix:\")\n", "print(new_extrinsic_matrix)" ] }, { "cell_type": "code", "execution_count": 319, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Euler Angles (degrees): [92.42400882034067, 0.8665419454060129, 90.09314479351303]\n", "Translation Vector: [-1109.96346502 -462.780196 91.38837728]\n", "New sy value: 8.098099999999999\n", "New Extrinsic Matrix:\n", "[[-1.31649404e-02 3.42343375e-01 8.09177693e+00 -1.10996347e+03]\n", " [ 8.09808930e+00 1.22932189e-01 7.97424878e-03 -4.62780196e+02]\n", " [-1.22485085e-01 8.09085378e+00 -3.42503596e-01 9.13883773e+01]\n", " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n" ] } ], "source": [ "import math\n", "import numpy as np\n", "\n", "# Function to convert rotation matrix to Euler angles\n", "def rotation_matrix_to_euler_angles(R, sy_fixed):\n", " sy = sy_fixed\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", "# Given extrinsic matrix\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", "# Set fixed sy value\n", "sy_fixed = 8.0981\n", "\n", "# Convert rotation matrix to Euler angles with fixed sy\n", "euler_angles_deg = rotation_matrix_to_euler_angles(R_matrix, sy_fixed)\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)\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\n", "\n", "# Construct the rotation matrix from the normalized Euler angles\n", "R_new = euler_angles_to_rotation_matrix(euler_angles_deg)\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", "# Verify that the sy value is as desired\n", "sy_new = math.sqrt(R_new_scaled[0, 0] * R_new_scaled[0, 0] + R_new_scaled[1, 0] * R_new_scaled[1, 0])\n", "print(\"New sy value:\", sy_new)\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)\n" ] }, { "cell_type": "code", "execution_count": 320, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "[92.42400882034067, 0.8665419454060129, 90.09314479351303]" ] }, "execution_count": 320, "metadata": {}, "output_type": "execute_result" } ], "source": [ "euler_angles_deg" ] }, { "cell_type": "code", "execution_count": 344, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "New sy value: 8.0981\n", "New Extrinsic Matrix:\n", "[[ 4.95865612e-16 4.95865612e-16 -8.09810000e+00 -1.10996347e+03]\n", " [-8.09810000e+00 3.03630117e-32 -4.95865612e-16 -4.62780196e+02]\n", " [ 0.00000000e+00 8.09810000e+00 4.95865612e-16 9.13883773e+01]\n", " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n" ] } ], "source": [ "# Set fixed sy value\n", "sy_fixed = 8.0981\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\n", "\n", "# Construct the rotation matrix from the normalized Euler angles\n", "euler_angles_deg = [90.0, 0.0, -90.0]\n", "R_new = euler_angles_to_rotation_matrix(euler_angles_deg)\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", "# Verify that the sy value is as desired\n", "sy_new = math.sqrt(R_new_scaled[0, 0] * R_new_scaled[0, 0] + R_new_scaled[1, 0] * R_new_scaled[1, 0])\n", "print(\"New sy value:\", sy_new)\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": 345, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[ 4.95865612e-16, 4.95865612e-16, -8.09810000e+00],\n", " [-8.09810000e+00, 3.03630117e-32, -4.95865612e-16],\n", " [ 0.00000000e+00, 8.09810000e+00, 4.95865612e-16]])" ] }, "execution_count": 345, "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" } }, "nbformat": 4, "nbformat_minor": 2 }