{ "cells": [ { "cell_type": "code", "execution_count": 1, "id": "2bc3579f-381e-4c88-bf83-547e17cfb91c", "metadata": {}, "outputs": [], "source": [ "import cv2\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "import PIL\n", "from PIL import Image\n", "import json\n", "import os\n", "from scipy.optimize import minimize\n", "from scipy.spatial import cKDTree" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "project_name = 'site1_small'\n", "\n", "img_checkerboard_set = []\n", "bim_checkerboard_set = []\n", "\n", "chessboard_json_name = 'checkerboard_position_site.json'" ] }, { "cell_type": "code", "execution_count": 3, "id": "ba767fce-62d6-4b61-a23a-90af2b7ecc68", "metadata": {}, "outputs": [], "source": [ "matrix_path = os.path.join(project_name,'transforms.json')\n", "with open(matrix_path,'r') as file:\n", " t_data = json.load(file)\n" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Checkerboard Detection (2checkerboards)" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "def find_chessboard_corners_and_area(image_path):\n", " checkerboard_size = (6,3)\n", "\n", " image = cv2.imread(image_path)\n", " if image is None:\n", " return None\n", " gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)\n", " ret, corners = cv2.findChessboardCorners(gray, checkerboard_size, None)\n", " if ret:\n", " criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)\n", " corners_refined = cv2.cornerSubPix(gray, corners, (6,3), (1, -1), criteria)\n", "\n", " for i, corner in enumerate(corners_refined):\n", " coord = np.array(corner)\n", "\n", " # # Remove the unnecessary dimension\n", " data_2d = np.squeeze(corners)\n", "\n", " num_rows,_ = checkerboard_size\n", "\n", " # Step 1: Determine row assignments by sorting y-values and slicing into `num_rows` bins\n", " sorted_indices_by_y = np.argsort(data_2d[:, 1])\n", " data_sorted_by_y = data_2d[sorted_indices_by_y]\n", " row_boundaries = np.linspace(0, len(data_sorted_by_y), num_rows + 1, dtype=int)\n", "\n", " # Step 2: For each row, sort by x-values\n", " sorted_img_coord = np.zeros_like(data_sorted_by_y)\n", " for i in range(num_rows):\n", " start_idx = row_boundaries[i]\n", " end_idx = row_boundaries[i+1]\n", " row_data = data_sorted_by_y[start_idx:end_idx]\n", " sorted_indices_by_x = np.argsort(row_data[:, 0])\n", " sorted_img_coord[start_idx:end_idx] = row_data[sorted_indices_by_x]\n", "\n", " return sorted_img_coord\n", " return None" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "def find_chessboard_img(project_name, start_id, end_id):\n", " transform_path = os.path.join(project_name,'transforms.json')\n", " with open(transform_path,'r') as file:\n", " t_data = json.load(file)\n", "\n", " id_chessboard_dic = {}\n", "\n", " for idx, frame in enumerate(t_data[\"frames\"]):\n", " if idx > end_id:\n", " break\n", "\n", " if idx > start_id:\n", " img_file_path = os.path.join(\"/home/worker/yt/autocon/dataset\", project_name, frame[\"file_path\"])\n", " sorted_img_coord = find_chessboard_corners_and_area(img_file_path)\n", " \n", " if sorted_img_coord is not None:\n", " x1 = sorted_img_coord[0][0]\n", " y1 = sorted_img_coord[0][1]\n", " x2 = sorted_img_coord[-1][0]\n", " y2 = sorted_img_coord[-1][1]\n", " area = (x2-x1) * (y2-y1)\n", " \n", " if x1>350 and area > 10000 and x2<700:\n", " id_chessboard_dic[idx] = sorted_img_coord\n", "\n", " \n", "\n", " return id_chessboard_dic\n" ] }, { "cell_type": "code", "execution_count": 11, "metadata": {}, "outputs": [], "source": [ "# start_id = 620\n", "# end_id = 690\n", "\n", "start_id = 900\n", "end_id = 950" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [], "source": [ "id_chessboard_dic = find_chessboard_img(project_name, start_id, end_id)" ] }, { "cell_type": "code", "execution_count": 13, "id": "2590c1bb", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "dict_keys([929, 930, 931, 932, 933, 934, 935])" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "id_chessboard_dic.keys()" ] }, { "cell_type": "code", "execution_count": 28, "id": "c1d38628", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[601.64044, 741.8987 ],\n", " [640.3201 , 743.0146 ],\n", " [677.86816, 744.0117 ],\n", " [601.94995, 782.5038 ],\n", " [640.3173 , 783.2275 ],\n", " [677.424 , 783.9391 ],\n", " [601.6655 , 823.0857 ],\n", " [639.5185 , 823.5291 ],\n", " [676.86676, 824.20325],\n", " [601.0231 , 863.4933 ],\n", " [638.9556 , 863.83905],\n", " [676.15845, 863.6311 ],\n", " [600.50275, 903.6656 ],\n", " [638.39246, 903.7394 ],\n", " [675.98315, 903.3695 ],\n", " [599.4354 , 943.422 ],\n", " [637.591 , 943.1937 ],\n", " [675.04315, 942.49005]], dtype=float32)" ] }, "execution_count": 28, "metadata": {}, "output_type": "execute_result" } ], "source": [ "id_chessboard_dic[929]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Camera-to-World Transform" ] }, { "cell_type": "code", "execution_count": 30, "metadata": {}, "outputs": [], "source": [ "def find_avg_world_coord(t_data, id_chessboard_dic):\n", " total_sorted_world_coord = []\n", " \n", " for key, chessboard_coord in id_chessboard_dic.items():\n", " sorted_world_coord = []\n", " # 2D point (u, v) and depth Z\n", " for img_coord in chessboard_coord:\n", " \n", " u,v = img_coord\n", " Z = 1\n", "\n", " # Intrinsic matrix\n", " K = np.array([\n", " [t_data['fl_x'], 0, t_data['cx']],\n", " [0, t_data['fl_y'], t_data['cy']],\n", " [0, 0, 1]\n", " ])\n", " inv_intrinsic = np.linalg.inv(K)\n", "\n", " # Convert to normalized camera coordinates\n", " normalized_coords = inv_intrinsic @ np.array([u, v, 1.0])\n", "\n", " camera_coords = normalized_coords * Z\n", "\n", " extrinsic = t_data['frames'][key]['transform_matrix']\n", "\n", " # Inverse of the extrinsic matrix\n", " inv_extrinsic = np.linalg.inv(extrinsic)\n", "\n", " # Convert to world coordinates\n", " world_coords = inv_extrinsic @ np.append(camera_coords, 1.0)\n", "\n", " # print(\"3D World Coordinates:\", world_coords[:3])\n", " sorted_world_coord.append(list(world_coords[:3]))\n", "\n", " total_sorted_world_coord.append(sorted_world_coord)\n", " \n", " averaged_sorted_world_coord = np.mean(np.array(total_sorted_world_coord), axis=0)\n", "\n", " return total_sorted_world_coord, averaged_sorted_world_coord\n", "\n", "total, averaged_sorted_world_coord = find_avg_world_coord(t_data, id_chessboard_dic)" ] }, { "cell_type": "code", "execution_count": 37, "id": "27e2573a", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[-2.20596983, 0.73878678, 2.78244215],\n", " [-2.19945219, 0.74139677, 2.74966271],\n", " [-2.19283866, 0.74396524, 2.7176778 ],\n", " [-2.2390896 , 0.7364595 , 2.77432135],\n", " [-2.23215425, 0.73910266, 2.74158333],\n", " [-2.22557206, 0.74165309, 2.70984357],\n", " [-2.27220386, 0.7341361 , 2.76614593],\n", " [-2.26505423, 0.73677817, 2.73371835],\n", " [-2.25792776, 0.73936852, 2.70208537],\n", " [-2.30507916, 0.73179928, 2.75851169],\n", " [-2.29775959, 0.73444996, 2.72617916],\n", " [-2.29033799, 0.73706271, 2.69459189],\n", " [-2.33779172, 0.72948039, 2.75081373],\n", " [-2.33016549, 0.73214632, 2.71865684],\n", " [-2.32249636, 0.73479123, 2.68689448],\n", " [-2.37005243, 0.72719925, 2.74313075],\n", " [-2.36234042, 0.72986049, 2.71116619],\n", " [-2.35439757, 0.73252617, 2.67944637]])" ] }, "execution_count": 37, "metadata": {}, "output_type": "execute_result" } ], "source": [ "np.array(total)[5]" ] }, { "cell_type": "code", "execution_count": 15, "metadata": {}, "outputs": [], "source": [ "img_checkerboard_set.append(list(list(row) for row in averaged_sorted_world_coord))" ] }, { "cell_type": "code", "execution_count": 27, "id": "60cf2f13", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "[[[4.481951804611874, 0.9115078138770324, 0.9122072649023761],\n", " [4.488444510595118, 0.9139387837402021, 0.8862358961825851],\n", " [4.4948848098983305, 0.9163478484604131, 0.86050812904543],\n", " [4.4557108225766875, 0.9093330299528476, 0.9048775711667555],\n", " [4.462341269699123, 0.9117730467272691, 0.878981338666575],\n", " [4.4688684692777425, 0.9141802234482092, 0.8534134366194882],\n", " [4.429627636826863, 0.9071866567180753, 0.8973732551439256],\n", " [4.436332711456188, 0.9096160800127542, 0.8717416105449226],\n", " [4.443011115109557, 0.9120265077528692, 0.8463510595699172],\n", " [4.40363270319986, 0.9050432093792089, 0.8899577376602936],\n", " [4.410507932310953, 0.9074825943945575, 0.8644334791139615],\n", " [4.417351302203976, 0.9098995004881478, 0.8391925427467042],\n", " [4.377655457277562, 0.9028930303360332, 0.8826627915563943],\n", " [4.384786650172604, 0.9053563057556453, 0.8571758391581845],\n", " [4.391753691643939, 0.9077729939082627, 0.8321219241605806],\n", " [4.35195971319477, 0.900771754538208, 0.8753656215064812],\n", " [4.359080938439423, 0.9032285686308695, 0.8499593455694724],\n", " [4.366259218211981, 0.9056668335361046, 0.8249066830017666]],\n", " [[-2.143894397968387, 0.7482764317120427, 2.697195049470595],\n", " [-2.1375374862855656, 0.7507783665345501, 2.665689181233374],\n", " [-2.1311657649126277, 0.7532389898583627, 2.634869767517209],\n", " [-2.176224974715105, 0.7459932848184778, 2.6892842329370366],\n", " [-2.169587681521868, 0.7485050128839035, 2.6580092776995246],\n", " [-2.1630574374524323, 0.750967115379818, 2.6273855416579224],\n", " [-2.2084970212697854, 0.7437014021537756, 2.6815966440637076],\n", " [-2.20158585069915, 0.7462212742580537, 2.6505688336186135],\n", " [-2.1948295266151776, 0.7486982038954337, 2.6200183328070925],\n", " [-2.24062636622408, 0.7414115805051517, 2.674072999977144],\n", " [-2.2336075102902435, 0.7439314645084626, 2.6431931200258885],\n", " [-2.2264127857946816, 0.746446183144963, 2.612640364892408],\n", " [-2.2726276854483833, 0.7391195458172656, 2.6667616392799607],\n", " [-2.2652240282882365, 0.741674290842944, 2.635851336885896],\n", " [-2.257899718474603, 0.7441939565208654, 2.6053981392986882],\n", " [-2.3041852188433345, 0.7368627461340916, 2.6594970005083423],\n", " [-2.2966953002683757, 0.739420330307461, 2.6286605730407606],\n", " [-2.2891364033906667, 0.7419559830373978, 2.5982732865609157]]]" ] }, "execution_count": 27, "metadata": {}, "output_type": "execute_result" } ], "source": [ "img_checkerboard_set" ] }, { "cell_type": "code", "execution_count": 16, "id": "8fe5ab52-dec7-47c1-8c95-1d1ec92d6647", "metadata": {}, "outputs": [], "source": [ "# img_path = os.path.join(project_name,\"\")\n", "# checkerboard_size = (6,3)\n", "\n", "# image = cv2.imread(img_path)\n", "# gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)\n", "# ret, corners = cv2.findChessboardCorners(gray, checkerboard_size, None)\n", "\n", "# if ret:\n", "# # Refine the corner positions\n", "# corners2 = cv2.cornerSubPix(gray, corners, (6, 3), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))\n", "# vis_corrected = image.copy()\n", "# for i, corner in enumerate(corners2):\n", "# corner_point = (int(corner[0][0]), int(corner[0][1])) # Ensure the coordinates are in integer format\n", "# cv2.circle(vis_corrected, corner_point, 5, (0, 0, 255), -1) # Red dot\n", "# cv2.putText(vis_corrected, str(i), (corner_point[0] + 3, corner_point[1] - 3), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 1)\n", "# # Display the corrected visualization\n", "# plt.imshow(cv2.cvtColor(vis_corrected, cv2.COLOR_BGR2RGB))\n", "# plt.title(\"Corrected Chessboard Corners Visualization\")\n", "# plt.show()\n", "# cv2.imwrite('result.png',vis_corrected)\n", "# else:\n", "# print(\"Chessboard corners not found.\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# World-to-BIM Transform" ] }, { "cell_type": "code", "execution_count": 17, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "(2, 18, 3)" ] }, "execution_count": 17, "metadata": {}, "output_type": "execute_result" } ], "source": [ "np.array(img_checkerboard_set).shape" ] }, { "cell_type": "code", "execution_count": 21, "metadata": {}, "outputs": [], "source": [ "checkerboard_name = 'checkboard_03'" ] }, { "cell_type": "code", "execution_count": 22, "id": "faa9f588-420b-43c6-a3c8-89e79744aba3", "metadata": {}, "outputs": [], "source": [ "bim_path = os.path.join(project_name,chessboard_json_name)\n", "with open(bim_path,'r') as file:\n", " bim_data = json.load(file)\n", "\n", "checkerboard_1 = bim_data[checkerboard_name]" ] }, { "cell_type": "code", "execution_count": 23, "id": "b8362727-fd1f-40e1-ae26-26a6c72d2cd7", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "(18, 3)\n", "[533.4455, 679.70435, 170.0]\n", "[537.4455, 679.70435, 170.0]\n", "[541.4455, 679.70435, 170.0]\n", "[533.4455, 679.70435, 166.0]\n", "[537.4455, 679.70435, 166.0]\n", "[541.4455, 679.70435, 166.0]\n", "[533.4455, 679.70435, 162.0]\n", "[537.4455, 679.70435, 162.0]\n", "[541.4455, 679.70435, 162.0]\n", "[533.4455, 679.70435, 158.0]\n", "[537.4455, 679.70435, 158.0]\n", "[541.4455, 679.70435, 158.0]\n", "[533.4455, 679.70435, 154.0]\n", "[537.4455, 679.70435, 154.0]\n", "[541.4455, 679.70435, 154.0]\n", "[533.4455, 679.70435, 150.0]\n", "[537.4455, 679.70435, 150.0]\n", "[541.4455, 679.70435, 150.0]\n" ] } ], "source": [ "bim_world_coord = []\n", "\n", "for idx, point in enumerate(checkerboard_1):\n", " temp = [point[\"x\"], point[\"y\"], point[\"z\"]]\n", "\n", " bim_world_coord.append(temp)\n", "\n", "print(np.array(bim_world_coord).shape)\n", "\n", "for row in bim_world_coord:\n", " print(row)\n", "\n", "bim_checkerboard_set.append(bim_world_coord)" ] }, { "cell_type": "code", "execution_count": 26, "id": "b2e6a5de", "metadata": {}, "outputs": [ { "data": { "text/plain": [ "[[4.481951804611874, 0.9115078138770324, 0.9122072649023761],\n", " [4.488444510595118, 0.9139387837402021, 0.8862358961825851],\n", " [4.4948848098983305, 0.9163478484604131, 0.86050812904543],\n", " [4.4557108225766875, 0.9093330299528476, 0.9048775711667555],\n", " [4.462341269699123, 0.9117730467272691, 0.878981338666575],\n", " [4.4688684692777425, 0.9141802234482092, 0.8534134366194882],\n", " [4.429627636826863, 0.9071866567180753, 0.8973732551439256],\n", " [4.436332711456188, 0.9096160800127542, 0.8717416105449226],\n", " [4.443011115109557, 0.9120265077528692, 0.8463510595699172],\n", " [4.40363270319986, 0.9050432093792089, 0.8899577376602936],\n", " [4.410507932310953, 0.9074825943945575, 0.8644334791139615],\n", " [4.417351302203976, 0.9098995004881478, 0.8391925427467042],\n", " [4.377655457277562, 0.9028930303360332, 0.8826627915563943],\n", " [4.384786650172604, 0.9053563057556453, 0.8571758391581845],\n", " [4.391753691643939, 0.9077729939082627, 0.8321219241605806],\n", " [4.35195971319477, 0.900771754538208, 0.8753656215064812],\n", " [4.359080938439423, 0.9032285686308695, 0.8499593455694724],\n", " [4.366259218211981, 0.9056668335361046, 0.8249066830017666]]" ] }, "execution_count": 26, "metadata": {}, "output_type": "execute_result" } ], "source": [ "img_checkerboard_set[0]" ] }, { "cell_type": "markdown", "id": "a12bd8e7-3758-431e-a059-646d4c7aeba6", "metadata": {}, "source": [ "# Find Transform" ] }, { "cell_type": "code", "execution_count": 24, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "(2, 18, 3)\n", "(2, 18, 3)\n" ] } ], "source": [ "print(np.array(img_checkerboard_set).shape)\n", "print(np.array(bim_checkerboard_set).shape)\n" ] }, { "cell_type": "code", "execution_count": 25, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "(array([[-0.96571335, 0.21260469, -0.14898648],\n", " [-0.02361833, 0.4995553 , 0.86595997],\n", " [ 0.25853414, 0.83978792, -0.47740586]]),\n", " array([ -59.26414933, -475.8021578 , -508.80899321]),\n", " 8007441.215974621)" ] }, "execution_count": 25, "metadata": {}, "output_type": "execute_result" } ], "source": [ "import numpy as np\n", "from scipy.optimize import minimize\n", "from scipy.spatial.transform import Rotation as R\n", "\n", "# Define your BIM points and Real World points here:\n", "bim_points = np.array(bim_checkerboard_set).reshape(-1, 3)\n", "real_points = np.array(img_checkerboard_set).reshape(-1, 3)\n", "\n", "def transform_points(points, R, T):\n", " return np.dot(points, R.T) + T\n", "\n", "def error_function(params, source_points, target_points):\n", " # Extract rotation parameters\n", " rotation_vector = params[:3]\n", " translation_vector = params[3:6]\n", " \n", " # Convert rotation vector to rotation matrix\n", " rotation = R.from_rotvec(rotation_vector).as_matrix()\n", " \n", " # Transform source points\n", " transformed_points = transform_points(source_points, rotation, translation_vector)\n", " \n", " # Compute the error\n", " error = np.sum((transformed_points - target_points)**2)\n", " \n", " return error\n", "\n", "def find_transformation(source_points, target_points):\n", " # Initial guess for the optimization (no rotation, no translation)\n", " initial_guess = np.zeros(6)\n", " \n", " # Perform the optimization\n", " result = minimize(error_function, initial_guess, args=(source_points, target_points), method='BFGS')\n", " \n", " # Extract the optimized rotation and translation\n", " optimized_params = result.x\n", " optimized_rotation_vector = optimized_params[:3]\n", " optimized_translation_vector = optimized_params[3:6]\n", " \n", " # Convert rotation vector to rotation matrix\n", " optimized_rotation = R.from_rotvec(optimized_rotation_vector).as_matrix()\n", " \n", " # Calculate the final loss\n", " final_loss = error_function(optimized_params, source_points, target_points)\n", " \n", " return optimized_rotation, optimized_translation_vector, final_loss\n", "\n", "# Print results\n", "# Find the transformation matrix\n", "R_opt, T_opt, final_loss = find_transformation(bim_points, real_points)\n", "\n", "R_opt, T_opt, final_loss\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Vaildate" ] }, { "cell_type": "code", "execution_count": 25, "metadata": {}, "outputs": [ { "ename": "IndexError", "evalue": "index 52 is out of bounds for axis 0 with size 36", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mIndexError\u001b[0m Traceback (most recent call last)", "Cell \u001b[0;32mIn[25], line 5\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[39mimport\u001b[39;00m \u001b[39mcopy\u001b[39;00m\n\u001b[1;32m 3\u001b[0m i \u001b[39m=\u001b[39m \u001b[39m52\u001b[39m\n\u001b[0;32m----> 5\u001b[0m input_4x1 \u001b[39m=\u001b[39m \u001b[39mlist\u001b[39m(copy\u001b[39m.\u001b[39mdeepcopy(bim_points[i]))\n\u001b[1;32m 6\u001b[0m input_4x1\u001b[39m.\u001b[39mappend(\u001b[39m1.0\u001b[39m)\n\u001b[1;32m 8\u001b[0m matrix_1 \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray(input_4x1)\n", "\u001b[0;31mIndexError\u001b[0m: index 52 is out of bounds for axis 0 with size 36" ] } ], "source": [ "import copy\n", "\n", "i = 52\n", "\n", "input_4x1 = list(copy.deepcopy(bim_points[i]))\n", "input_4x1.append(1.0)\n", "\n", "matrix_1 = np.array(input_4x1)\n", "matrix_2 = np.array(optimized_T)\n", "\n", "print(matrix_1.shape)\n", "print(matrix_2.shape)\n", "\n", "result = np.dot(matrix_2, matrix_1)\n", "\n", "print(result)\n", "\n", "print(bim_points[i])\n", "print(real_points[i])" ] }, { "cell_type": "code", "execution_count": 26, "metadata": {}, "outputs": [ { "ename": "NameError", "evalue": "name 'optimized_T' is not defined", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)", "Cell \u001b[0;32mIn[26], line 9\u001b[0m\n\u001b[1;32m 6\u001b[0m input_4x1\u001b[39m.\u001b[39mappend(\u001b[39m1.0\u001b[39m)\n\u001b[1;32m 8\u001b[0m matrix_1 \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray(input_4x1)\n\u001b[0;32m----> 9\u001b[0m matrix_2 \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39mlinalg\u001b[39m.\u001b[39minv(np\u001b[39m.\u001b[39marray(optimized_T))\n\u001b[1;32m 11\u001b[0m \u001b[39mprint\u001b[39m(matrix_1\u001b[39m.\u001b[39mshape)\n\u001b[1;32m 12\u001b[0m \u001b[39mprint\u001b[39m(matrix_2\u001b[39m.\u001b[39mshape)\n", "\u001b[0;31mNameError\u001b[0m: name 'optimized_T' is not defined" ] } ], "source": [ "import copy\n", "\n", "i = 1\n", "\n", "input_4x1 = list(copy.deepcopy(real_points[i]))\n", "input_4x1.append(1.0)\n", "\n", "matrix_1 = np.array(input_4x1)\n", "matrix_2 = np.linalg.inv(np.array(optimized_T))\n", "\n", "print(matrix_1.shape)\n", "print(matrix_2.shape)\n", "\n", "result = np.dot(matrix_2, matrix_1)\n", "\n", "print(result)\n", "\n", "print(bim_points[i])" ] }, { "cell_type": "code", "execution_count": 34, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([-92.68070808, -9.63774034, 82.03935557])" ] }, "execution_count": 34, "metadata": {}, "output_type": "execute_result" } ], "source": [ "real_points[i]" ] }, { "cell_type": "code", "execution_count": 47, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[-8.55722911e+00, 1.51345414e+03, 5.72364316e+02,\n", " -4.61040178e+04],\n", " [-5.36006705e+00, 2.59038223e+02, 3.09821117e+01,\n", " -2.11250811e+03],\n", " [-2.69241894e-01, -4.89531081e+02, -1.93163097e+02,\n", " 1.12960521e+04],\n", " [-3.64343226e-05, -1.21340427e-02, -4.21268065e-03,\n", " 1.23026114e+00]])" ] }, "execution_count": 47, "metadata": {}, "output_type": "execute_result" } ], "source": [ "np.linalg.inv(np.array(optimized_T))" ] }, { "cell_type": "code", "execution_count": 50, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[-1.18780424e-01, 2.91547237e-02, -3.14218554e-01,\n", " -1.51613015e+03],\n", " [-3.57023343e-03, 6.42955596e-03, -8.59090898e-03,\n", " -4.38738237e+01],\n", " [ 8.68843546e-03, -1.57249011e-02, 1.44214229e-02,\n", " 1.66182324e+02],\n", " [-8.97975620e-06, 1.04325430e-05, -4.46555168e-05,\n", " 9.04252763e-01]])" ] }, "execution_count": 50, "metadata": {}, "output_type": "execute_result" } ], "source": [ "optimized_T" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Save Transform" ] }, { "cell_type": "code", "execution_count": 45, "metadata": {}, "outputs": [], "source": [ "BIM_camera_transform = {\"b2c_transform\": list(list(row) for row in optimized_T)}\n", "\n" ] }, { "cell_type": "code", "execution_count": 46, "metadata": {}, "outputs": [], "source": [ "# Define the filename\n", "filename = os.path.join(project_name,'transform_matrix.json')\n", "\n", "# Write the updated dictionary back to the JSON file\n", "with open(filename, 'w') as f:\n", " json.dump(BIM_camera_transform, f, indent=4)" ] }, { "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": 5 }