{ "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_large'\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" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Checkerboard Detection (3checkerboards)" ] }, { "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/yt/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>250 and area > 10000 and x2<800:\n", " id_chessboard_dic[idx] = sorted_img_coord\n", "\n", " return id_chessboard_dic\n" ] }, { "cell_type": "code", "execution_count": 15, "metadata": {}, "outputs": [], "source": [ "# start_id = 550\n", "# end_id = 650\n", "\n", "# start_id = 1100\n", "# end_id = 1200\n", "\n", "start_id = 1490\n", "end_id = 1590" ] }, { "cell_type": "code", "execution_count": 16, "metadata": {}, "outputs": [], "source": [ "id_chessboard_dic = find_chessboard_img(project_name, start_id, end_id)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Camera-to-World Transform" ] }, { "cell_type": "code", "execution_count": 17, "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", " \n", " averaged_sorted_world_coord = np.mean(np.array(total_sorted_world_coord), axis=0)\n", "\n", " return averaged_sorted_world_coord\n", "\n", "averaged_sorted_world_coord = find_avg_world_coord(t_data, id_chessboard_dic)" ] }, { "cell_type": "code", "execution_count": 18, "metadata": {}, "outputs": [], "source": [ "img_checkerboard_set.append(list(list(row) for row in averaged_sorted_world_coord))" ] }, { "cell_type": "code", "execution_count": 10, "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": 19, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "(3, 18, 3)" ] }, "execution_count": 19, "metadata": {}, "output_type": "execute_result" } ], "source": [ "np.array(img_checkerboard_set).shape" ] }, { "cell_type": "code", "execution_count": 29, "metadata": {}, "outputs": [], "source": [ "checkerboard_name = 'checkboard_03'" ] }, { "cell_type": "code", "execution_count": 30, "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": 31, "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": "markdown", "id": "a12bd8e7-3758-431e-a059-646d4c7aeba6", "metadata": {}, "source": [ "# Find Transform" ] }, { "cell_type": "code", "execution_count": 32, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "(3, 18, 3)\n", "(3, 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": 35, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "(array([[-0.84745431, -0.53018577, -0.02691186],\n", " [-0.27256567, 0.47805414, -0.83496838],\n", " [ 0.45555368, -0.7002623 , -0.54963948]]),\n", " array([-145.89157086, -120.88483493, 546.11585496]),\n", " 37672918.39029274)" ] }, "execution_count": 35, "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": 38, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "(4,)\n", "(4, 4)\n", "[-963.05282009 46.1067744 -9.08497882 1. ]\n", "[-12815.54102 -1562.91003 150. ]\n", "[-90.89958626 -9.57438392 83.31928567]\n" ] } ], "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": 42, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "(4,)\n", "(4, 4)\n", "[-1.37921848e+04 -1.98572172e+03 1.59764768e+02 1.00000000e+00]\n", "[-14605.52441 -2702.89014 170. ]\n" ] } ], "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.18" } }, "nbformat": 4, "nbformat_minor": 5 }