From 5e8bd79ca25f133492702f6b4001c0c2dd5dfa4c Mon Sep 17 00:00:00 2001 From: anastasiia-kornilova Date: Sat, 18 Jun 2022 19:37:09 +0300 Subject: [PATCH 1/3] Added notebook for basic processing: undistortion and depth reprojection --- Basic processing.ipynb | 196 ++++++++++++++++++++++++++++++++++++++++ utils/__init__.py | 0 utils/calib_io_utils.py | 47 ++++++++++ utils/camera.py | 22 +++++ utils/cv_utils.py | 41 +++++++++ 5 files changed, 306 insertions(+) create mode 100644 Basic processing.ipynb create mode 100644 utils/__init__.py create mode 100644 utils/calib_io_utils.py create mode 100644 utils/camera.py create mode 100644 utils/cv_utils.py diff --git a/Basic processing.ipynb b/Basic processing.ipynb new file mode 100644 index 0000000..62d40e9 --- /dev/null +++ b/Basic processing.ipynb @@ -0,0 +1,196 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "6da669a4-f30b-4d57-b340-c283502ab356", + "metadata": {}, + "source": [ + "### Basic processing of Samsung data from Azure cameras\n", + "\n", + "It includes undistortion of color and depth images and reprojection of depth image to undistrorted color virtual camera.\n", + "\n", + "Parameters to set: `SEQUENCE_PATH` - path for recorded sequence, `OUTPUT_DIR` - where postprocessed data will be located.\n", + "\n", + "**Attention**: calibration file `calib_params.yaml` for every camera should be located in original camera data directories." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "10d3a173-38a2-4b7a-abb4-82f2a85dc42d", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Jupyter environment detected. Enabling Open3D WebVisualizer.\n", + "[Open3D INFO] WebRTC GUI backend enabled.\n", + "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "INFO - 2022-06-18 19:32:50,469 - utils - Note: NumExpr detected 12 cores but \"NUMEXPR_MAX_THREADS\" not set, so enforcing safe limit of 8.\n", + "INFO - 2022-06-18 19:32:50,470 - utils - NumExpr defaulting to 8 threads.\n" + ] + } + ], + "source": [ + "import cv2\n", + "import imageio\n", + "import os\n", + "import shutil\n", + "\n", + "from tqdm import tqdm\n", + "\n", + "from utils.calib_io_utils import load_azure_params\n", + "from utils.cv_utils import undistort_image, reproject_depth" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "26e668be-ceda-4694-9514-a595d16f4cf6", + "metadata": {}, + "outputs": [], + "source": [ + "SEQUENCE_PATH = '/home/anasyasiia/Downloads/2022-04-16-12-15-47_round0/2022-04-16-12-15-47_round0/'\n", + "OUTPUT_DIR = 'output'" + ] + }, + { + "cell_type": "markdown", + "id": "2ccca8bf-0dcb-470f-a622-8dc60075de6c", + "metadata": {}, + "source": [ + "### List recorded cameras" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "e4862016-0023-4752-818e-2cf4707e399f", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "['3m', '5s']" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "camera_names = os.listdir(SEQUENCE_PATH)\n", + "camera_names" + ] + }, + { + "cell_type": "markdown", + "id": "1b394bb3-aab5-4209-80d8-68fbb71f8153", + "metadata": {}, + "source": [ + "### Prepare infrastructure for postprocessed data" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "3dc3e95d-26af-4aea-a2d5-b5489a6c5979", + "metadata": {}, + "outputs": [], + "source": [ + "dataset_basedir = os.path.basename(os.path.normpath(SEQUENCE_PATH))\n", + "postprocessed_data_dir = os.path.join(OUTPUT_DIR, dataset_basedir)\n", + "\n", + "if os.path.exists(postprocessed_data_dir):\n", + " shutil.rmtree(postprocessed_data_dir)\n", + " \n", + "os.mkdir(postprocessed_data_dir)\n", + "\n", + "for camera_name in camera_names:\n", + " os.mkdir(os.path.join(postprocessed_data_dir, camera_name))\n", + " os.mkdir(os.path.join(postprocessed_data_dir, camera_name, 'color'))\n", + " os.mkdir(os.path.join(postprocessed_data_dir, camera_name, 'depth'))" + ] + }, + { + "cell_type": "markdown", + "id": "b0556619-dc87-41d3-b6ba-fa78c5e41b91", + "metadata": {}, + "source": [ + "### Iterate over cameras and do undistortion and reprojection " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d7364fda-9a92-4608-a4e5-17e6888afa1f", + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + " 7%|▋ | 114/1699 [01:50<44:15, 1.68s/it]" + ] + } + ], + "source": [ + "for camera_name in camera_names:\n", + " camera_data_path = os.path.join(SEQUENCE_PATH, camera_name)\n", + " azure_calib_path = os.path.join(camera_data_path, 'calib_params.json')\n", + "\n", + " color_camera, depth_camera, T_depth2color = load_azure_params(azure_calib_path)\n", + "\n", + " color_images_dir = os.path.join(camera_data_path, 'color')\n", + " color_image_names = os.listdir(color_images_dir)\n", + " color_image_names.sort()\n", + " postprocessed_color_dir = os.path.join(postprocessed_data_dir, camera_name, 'color')\n", + "\n", + " for color_image_name in tqdm(color_image_names):\n", + " image = cv2.imread(os.path.join(color_images_dir, color_image_name), -1)\n", + " undistorted_image = undistort_image(image, color_camera)\n", + " imageio.imwrite(os.path.join(postprocessed_color_dir, color_image_name), image)\n", + "\n", + " depth_images_dir = os.path.join(camera_data_path, 'depth')\n", + " depth_image_names = os.listdir(depth_images_dir)\n", + " depth_image_names.sort()\n", + " postprocessed_depth_dir = os.path.join(postprocessed_data_dir, camera_names, 'depth')\n", + "\n", + " for depth_image_name in tqdm(depth_image_names):\n", + " image = cv2.imread(os.path.join(depth_images_dir, depth_image_name), -1)\n", + " undistorted_image = undistort_image(image, depth_camera)\n", + " reprojected_image = reproject_depth(undistorted_image, depth_camera, color_camera, T_depth2color)\n", + " imageio.imwrite(os.path.join(postprocessed_depth_dir, depth_image_name), reprojected_image)" + ] + } + ], + "metadata": { + "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.8.8" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/utils/__init__.py b/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/utils/calib_io_utils.py b/utils/calib_io_utils.py new file mode 100644 index 0000000..dcb2070 --- /dev/null +++ b/utils/calib_io_utils.py @@ -0,0 +1,47 @@ +import json +import numpy as np + +from utils.camera import Camera + +def __camera_intrinsics_from_coefs_dict(coefs_dict): + intrinsics_coefs_dict = coefs_dict['intrinsics']['parameters']['parameters_as_dict'] + K = np.eye(3) + K[0, 0] = intrinsics_coefs_dict['fx'] + K[1, 1] = intrinsics_coefs_dict['fy'] + K[0, 2] = intrinsics_coefs_dict['cx'] + K[1, 2] = intrinsics_coefs_dict['cy'] + + dist_coefs = np.asarray([ + intrinsics_coefs_dict['k1'], + intrinsics_coefs_dict['k2'], + intrinsics_coefs_dict['p1'], + intrinsics_coefs_dict['p1'], + intrinsics_coefs_dict['k3'], + intrinsics_coefs_dict['k4'], + intrinsics_coefs_dict['k5'], + intrinsics_coefs_dict['k6'] + ]) + + height = coefs_dict['resolution_height'] + width = coefs_dict['resolution_width'] + + return Camera(K, dist_coefs, height, width) + + +def load_azure_params(calib_params_path): + with open(calib_params_path, 'r') as calib_file: + calib_params = json.load(calib_file) + + color_intrinsics_dict = calib_params['color_camera'] + color_camera = __camera_intrinsics_from_coefs_dict(color_intrinsics_dict) + + depth_intrinsics_dict = calib_params['depth_camera'] + depth_camera = __camera_intrinsics_from_coefs_dict(depth_intrinsics_dict) + + R = np.array(calib_params['color_camera']['extrinsics']['rotation']).reshape(3, 3) + t = np.array(calib_params['color_camera']['extrinsics']['translation_in_meters']) + T_color2depth = np.eye(4) + T_color2depth[:3, :3] = R + T_color2depth[:3, 3] = t + + return color_camera, depth_camera, T_color2depth diff --git a/utils/camera.py b/utils/camera.py new file mode 100644 index 0000000..024025d --- /dev/null +++ b/utils/camera.py @@ -0,0 +1,22 @@ +import cv2 + +class Camera: + def __init__(self, K, dist_coefs, height, width): + self.K = K + self.dist_coefs = dist_coefs + self.height = height + self.width = width + self.shape = (self.height, self.width) + + self.K_undist, self.map_x_undist, self.map_y_undist = self.__get_undist_params() + + def __get_undist_params(self): + # OpenCV has its own view on shape style + shape = (self.width, self.height) + K_undist, _ = cv2.getOptimalNewCameraMatrix(self.K, self.dist_coefs, shape, 1, shape) + + map_x_undist, map_y_undist = cv2.initUndistortRectifyMap( + self.K, self.dist_coefs, None, K_undist, shape, cv2.CV_32FC1 + ) + + return K_undist, map_x_undist, map_y_undist diff --git a/utils/cv_utils.py b/utils/cv_utils.py new file mode 100644 index 0000000..708c05b --- /dev/null +++ b/utils/cv_utils.py @@ -0,0 +1,41 @@ +import cv2 +import numpy as np +import open3d as o3d + + +def __project_pc_to_depth(pcd, new_camera, scaling_factor=5000): + shape = new_camera.shape + I = np.zeros(shape, np.float32) + + points = np.asarray(pcd.points) + d = np.linalg.norm(points, axis=1) + normalized_points = points / np.expand_dims(points[:, 2], axis=1) + proj_pcd = np.round(new_camera.K_undist @ normalized_points.T).astype(np.int)[:2].T + + h, w = shape + proj_mask = (proj_pcd[:, 0] >= 0) & (proj_pcd[:, 0] < w) & (proj_pcd[:, 1] >= 0) & (proj_pcd[:, 1] < h) + proj_pcd = proj_pcd[proj_mask, :] + d = d[proj_mask] + + pcd_image = np.zeros(new_camera.shape) + pcd_image[proj_pcd[:, 1], proj_pcd[:, 0]] = d * scaling_factor + + return pcd_image + + +def reproject_depth(depth_undistorted, original_camera, new_camera, T_original2new): + image = o3d.geometry.Image(depth_undistorted) + + o3d_intrinsic = o3d.camera.PinholeCameraIntrinsic() + o3d_intrinsic.height, o3d_intrinsic.width = original_camera.shape + o3d_intrinsic.intrinsic_matrix = original_camera.K_undist + + pcd = o3d.geometry.PointCloud().create_from_depth_image(image, o3d_intrinsic) + pcd = pcd.transform(T_original2new) + + reprojected_depth = __project_pc_to_depth(pcd, new_camera) + + return reprojected_depth + +def undistort_image(image, camera): + return cv2.remap(image, camera.map_x_undist, camera.map_y_undist, cv2.INTER_NEAREST) From fb2722d63e51292cc2f458d5195434b8f2942ff6 Mon Sep 17 00:00:00 2001 From: anastasiia-kornilova Date: Mon, 20 Jun 2022 18:42:06 +0300 Subject: [PATCH 2/3] Added renaming to seconds format for images and fixed bug with saving depth as uint16 --- Basic processing.ipynb | 58 +++++++++--------------------------------- utils/cv_utils.py | 2 +- utils/io_utils.py | 2 ++ 3 files changed, 15 insertions(+), 47 deletions(-) create mode 100644 utils/io_utils.py diff --git a/Basic processing.ipynb b/Basic processing.ipynb index 62d40e9..fa44b95 100644 --- a/Basic processing.ipynb +++ b/Basic processing.ipynb @@ -16,28 +16,10 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "10d3a173-38a2-4b7a-abb4-82f2a85dc42d", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Jupyter environment detected. Enabling Open3D WebVisualizer.\n", - "[Open3D INFO] WebRTC GUI backend enabled.\n", - "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "INFO - 2022-06-18 19:32:50,469 - utils - Note: NumExpr detected 12 cores but \"NUMEXPR_MAX_THREADS\" not set, so enforcing safe limit of 8.\n", - "INFO - 2022-06-18 19:32:50,470 - utils - NumExpr defaulting to 8 threads.\n" - ] - } - ], + "outputs": [], "source": [ "import cv2\n", "import imageio\n", @@ -46,13 +28,14 @@ "\n", "from tqdm import tqdm\n", "\n", + "from utils.io_utils import filename_to_seconds\n", "from utils.calib_io_utils import load_azure_params\n", "from utils.cv_utils import undistort_image, reproject_depth" ] }, { "cell_type": "code", - "execution_count": 6, + "execution_count": null, "id": "26e668be-ceda-4694-9514-a595d16f4cf6", "metadata": {}, "outputs": [], @@ -71,21 +54,10 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": null, "id": "e4862016-0023-4752-818e-2cf4707e399f", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "['3m', '5s']" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "camera_names = os.listdir(SEQUENCE_PATH)\n", "camera_names" @@ -101,7 +73,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": null, "id": "3dc3e95d-26af-4aea-a2d5-b5489a6c5979", "metadata": {}, "outputs": [], @@ -133,15 +105,7 @@ "execution_count": null, "id": "d7364fda-9a92-4608-a4e5-17e6888afa1f", "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - " 7%|▋ | 114/1699 [01:50<44:15, 1.68s/it]" - ] - } - ], + "outputs": [], "source": [ "for camera_name in camera_names:\n", " camera_data_path = os.path.join(SEQUENCE_PATH, camera_name)\n", @@ -157,7 +121,8 @@ " for color_image_name in tqdm(color_image_names):\n", " image = cv2.imread(os.path.join(color_images_dir, color_image_name), -1)\n", " undistorted_image = undistort_image(image, color_camera)\n", - " imageio.imwrite(os.path.join(postprocessed_color_dir, color_image_name), image)\n", + " new_color_image_name = filename_to_seconds(color_image_name)\n", + " imageio.imwrite(os.path.join(postprocessed_color_dir, new_color_image_name), image)\n", "\n", " depth_images_dir = os.path.join(camera_data_path, 'depth')\n", " depth_image_names = os.listdir(depth_images_dir)\n", @@ -168,7 +133,8 @@ " image = cv2.imread(os.path.join(depth_images_dir, depth_image_name), -1)\n", " undistorted_image = undistort_image(image, depth_camera)\n", " reprojected_image = reproject_depth(undistorted_image, depth_camera, color_camera, T_depth2color)\n", - " imageio.imwrite(os.path.join(postprocessed_depth_dir, depth_image_name), reprojected_image)" + " new_depth_image_name = filename_to_seconds(depth_image_name)\n", + " imageio.imwrite(os.path.join(postprocessed_depth_dir, new_depth_image_name), reprojected_image)" ] } ], diff --git a/utils/cv_utils.py b/utils/cv_utils.py index 708c05b..81bdcf7 100644 --- a/utils/cv_utils.py +++ b/utils/cv_utils.py @@ -35,7 +35,7 @@ def reproject_depth(depth_undistorted, original_camera, new_camera, T_original2n reprojected_depth = __project_pc_to_depth(pcd, new_camera) - return reprojected_depth + return reprojected_depth.astype(np.uint16) def undistort_image(image, camera): return cv2.remap(image, camera.map_x_undist, camera.map_y_undist, cv2.INTER_NEAREST) diff --git a/utils/io_utils.py b/utils/io_utils.py new file mode 100644 index 0000000..0171f06 --- /dev/null +++ b/utils/io_utils.py @@ -0,0 +1,2 @@ +def filename_to_seconds(name_milliseconds): + return name_milliseconds[:6] + '.' + name_milliseconds[6:] \ No newline at end of file From f23eb2eb4251c126cf977fadec586e06870e9426 Mon Sep 17 00:00:00 2001 From: anastasiia-kornilova Date: Mon, 20 Jun 2022 18:44:27 +0300 Subject: [PATCH 3/3] Added notebook to generate all needed for ORB from basically processed data --- TUM conversion (one view).ipynb | 196 +++++++++++++++++++++++++++++++ configs/ORB_one_view_config.yaml | 71 +++++++++++ 2 files changed, 267 insertions(+) create mode 100644 TUM conversion (one view).ipynb create mode 100644 configs/ORB_one_view_config.yaml diff --git a/TUM conversion (one view).ipynb b/TUM conversion (one view).ipynb new file mode 100644 index 0000000..517a239 --- /dev/null +++ b/TUM conversion (one view).ipynb @@ -0,0 +1,196 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 96, + "id": "592949e2-2689-4e9c-90a2-ab08dede8d97", + "metadata": {}, + "outputs": [], + "source": [ + "ONE_CAMERA_SEQUENCE_PATH = 'output/2022-04-16-12-15-47_round0/3m/'" + ] + }, + { + "cell_type": "code", + "execution_count": 97, + "id": "888d1905-7246-4f64-a212-222554a4165f", + "metadata": {}, + "outputs": [], + "source": [ + "import cv2\n", + "import numpy as np\n", + "import os\n", + "from pathlib import Path" + ] + }, + { + "cell_type": "markdown", + "id": "4cb81d62-1c7c-43cf-be44-14ed4ce2c297", + "metadata": {}, + "source": [ + "### Assoсiate color and depth timestamps" + ] + }, + { + "cell_type": "code", + "execution_count": 88, + "id": "8ee3a09a-8851-4943-8055-f8028cf8155b", + "metadata": {}, + "outputs": [], + "source": [ + "def read_folder(path_to_folder: str):\n", + " \"\"\"\n", + " Reads all images' paths and timestamps from folder\n", + " :param path_to_folder: path to folder to read\n", + " :return: dictionary where keys are timestamps and values are path to image\n", + " \"\"\"\n", + " files = os.listdir(path_to_folder)\n", + " timestamps = [float(Path(file).stem) for file in files]\n", + " files = [os.path.join(os.path.basename(path_to_folder), x) for x in files]\n", + " timestamp_image_kvp = dict(zip(timestamps, files))\n", + " return timestamp_image_kvp\n", + "\n", + "\n", + "def associate(\n", + " color_images: dict, depth_images: dict, offset: float, max_difference: float\n", + ") -> list:\n", + " \"\"\"\n", + " Associates color and depth images\n", + " :param color_images: (timestamp, path) KVP for color images\n", + " :param depth_images: (timestamp, path) KVP for depth images\n", + " :param offset: time offset added to the timestamps of the depth images\n", + " :param max_difference: maximally allowed time difference for matching entries\n", + " :return: best matches for color and depth images\n", + " \"\"\"\n", + " first_keys = np.asarray(list(color_images.keys()))\n", + " second_keys = np.asarray(list(depth_images.keys()))\n", + " best_matches = list()\n", + " for timestamp in first_keys:\n", + " best_match = second_keys[np.argmin(np.abs(second_keys + offset - timestamp))]\n", + " if abs(best_match + offset - timestamp) < max_difference:\n", + " best_matches.append((timestamp, best_match))\n", + " return sorted(best_matches)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 102, + "id": "f8c9873f-ddd7-4db4-aaf3-564f0254d765", + "metadata": {}, + "outputs": [], + "source": [ + "def reformat_names_to_seconds(path):\n", + " names = os.listdir(path)\n", + " names.sort()\n", + "\n", + " for name in names:\n", + " new_name = name[:6] + '.' + name[6:]\n", + "\n", + " shutil.move(os.path.join(path, name), \n", + " os.path.join(path, new_name))\n", + " \n", + "color_path = 'output/2022-04-16-12-15-47_round0/3m/color/'\n", + "depth_path = 'output/2022-04-16-12-15-47_round0/3m/depth/'\n", + "# reformat_names_to_seconds(color_path)\n", + "reformat_names_to_seconds(depth_path)" + ] + }, + { + "cell_type": "code", + "execution_count": 103, + "id": "42290078-32fc-4063-bc4f-93675323205e", + "metadata": {}, + "outputs": [ + { + "ename": "ValueError", + "evalue": "could not convert string to float: '.ipynb.'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mValueError\u001b[0m Traceback (most recent call last)", + "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[0mfirst_list_M\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mread_folder\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mjoin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mONE_CAMERA_SEQUENCE_PATH\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'color'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m \u001b[0msecond_list_M\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mread_folder\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mjoin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mONE_CAMERA_SEQUENCE_PATH\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'depth'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 3\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mmatches_M\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0massociate\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfirst_list_M\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msecond_list_M\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1e-3\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n", + "\u001b[0;32m\u001b[0m in \u001b[0;36mread_folder\u001b[0;34m(path_to_folder)\u001b[0m\n\u001b[1;32m 6\u001b[0m \"\"\"\n\u001b[1;32m 7\u001b[0m \u001b[0mfiles\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlistdir\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpath_to_folder\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 8\u001b[0;31m \u001b[0mtimestamps\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mfloat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mPath\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfile\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstem\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mfile\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 9\u001b[0m \u001b[0mfiles\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mjoin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbasename\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpath_to_folder\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mx\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mx\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 10\u001b[0m \u001b[0mtimestamp_image_kvp\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdict\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mzip\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtimestamps\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", + "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m(.0)\u001b[0m\n\u001b[1;32m 6\u001b[0m \"\"\"\n\u001b[1;32m 7\u001b[0m \u001b[0mfiles\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlistdir\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpath_to_folder\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 8\u001b[0;31m \u001b[0mtimestamps\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mfloat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mPath\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfile\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstem\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mfile\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 9\u001b[0m \u001b[0mfiles\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mjoin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbasename\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpath_to_folder\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mx\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mx\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 10\u001b[0m \u001b[0mtimestamp_image_kvp\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdict\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mzip\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtimestamps\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mfiles\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", + "\u001b[0;31mValueError\u001b[0m: could not convert string to float: '.ipynb.'" + ] + } + ], + "source": [ + "first_list_M = read_folder(os.path.join(ONE_CAMERA_SEQUENCE_PATH, 'color'))\n", + "second_list_M = read_folder(os.path.join(ONE_CAMERA_SEQUENCE_PATH, 'depth'))\n", + "\n", + "matches_M = associate(first_list_M, second_list_M, 0, 1e-3)\n", + "\n", + "with open(os.path.join(ONE_CAMERA_SEQUENCE_PATH, 'associations.txt'), 'w') as f:\n", + " for (a, b) in matches_M:\n", + " f.write('{0} {1} {2} {3}\\n'.format(\n", + " a,\n", + " first_list_M[a],\n", + " b,\n", + " second_list_M[b]\n", + " ))" + ] + }, + { + "cell_type": "markdown", + "id": "c6e53f2e-ec3f-4970-bc37-8a6a0262b20f", + "metadata": {}, + "source": [ + "### Create ORB config" + ] + }, + { + "cell_type": "code", + "execution_count": 100, + "id": "f1fc972a-96f8-489c-903e-6855bb4dbe6a", + "metadata": {}, + "outputs": [], + "source": [ + "import shutil\n", + "\n", + "K_undist = np.load(os.path.join(ONE_CAMERA_SEQUENCE_PATH, 'calib_params.npy'))\n", + "\n", + "new_config_path = os.path.join(ONE_CAMERA_SEQUENCE_PATH, 'ORB_one_view_config.yaml')\n", + "shutil.copyfile('configs/ORB_one_view_config.yaml', new_config_path)\n", + "\n", + "with open(new_config_path, 'r') as f:\n", + " lines[10] = 'Camera1.fx: {}\\n'.format(K_undist[0, 0])\n", + " lines[11] = 'Camera1.fy: {}\\n'.format(K_undist[1, 1])\n", + " lines[12] = 'Camera1.cx: {}\\n'.format(K_undist[0, 2])\n", + " lines[13] = 'Camera1.cy: {}\\n'.format(K_undist[1, 2])\n", + "\n", + "with open(new_config_path, 'w') as f:\n", + " f.writelines(lines)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2fdc4b71-7da4-456d-912b-dbc4c48048a5", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "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.8.8" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/configs/ORB_one_view_config.yaml b/configs/ORB_one_view_config.yaml new file mode 100644 index 0000000..3022a21 --- /dev/null +++ b/configs/ORB_one_view_config.yaml @@ -0,0 +1,71 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera1.fx: 517.306408 +Camera1.fy: 516.469215 +Camera1.cx: 318.643040 +Camera1.cy: 255.313989 + +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 +Camera1.k3: 0.0 + +Camera.width: 1280 +Camera.height: 720 + +# Camera frames per second +Camera.fps: 5 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +Stereo.ThDepth: 40.0 +Stereo.b: 0.07732 + +# Depth map values factor +RGBD.DepthMapFactor: 5000.0 # 1.0 for ROS_bag + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500.0 +