{"metadata":{"jupytext":{"formats":"ipynb,py:percent"},"kaggle":{"accelerator":"none","dataSources":[{"sourceId":71885,"databundleVersionId":8143495,"sourceType":"competition"},{"sourceId":169973704,"sourceType":"kernelVersion"}],"dockerImageVersionId":30698,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false},"kernelspec":{"display_name":"Python 3","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.10.13"}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"## Outline\n\nThis notebook demonstrated how to align the ground truth and estimated camera poses using \n\n- The [metrics (mAA) calculation code](https://www.kaggle.com/code/fabiobellavia/imc2024-3d-metric-evaluation-example) provided by the competition organizers\n- visualization code from the [HLoc library](https://github.com/cvg/Hierarchical-Localization/blob/master/hloc/utils/viz_3d.py)\n\nWe use the output of [asarvazyan/imc-understanding-the-baseline](https://www.kaggle.com/code/asarvazyan/imc-understanding-the-baseline) as the estimated camera poses.","metadata":{}},{"cell_type":"markdown","source":"## Install the packages required for the notebook","metadata":{}},{"cell_type":"code","source":"!pip install pycolmap","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.213589Z","start_time":"2024-05-27T14:08:14.078928Z"},"_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","execution":{"iopub.execute_input":"2024-05-27T09:58:53.735289Z","iopub.status.busy":"2024-05-27T09:58:53.734685Z","iopub.status.idle":"2024-05-27T09:59:05.609012Z","shell.execute_reply":"2024-05-27T09:59:05.607732Z","shell.execute_reply.started":"2024-05-27T09:58:53.735252Z"}},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Utility code for metrics calculation","metadata":{}},{"cell_type":"code","source":"'''Image Matching Challenge 2024 - Hexathlon | Metric Example Notebook'''\n\nimport time\nimport math\nimport numpy as np\nimport pandas as pd\nimport tqdm\nimport logging\nfrom typing import Optional\n\nimport plotly.graph_objects as go\nfrom pathlib import Path\nimport pycolmap\nfrom scipy.spatial.transform import Rotation\n\n_EPS = np.finfo(float).eps * 4.0\n\n# mAA evaluation thresholds per scene, different accoring to the scene\ntranslation_thresholds_meters_dict = {\n    'multi-temporal-temple-baalshamin': np.array([0.025, 0.05, 0.1, 0.2, 0.5, 1.0]),\n    'pond': np.array([0.025, 0.05, 0.1, 0.2, 0.5, 1.0]),\n    'transp_obj_glass_cylinder': np.array([0.0025, 0.005, 0.01, 0.02, 0.05, 0.1]),\n    'transp_obj_glass_cup': np.array([0.0025, 0.005, 0.01, 0.02, 0.05, 0.1]),\n    'church': np.array([0.025, 0.05, 0.1, 0.2, 0.5, 1.0]),\n    'lizard': np.array([0.025, 0.05, 0.1, 0.2, 0.5, 1.0]),\n    'dioscuri': np.array([0.025, 0.05, 0.1, 0.2, 0.5, 1.0]),\n}\n\n\ndef vector_norm(data, axis=None, out=None):\n    '''Return length, i.e. Euclidean norm, of ndarray along axis.'''\n    data = np.array(data, dtype=np.float64, copy=True)\n    if out is None:\n        if data.ndim == 1:\n            return math.sqrt(np.dot(data, data))\n        data *= data\n        out = np.atleast_1d(np.sum(data, axis=axis))\n        np.sqrt(out, out)\n        return out\n    data *= data\n    np.sum(data, axis=axis, out=out)\n    np.sqrt(out, out)\n    return None\n\n\ndef quaternion_matrix(quaternion):\n    '''Return homogeneous rotation matrix from quaternion.'''\n    q = np.array(quaternion, dtype=np.float64, copy=True)\n    n = np.dot(q, q)\n    if n < _EPS:\n        # print(\"special case\")\n        return np.identity(4)\n    q *= math.sqrt(2.0 / n)\n    q = np.outer(q, q)\n    return np.array(\n        [\n            [\n                1.0 - q[2, 2] - q[3, 3],\n                q[1, 2] - q[3, 0],\n                q[1, 3] + q[2, 0],\n                0.0,\n            ],\n            [\n                q[1, 2] + q[3, 0],\n                1.0 - q[1, 1] - q[3, 3],\n                q[2, 3] - q[1, 0],\n                0.0,\n            ],\n            [\n                q[1, 3] - q[2, 0],\n                q[2, 3] + q[1, 0],\n                1.0 - q[1, 1] - q[2, 2],\n                0.0,\n            ],\n            [0.0, 0.0, 0.0, 1.0],\n        ]\n    )\n\n\n# based on the 3D registration from https://github.com/cgohlke/transformations\ndef affine_matrix_from_points(v0, v1, shear=False, scale=True, usesvd=True):\n    '''Return affine transform matrix to register two point sets.\n    v0 and v1 are shape (ndims, -1) arrays of at least ndims non-homogeneous\n    coordinates, where ndims is the dimensionality of the coordinate space.\n    If shear is False, a similarity transformation matrix is returned.\n    If also scale is False, a rigid/Euclidean traffansformation matrix\n    is returned.\n    By default the algorithm by Hartley and Zissermann [15] is used.\n    If usesvd is True, similarity and Euclidean transformation matrices\n    are calculated by minimizing the weighted sum of squared deviations\n    (RMSD) according to the algorithm by Kabsch [8].\n    Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]\n    is used, which is slower when using this Python implementation.\n    The returned matrix performs rotation, translation and uniform scaling\n    (if specified).'''\n\n    v0 = np.array(v0, dtype=np.float64, copy=True)\n    v1 = np.array(v1, dtype=np.float64, copy=True)\n\n    ndims = v0.shape[0]\n    if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:\n        raise ValueError(\"input arrays are of wrong shape or type\")\n\n    # move centroids to origin\n    t0 = -np.mean(v0, axis=1)\n    M0 = np.identity(ndims + 1)\n    M0[:ndims, ndims] = t0\n    v0 += t0.reshape(ndims, 1)\n    t1 = -np.mean(v1, axis=1)\n    M1 = np.identity(ndims + 1)\n    M1[:ndims, ndims] = t1\n    v1 += t1.reshape(ndims, 1)\n\n    if shear:\n        # Affine transformation\n        A = np.concatenate((v0, v1), axis=0)\n        u, s, vh = np.linalg.svd(A.T)\n        vh = vh[:ndims].transformation_matrix\n        B = vh[:ndims]\n        C = vh[ndims: 2 * ndims]\n        t = np.dot(C, np.linalg.pinv(B))\n        t = np.concatenate((t, np.zeros((ndims, 1))), axis=1)\n        M = np.vstack((t, ((0.0,) * ndims) + (1.0,)))\n    elif usesvd or ndims != 3:\n        # Rigid transformation via SVD of covariance matrix\n        u, s, vh = np.linalg.svd(np.dot(v1, v0.T))\n        # rotation matrix from SVD orthonormal bases\n        R = np.dot(u, vh)\n        if np.linalg.det(R) < 0.0:\n            # R does not constitute right handed system\n            R -= np.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)\n            s[-1] *= -1.0\n        # homogeneous transformation matrix\n        M = np.identity(ndims + 1)\n        M[:ndims, :ndims] = R\n    else:\n        # Rigid transformation matrix via quaternion\n        # compute symmetric matrix N\n        xx, yy, zz = np.sum(v0 * v1, axis=1)\n        xy, yz, zx = np.sum(v0 * np.roll(v1, -1, axis=0), axis=1)\n        xz, yx, zy = np.sum(v0 * np.roll(v1, -2, axis=0), axis=1)\n        N = [\n            [xx + yy + zz, 0.0, 0.0, 0.0],\n            [yz - zy, xx - yy - zz, 0.0, 0.0],\n            [zx - xz, xy + yx, yy - xx - zz, 0.0],\n            [xy - yx, zx + xz, yz + zy, zz - xx - yy],\n        ]\n        # quaternion: eigenvector corresponding to most positive eigenvalue\n        w, V = np.linalg.eigh(N)\n        q = V[:, np.argmax(w)]\n        # print (vector_norm(q), np.linalg.norm(q))\n        q /= vector_norm(q)  # unit quaternion\n        # homogeneous transformation matrix\n        M = quaternion_matrix(q)\n\n    if scale and not shear:\n        # Affine transformation; scale is ratio of RMS deviations from centroid\n        v0 *= v0\n        v1 *= v1\n        M[:ndims, :ndims] *= math.sqrt(np.sum(v1) / np.sum(v0))\n\n    # move centroids back\n    M = np.dot(np.linalg.inv(M1), np.dot(M, M0))\n    M /= M[ndims, ndims]\n\n    # print(\"transformation matrix Python Script: \", M)\n\n    return M\n\n\n# This is the IMC 3D error metric code\ndef register_by_Horn(ev_coord, gt_coord, ransac_threshold, inl_cf, strict_cf):\n    '''Return the best similarity transforms transformation_matrix that registers 3D points pt_ev in <ev_coord> to\n    the corresponding ones pt_gt in <gt_coord> according to a RANSAC-like approach for each\n    threshold value th in <ransac_threshold>.\n\n    Given th, each triplet of 3D correspondences is examined if not already present as strict inlier,\n    a correspondence is a strict inlier if <strict_cf> * err_best < th, where err_best is the registration\n    error for the best model so far.\n    The minimal model given by the triplet is then refined using also its inliers if their total is greater\n    than <inl_cf> * ninl_best, where ninl_best is th number of inliers for the best model so far. Inliers\n    are 3D correspondences (pt_ev, pt_gt) for which the Euclidean distance |pt_gt-transformation_matrix*pt_ev| is less than th.'''\n\n    # remove invalid cameras, the index is returned\n    idx_cams = np.all(np.isfinite(ev_coord), axis=0)\n    ev_coord = ev_coord[:, idx_cams]\n    gt_coord = gt_coord[:, idx_cams]\n\n    # initialization\n    n = ev_coord.shape[1]\n    r = ransac_threshold.shape[0]\n    ransac_threshold = np.expand_dims(ransac_threshold, axis=0)\n    ransac_threshold2 = ransac_threshold ** 2\n    ev_coord_1 = np.vstack((ev_coord, np.ones(n)))\n\n    max_no_inl = np.zeros((1, r))\n    best_inl_err = np.full(r, np.Inf)\n    best_transf_matrix = np.zeros((r, 4, 4))\n    best_err = np.full((n, r), np.Inf)\n    strict_inl = np.full((n, r), False)\n    triplets_used = np.zeros((3, r))\n\n    # run on camera triplets\n    for ii in tqdm.tqdm(range(n - 2)):\n        for jj in range(ii + 1, n - 1):\n            for kk in range(jj + 1, n):\n                i = [ii, jj, kk]\n                triplets_used_now = np.full((n), False)\n                triplets_used_now[i] = True\n                # if both ii, jj, kk are strict inliers for the best current model just skip\n                if np.all(strict_inl[i]):\n                    continue\n                # get transformation transformation_matrix by Horn on the triplet camera center correspondences\n                transf_matrix = affine_matrix_from_points(ev_coord[:, i], gt_coord[:, i], usesvd=False)\n                # apply transformation transformation_matrix to test camera centres\n                rotranslated = np.matmul(transf_matrix[:3], ev_coord_1)\n                # compute error and inliers\n                err = np.sum((rotranslated - gt_coord) ** 2, axis=0)\n                inl = np.expand_dims(err, axis=1) < ransac_threshold2\n                no_inl = np.sum(inl, axis=0)\n                # if the number of inliers is close to that of the best model so far, go for refinement\n                to_ref = np.squeeze(((no_inl > 2) & (no_inl > max_no_inl * inl_cf)), axis=0)\n                for q in np.argwhere(to_ref):\n                    qq = q[0]\n                    if np.any(np.all((np.expand_dims(inl[:, qq], axis=1) == inl[:, :qq]), axis=0)):\n                        # already done for this set of inliers\n                        continue\n                    # get transformation transformation_matrix by Horn on the inlier camera center correspondences\n                    transf_matrix = affine_matrix_from_points(ev_coord[:, inl[:, qq]], gt_coord[:, inl[:, qq]])\n                    # apply transformation transformation_matrix to test camera centres\n                    rotranslated = np.matmul(transf_matrix[:3], ev_coord_1)\n                    # compute error and inliers\n                    err_ref = np.sum((rotranslated - gt_coord) ** 2, axis=0)\n                    err_ref_sum = np.sum(err_ref, axis=0)\n                    err_ref = np.expand_dims(err_ref, axis=1)\n                    inl_ref = err_ref < ransac_threshold2\n                    no_inl_ref = np.sum(inl_ref, axis=0)\n                    # update the model if better for each threshold\n                    to_update = np.squeeze(\n                        (no_inl_ref > max_no_inl) | ((no_inl_ref == max_no_inl) & (err_ref_sum < best_inl_err)), axis=0)\n                    if np.any(to_update):\n                        triplets_used[0, to_update] = ii\n                        triplets_used[1, to_update] = jj\n                        triplets_used[2, to_update] = kk\n                        max_no_inl[:, to_update] = no_inl_ref[to_update]\n                        best_err[:, to_update] = np.sqrt(err_ref)\n                        best_inl_err[to_update] = err_ref_sum\n                        strict_inl[:, to_update] = (best_err[:, to_update] < strict_cf * ransac_threshold[:, to_update])\n                        best_transf_matrix[to_update] = transf_matrix\n\n    for i in range(r):\n        print(f'Registered cameras {int(max_no_inl[0, i])}/{n} for threshold {ransac_threshold[0, i]}')\n\n    best_model = {\n        \"valid_cams\": idx_cams,\n        \"no_inl\": max_no_inl,\n        \"err\": best_err,\n        \"triplets_used\": triplets_used,\n        \"transf_matrix\": best_transf_matrix}\n    return best_model\n\n\n# mAA computation\ndef mAA_on_cameras(err, thresholds, n, skip_top_thresholds, to_dec=3):\n    '''mAA is the mean of mAA_i, where for each threshold th_i in <thresholds>, excluding the first <skip_top_thresholds values>,\n    mAA_i = max(0, sum(err_i < th_i) - <to_dec>) / (n - <to_dec>)\n    where <n> is the number of ground-truth cameras and err_i is the camera registration error for the best\n    registration corresponding to threshold th_i'''\n\n    aux = err[:, skip_top_thresholds:] < np.expand_dims(np.asarray(thresholds[skip_top_thresholds:]), axis=0)\n    return np.sum(np.maximum(np.sum(aux, axis=0) - to_dec, 0)) / (len(thresholds[skip_top_thresholds:]) * (n - to_dec))\n\n\n# import data - no error handling in case float(x) fails\ndef get_camera_centers_from_df(df):\n    out = {}\n    for row in df.iterrows():\n        row = row[1]\n        fname = row['image_path']\n        R = np.array([float(x) for x in (row['rotation_matrix'].split(';'))]).reshape(3, 3)\n        t = np.array([float(x) for x in (row['translation_vector'].split(';'))]).reshape(3)\n        center = -R.T @ t\n        out[fname] = center\n    return out\n\n\ndef evaluate_rec(gt_df, user_df, inl_cf=0.8, strict_cf=0.5, skip_top_thresholds=2, to_dec=3,\n                 thresholds=[0.005, 0.01, 0.02, 0.03, 0.04, 0.05, 0.1, 0.15, 0.2]):\n    ''' Register the <user_df> camera centers to the ground-truth <gt_df> camera centers and\n    return the corresponding mAA as the average percentage of registered camera threshold.\n\n    For each threshold value in <thresholds>, the best similarity transformation found which\n    maximizes the number of registered cameras is employed. A camera is marked as registered\n    if after the transformation its Euclidean distance to the corresponding ground-truth camera\n    center is less than the mentioned threshold. Current measurements are in meter.\n\n    Registration parameters:\n    <inl_cf> coefficient to activate registration refinement, set to 1 to refine a new model\n    only when it gives more inliers, to 0 to refine a new model always; high values increase\n    speed but decrease precision.\n    <strict_cf> threshold coefficient to define strict inliers for the best registration so far,\n    new minimal models made up of strict inliers are skipped. It can vary from 0 (slower) to\n    1 (faster); set to -1 to check exhaustively all the minimal model triplets.\n\n    mAA parameters:\n    <skip_top_thresholds> excluded lower thresholds in the mAA computation; in case of using\n    heuristics for the registration, i.e. inl_cf!=0 and strict_cf!=-1, best model for lower\n    threshold can be not the optimal, so skip them in the mAA computation.\n    <to_dec> excludes the minimal model cameras from the computation of the mAA. Given the\n    minimal model, i.e. three pairs of 3D correspondences, there is a high chance to register by\n    a similarity transformation at any threshold, so do not account for mAA'''\n\n    # get camera centers\n    ucameras = get_camera_centers_from_df(user_df)\n    gcameras = get_camera_centers_from_df(gt_df)\n\n    # the denominator for mAA ratio\n    m = gt_df.shape[0]\n\n    # get the image list to use\n    good_cams = []\n    for image_path in gcameras.keys():\n        if image_path in ucameras.keys():\n            good_cams.append(image_path)\n\n    # put corresponding camera centers into matrices\n    n = len(good_cams)\n    u_cameras = np.zeros((3, n))\n    g_cameras = np.zeros((3, n))\n\n    ii = 0\n    for i in good_cams:\n        u_cameras[:, ii] = ucameras[i]\n        g_cameras[:, ii] = gcameras[i]\n        ii += 1\n\n    # Horn camera centers registration, a different best model for each camera threshold\n    model = register_by_Horn(u_cameras, g_cameras, np.asarray(thresholds), inl_cf, strict_cf)\n\n    # transformation matrix\n    print(\"\\nTransformation matrix for maximum threshold\")\n    T = np.squeeze(model['transf_matrix'][-1])\n    print(T)\n\n    # mAA\n    mAA = mAA_on_cameras(model[\"err\"], thresholds, m, skip_top_thresholds, to_dec)\n    # print(f'mAA = {mAA * 100 : .2f}% considering {m} input cameras - {to_dec}')\n    return mAA\n\n\ndef score(solution: pd.DataFrame, submission: pd.DataFrame) -> float:\n    '''The metric is an mean average accuracy between solution and submission camera centers.\n    Prior to calculate the metric, a function performs exhaustive registration (like RANSAC, but\n    not random, considering all possible configurations) to align the user camera system to the GT'''\n\n    scenes = list(set(solution['dataset'].tolist()))\n    results_per_dataset = []\n    for dataset in scenes:\n        print(f\"\\n*** {dataset} ***\")\n        start = time.time()\n        gt_ds = solution[solution['dataset'] == dataset]\n        user_ds = submission[submission['dataset'] == dataset]\n        gt_ds = gt_ds.sort_values(by=['image_path'], ascending=True)\n        user_ds = user_ds.sort_values(by=['image_path'], ascending=True)\n        result = evaluate_rec(gt_ds, user_ds, inl_cf=0, strict_cf=-1, skip_top_thresholds=0, to_dec=3,\n                              thresholds=translation_thresholds_meters_dict[dataset])\n        end = time.time()\n        print(f\"\\nmAA: {result * 100}%\")\n        print(\"Running time: %s\" % (end - start))\n        results_per_dataset.append(result)\n    return float(np.array(results_per_dataset).mean())","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.245866Z","start_time":"2024-05-27T14:08:15.215605Z"},"lines_to_next_cell":1,"jupyter":{"source_hidden":true}},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Code for aligning camera poses","metadata":{}},{"cell_type":"code","source":"\ndef align_camera_poses_from_colmap(gt_colmap_model, user_colmap_model):\n    result = {\n        'image_name': [],\n        'gt_camera_center': [],\n        'user_camera_center': [],\n        'gt_camera_rotation': [],\n        'user_camera_rotation': [],\n        'user_camera_mtx': [],\n    }\n\n    # Efficiently match images\n    user_image_stems = {str(Path(user_image.name).stem): user_image for user_image_id, user_image in\n                        user_colmap_model.images.items()}\n\n    logging.debug(f\"Found {len(gt_colmap_model.images.items())} gt images and {len(user_image_stems.items())} user images\")\n\n    for gt_image_id, gt_image in gt_colmap_model.images.items():\n        gt_image_stem = str(Path(gt_image.name).stem)\n\n        # Find matching user image or use defaults\n        user_image = user_image_stems.get(gt_image_stem, None)\n\n        result['image_name'].append(gt_image.name)\n        result['gt_camera_center'].append(gt_image.cam_from_world.inverse().translation)\n        result['gt_camera_rotation'].append(gt_image.cam_from_world.inverse().rotation.matrix())\n\n        # Populate user camera data (defaults if not found)\n        if user_image:\n            result['user_camera_center'].append(user_image.cam_from_world.inverse().translation)\n            result['user_camera_rotation'].append(user_image.cam_from_world.inverse().rotation.matrix())\n            result['user_camera_mtx'].append(user_colmap_model.cameras[user_image.camera_id])\n        else:\n            result['user_camera_center'].append(np.zeros(3))  # Default center\n            result['user_camera_rotation'].append(np.eye(3))  # Default rotation (identity matrix)\n            result['user_camera_mtx'].append(None)  # No matching camera matrix\n\n    result['gt_camera_center'] = np.array(result['gt_camera_center']).T  # 3xn\n    result['user_camera_center'] = np.array(result['user_camera_center']).T  # 3xn\n\n    # Assertion for debugging (optional)\n    assert result['gt_camera_center'].shape == result['user_camera_center'].shape\n\n    return result\n\n\n\n\ndef create_transformation_matrix(rotation_vector, translation_vector):\n    \"\"\"\n    Create a 4x4 transformation matrix from a rotation vector and a translation vector.\n\n    Args:\n        rotation_vector (np.ndarray): A 3D rotation vector.\n        translation_vector (np.ndarray): A 3D translation vector.\n\n    Returns:\n        np.ndarray: A 4x4 transformation matrix.\n    \"\"\"\n    rotation_matrix = Rotation.from_rotvec(rotation_vector).as_matrix()\n    return create_matrix_from_rotation_and_translation(rotation_matrix, translation_vector)\n\n\ndef create_matrix_from_rotation_and_translation(rotation_matrix, translation_vector):\n    \"\"\"\n    Create a 4x4 transformation matrix from a rotation matrix and a translation vector.\n\n    Args:\n        rotation_matrix (np.ndarray): A 3x3 rotation matrix.\n        translation_vector (np.ndarray): A 3D translation vector.\n\n    Returns:\n        np.ndarray: A 4x4 transformation matrix.\n    \"\"\"\n    transformation_matrix = np.zeros((4, 4))\n    transformation_matrix[:3, :3] = rotation_matrix\n    transformation_matrix[:3, 3] = translation_vector\n    transformation_matrix[3, :] = [0, 0, 0, 1]\n    return transformation_matrix\n\ndef decompose_transformation_matrix(transformation_matrix):\n    \"\"\"\n    Decompose a 4x4 transformation matrix into a rotation matrix and a translation vector.\n\n    Args:\n        transformation_matrix (np.ndarray): A 4x4 transformation matrix.\n\n    Returns:\n        tuple: A tuple containing a 3x3 rotation matrix and a 3D translation vector.\n\n    Raises:\n        ValueError: If the transformation matrix is not valid.\n    \"\"\"\n    if transformation_matrix.shape != (4, 4) or not np.allclose(transformation_matrix[3, :], [0, 0, 0, 1]):\n        raise ValueError(\"Invalid transformation matrix\")\n\n    rotation_matrix = transformation_matrix[:3, :3]\n    translation_vector = transformation_matrix[:3, 3]\n\n    return rotation_matrix, translation_vector\n\ndef transform_colmap_camera_matrices(rotation_matrices, translation_vectors, transformation_matrix):\n    \"\"\"\n    Transform a list of rotation matrices and translation vectors using a given transformation matrix.\n\n    Args:\n        rotation_matrices (list of np.ndarray): A list of 3x3 rotation matrices.\n        translation_vectors (list of np.ndarray): A list of 3D translation vectors.\n        transformation_matrix (np.ndarray): A 4x4 transformation matrix.\n\n    Returns:\n        tuple: A tuple containing the transformed rotation matrices and translation vectors.\n    \"\"\"\n    transformed_rotation_matrices = []\n    transformed_translation_vectors = []\n    \n    for rotation_matrix, translation_vector in zip(rotation_matrices, translation_vectors):\n        translation_vector = translation_vector.squeeze()\n        camera_transformation_matrix = create_matrix_from_rotation_and_translation(rotation_matrix, translation_vector)\n        transformed_matrix = np.dot(transformation_matrix, camera_transformation_matrix)\n        transformed_rotation_matrix, transformed_translation_vector = decompose_transformation_matrix(transformed_matrix)\n        \n        transformed_rotation_matrices.append(transformed_rotation_matrix)\n        transformed_translation_vectors.append(transformed_translation_vector)\n\n    return np.array(transformed_translation_vectors), np.array(transformed_rotation_matrices)\n\n\nclass CameraAlignment:\n    def __init__(self, gt_colmap_reconstruction_model, colmap_reconstruction_model):\n        self.gt_model = gt_colmap_reconstruction_model\n        self.user_model = colmap_reconstruction_model\n        self.result = None\n        self.model = None\n        self.mAA = None\n        self.transformation_matrix = None\n\n    def align_camera_poses(self):\n        self.result = align_camera_poses_from_colmap(self.gt_model, self.user_model)\n\n    def register_by_Horn(self, thresholds, inl_cf, strict_cf):\n        self.model = register_by_Horn(ev_coord=self.result['user_camera_center'],\n                                      gt_coord=self.result['gt_camera_center'],\n                                      ransac_threshold=thresholds,\n                                      inl_cf=inl_cf,\n                                      strict_cf=strict_cf)\n\n    def calculate_mAA(self, thresholds, to_dec):\n        m = len(self.result['image_name'])\n        self.mAA = mAA_on_cameras(self.model[\"err\"], thresholds, n=m, skip_top_thresholds=2, to_dec=to_dec)\n        print(f'mAA = {self.mAA * 100 : .2f}% considering {m} input cameras - {to_dec}')\n\n    def print_transformation_matrix(self):\n        print(\"\\nTransformation matrix for maximum threshold\")\n        self.transformation_matrix = np.squeeze(self.model['transf_matrix'][-1])\n        print(self.transformation_matrix)\n\n","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.256219Z","start_time":"2024-05-27T14:08:15.246748Z"},"execution":{"iopub.execute_input":"2024-05-27T09:59:05.612301Z","iopub.status.busy":"2024-05-27T09:59:05.611931Z","iopub.status.idle":"2024-05-27T09:59:05.686952Z","shell.execute_reply":"2024-05-27T09:59:05.685809Z","shell.execute_reply.started":"2024-05-27T09:59:05.612264Z"},"jupyter":{"source_hidden":true}},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Utility code for camera pose and preimage points visualizations","metadata":{}},{"cell_type":"code","source":"\"\"\"\ncopied from: https://github.com/cvg/Hierarchical-Localization/blob/master/hloc/utils/viz_3d.py\n3D visualization based on plotly.\n\nWorks for a small number of points and cameras, might be slow otherwise.\n\n1) Initialize a figure with `init_figure`\n2) Add 3D points, camera frustums, or both as a pycolmap.Reconstruction\n\nWritten by Paul-Edouard Sarlin and Philipp Lindenberger.\n\"\"\"\n\n\ndef to_homogeneous(points):\n    pad = np.ones((points.shape[:-1] + (1,)), dtype=points.dtype)\n    return np.concatenate([points, pad], axis=-1)\n\n\ndef init_figure(height: int = 800) -> go.Figure:\n    \"\"\"Initialize a 3D figure.\"\"\"\n    fig = go.Figure()\n    axes = dict(\n        visible=False,\n        showbackground=False,\n        showgrid=False,\n        showline=False,\n        showticklabels=True,\n        autorange=True,\n    )\n    fig.update_layout(\n        template=\"plotly_dark\",\n        height=height,\n        scene_camera=dict(\n            eye=dict(x=0.0, y=-0.1, z=-2),\n            up=dict(x=0, y=-1.0, z=0),\n            projection=dict(type=\"orthographic\"),\n        ),\n        scene=dict(\n            xaxis=axes,\n            yaxis=axes,\n            zaxis=axes,\n            aspectmode=\"data\",\n            dragmode=\"orbit\",\n        ),\n        margin=dict(l=0, r=0, b=0, t=0, pad=0),\n        legend=dict(orientation=\"h\", yanchor=\"top\", y=0.99, xanchor=\"left\", x=0.1),\n    )\n    return fig\n\n\ndef plot_points(\n    fig: go.Figure,\n    pts: np.ndarray,\n    color: str = \"rgba(255, 0, 0, 1)\",\n    ps: int = 2,\n    colorscale: Optional[str] = None,\n    name: Optional[str] = None,\n):\n    \"\"\"Plot a set of 3D points.\"\"\"\n    x, y, z = pts.T\n    tr = go.Scatter3d(\n        x=x,\n        y=y,\n        z=z,\n        mode=\"markers\",\n        name=name,\n        legendgroup=name,\n        marker=dict(size=ps, color=color, line_width=0.0, colorscale=colorscale),\n    )\n    fig.add_trace(tr)\n\n\ndef plot_camera(\n    fig: go.Figure,\n    R: np.ndarray,\n    t: np.ndarray,\n    K: np.ndarray,\n    color: str = \"rgb(0, 0, 255)\",\n    name: Optional[str] = None,\n    legendgroup: Optional[str] = None,\n    fill: bool = False,\n    size: float = 1.0,\n    text: Optional[str] = None,\n):\n    \"\"\"Plot a camera frustum from pose and intrinsic matrix.\"\"\"\n    W, H = K[0, 2] * 2, K[1, 2] * 2\n    corners = np.array([[0, 0], [W, 0], [W, H], [0, H], [0, 0]])\n    if size is not None:\n        image_extent = max(size * W / 1024.0, size * H / 1024.0)\n        world_extent = max(W, H) / (K[0, 0] + K[1, 1]) / 0.5\n        scale = 0.5 * image_extent / world_extent\n    else:\n        scale = 1.0\n    corners = to_homogeneous(corners) @ np.linalg.inv(K).T\n    corners = (corners / 2 * scale) @ R.T + t\n    legendgroup = legendgroup if legendgroup is not None else name\n\n    x, y, z = np.concatenate(([t], corners)).T\n    i = [0, 0, 0, 0]\n    j = [1, 2, 3, 4]\n    k = [2, 3, 4, 1]\n\n    if fill:\n        pyramid = go.Mesh3d(\n            x=x,\n            y=y,\n            z=z,\n            color=color,\n            i=i,\n            j=j,\n            k=k,\n            legendgroup=legendgroup,\n            name=name,\n            showlegend=False,\n            hovertemplate=text.replace(\"\\n\", \"<br>\"),\n        )\n        fig.add_trace(pyramid)\n\n    triangles = np.vstack((i, j, k)).T\n    vertices = np.concatenate(([t], corners))\n    tri_points = np.array([vertices[i] for i in triangles.reshape(-1)])\n    x, y, z = tri_points.T\n\n    pyramid = go.Scatter3d(\n        x=x,\n        y=y,\n        z=z,\n        mode=\"lines\",\n        legendgroup=legendgroup,\n        name=name,\n        line=dict(color=color, width=1),\n        showlegend=False,\n        hovertemplate=text.replace(\"\\n\", \"<br>\"),\n    )\n    fig.add_trace(pyramid)\n\n\ndef plot_camera_colmap(\n    fig: go.Figure,\n    image: pycolmap.Image,\n    camera: pycolmap.Camera,\n    name: Optional[str] = None,\n    **kwargs\n):\n    \"\"\"Plot a camera frustum from PyCOLMAP objects\"\"\"\n    world_t_camera = image.cam_from_world.inverse()\n    plot_camera(\n        fig,\n        world_t_camera.rotation.matrix(),\n        world_t_camera.translation,\n        camera.calibration_matrix(),\n        name=name or str(image.image_id),\n        text=str(image),\n        **kwargs\n    )\n\n\ndef plot_cameras(fig: go.Figure, reconstruction: pycolmap.Reconstruction, **kwargs):\n    \"\"\"Plot a camera as a cone with camera frustum.\"\"\"\n    for image_id, image in reconstruction.images.items():\n        plot_camera_colmap(\n            fig, image, reconstruction.cameras[image.camera_id], **kwargs\n        )\n\n\ndef plot_reconstruction(\n    fig: go.Figure,\n    rec: pycolmap.Reconstruction,\n    max_reproj_error: float = 6.0,\n    color: str = \"rgb(0, 0, 255)\",\n    name: Optional[str] = None,\n    min_track_length: int = 2,\n    points: bool = True,\n    cameras: bool = True,\n    points_rgb: bool = True,\n    cs: float = 1.0,\n):\n    # Filter outliers\n    bbs = rec.compute_bounding_box(0.001, 0.999)\n    # Filter points, use original reproj error here\n    p3Ds = [\n        p3D\n        for _, p3D in rec.points3D.items()\n        if (\n            (p3D.xyz >= bbs[0]).all()\n            and (p3D.xyz <= bbs[1]).all()\n            and p3D.error <= max_reproj_error\n            and p3D.track.length() >= min_track_length\n        )\n    ]\n    xyzs = [p3D.xyz for p3D in p3Ds]\n    if points_rgb:\n        pcolor = [p3D.color for p3D in p3Ds]\n    else:\n        pcolor = color\n    if points:\n        plot_points(fig, np.array(xyzs), color=pcolor, ps=1, name=name)\n    if cameras:\n        plot_cameras(fig, rec, color=color, legendgroup=name, size=cs)\n\n","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.269Z","start_time":"2024-05-27T14:08:15.257553Z"},"execution":{"iopub.execute_input":"2024-05-27T09:59:05.689249Z","iopub.status.busy":"2024-05-27T09:59:05.688898Z","iopub.status.idle":"2024-05-27T09:59:05.718978Z","shell.execute_reply":"2024-05-27T09:59:05.717571Z","shell.execute_reply.started":"2024-05-27T09:59:05.68922Z"},"jupyter":{"source_hidden":true}},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Load ground truth and estimated camera poses and align them","metadata":{}},{"cell_type":"code","source":"\n\ndata_root = Path(\"/kaggle/input/\")\noutput_path = data_root.joinpath(Path(\n    \"image-matching-2024-hloc-baseline/outputs\"))\n\nimage_set_name = \"image-matching-challenge-2024/train/pond/\"\nexperiment_folder = data_root.joinpath(image_set_name)\n\noutputs = output_path.joinpath(image_set_name).joinpath(\"output\")\nground_truth_output = experiment_folder.joinpath(\"sfm\")\n\nimages = experiment_folder.joinpath(\"images\")\n\nestimated_colmap_model_path = Path(\"/kaggle/input/imc-understanding-the-baseline/.feature_outputs/church_church/colmap_rec_aliked/0/\")\n\ngt_colmap_model_path=Path(\"/kaggle/input/image-matching-challenge-2024/train/church/sfm\")\n\nestimated_colmap_reconstruction_model = pycolmap.Reconstruction(estimated_colmap_model_path)\n\ngt_colmap_reconstruction_model = pycolmap.Reconstruction(gt_colmap_model_path)\n\ncamera_alignment = CameraAlignment(gt_colmap_reconstruction_model, estimated_colmap_reconstruction_model)\ncamera_alignment.align_camera_poses()\n\nthresholds = np.array([0.005, 0.01, 0.02, 0.03, 0.04, 0.05, 0.1, 0.15, 0.2])\n\ncamera_alignment.register_by_Horn(thresholds, inl_cf=0.8, strict_cf=0.5)\n\ncamera_alignment.calculate_mAA(thresholds, to_dec=3)\ncamera_alignment.print_transformation_matrix()\n\ntransformed_camera_centers, transformed_camera_rotations = transform_colmap_camera_matrices(\n    rotation_matrices=camera_alignment.result['user_camera_rotation'], translation_vectors=camera_alignment.result['user_camera_center'].T,\n    transformation_matrix=camera_alignment.transformation_matrix)","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.284451Z","start_time":"2024-05-27T14:08:15.269649Z"},"lines_to_next_cell":2},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Plot ground truth camera poses","metadata":{}},{"cell_type":"code","source":"fig = init_figure()\n\nplot_cameras(fig, gt_colmap_reconstruction_model, color=\"rgb(255, 0, 255)\")\nfig.show()","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.285233Z","start_time":"2024-05-27T14:08:15.285180Z"}},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Plot estimated camera positions","metadata":{}},{"cell_type":"code","source":"fig = init_figure()\n\nplot_cameras(fig, estimated_colmap_reconstruction_model, color=\"rgb(255, 0, 255)\")\nfig.show()","metadata":{"ExecuteTime":{"end_time":"2024-05-27T14:08:15.285817Z","start_time":"2024-05-27T14:08:15.285760Z"},"lines_to_next_cell":2},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"We could plot the reconstruction results as well","metadata":{}},{"cell_type":"code","source":"fig = init_figure()\nplot_reconstruction(fig, estimated_colmap_reconstruction_model)\nfig.show()","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Transform the estimated camera poses and plot them with ground truth\nLet's plot the whole things together\n\n- magneta: ground truth\n- blue: estimated\n","metadata":{}},{"cell_type":"code","source":"\nfig = init_figure()\n\nfor idx in range(len(transformed_camera_centers)):\n    if camera_alignment.result['user_camera_mtx'][idx] is not None:\n        plot_camera(fig, transformed_camera_rotations[idx], transformed_camera_centers[idx],\n                camera_alignment.result['user_camera_mtx'][idx].calibration_matrix(),\n                name=str(idx), text=\"estimated pose: id {}\".format(camera_alignment.result['image_name'][idx]))\n\nplot_cameras(fig, gt_colmap_reconstruction_model, color=\"rgb(255, 0, 255)\")\nfig.show()","metadata":{"execution":{"iopub.execute_input":"2024-05-27T10:11:02.543647Z","iopub.status.busy":"2024-05-27T10:11:02.543229Z","iopub.status.idle":"2024-05-27T10:12:20.40586Z","shell.execute_reply":"2024-05-27T10:12:20.403758Z","shell.execute_reply.started":"2024-05-27T10:11:02.543617Z"},"lines_to_next_cell":2},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}