diff --git a/.gitignore b/.gitignore index 3b4ca01..ca78eb4 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,13 @@ tools/_jupyter/.ipynb_checkpoints training validation testing +void-dataset +test_data +pretrained +trained_kbnet/ +setup/__pycache__ +trained_kbnet_orb_poses/ +trained_kbnet_orb_posenet_structureloss/ +training_results_orb/ +evaluation_results/ + diff --git a/Unsupervised Depth Completion with Calibrated Backprojection Layers.pdf b/Unsupervised Depth Completion with Calibrated Backprojection Layers.pdf new file mode 100644 index 0000000..f426764 Binary files /dev/null and b/Unsupervised Depth Completion with Calibrated Backprojection Layers.pdf differ diff --git a/bash/void/run_kbnet_orb.sh b/bash/void/run_kbnet_orb.sh new file mode 100644 index 0000000..4bcfa06 --- /dev/null +++ b/bash/void/run_kbnet_orb.sh @@ -0,0 +1,36 @@ +#!/bin/bash + +export CUDA_VISIBLE_DEVICES=0 + +python src/run_kbnet.py \ +--is_orb_data 1 \ +--to_scale_depth 0 \ +--image_path testing/orb/test_image.txt \ +--sparse_depth_path testing/orb/test_sparse_depth.txt \ +--intrinsics_path testing/orb/test_intrinsics.txt \ +--ground_truth_path testing/orb/test_ground_truth.txt \ +--input_channels_image 3 \ +--input_channels_depth 2 \ +--normalized_image_range 0 1 \ +--outlier_removal_kernel_size 7 \ +--outlier_removal_threshold 1.5 \ +--min_pool_sizes_sparse_to_dense_pool 15 17 19 \ +--max_pool_sizes_sparse_to_dense_pool 23 27 \ +--n_convolution_sparse_to_dense_pool 3 \ +--n_filter_sparse_to_dense_pool 8 \ +--n_filters_encoder_image 48 96 192 384 384 \ +--n_filters_encoder_depth 16 32 64 128 128 \ +--resolutions_backprojection 0 1 2 3 \ +--n_filters_decoder 256 128 128 64 12 \ +--deconv_type up \ +--weight_initializer xavier_normal \ +--activation_func leaky_relu \ +--min_predict_depth 0.01 \ +--max_predict_depth 8.0 \ +--min_evaluate_depth 0.02 \ +--max_evaluate_depth 10.0 \ +--save_outputs \ +--depth_model_restore_path orb_model.pth \ +--output_path \ +evaluation_results/orb/670/orb_trSmteB/ \ +--device gpu \ No newline at end of file diff --git a/bash/void/run_kbnet_void1500.sh b/bash/void/run_kbnet_void1500.sh index 9956def..3ca5d53 100644 --- a/bash/void/run_kbnet_void1500.sh +++ b/bash/void/run_kbnet_void1500.sh @@ -28,8 +28,7 @@ python src/run_kbnet.py \ --min_evaluate_depth 0.2 \ --max_evaluate_depth 5.0 \ --save_outputs \ ---depth_model_restore_path \ -pretrained_models/void/kbnet-void1500.pth \ +--depth_model_restore_path traine_with_poses.pth \ --output_path \ -pretrained_models/void/evaluation_results/void1500 \ ---device gpu +evaluation_results/void1500_poses \ +--device gpu \ No newline at end of file diff --git a/bash/void/train_kbnet_orb.sh b/bash/void/train_kbnet_orb.sh new file mode 100644 index 0000000..43e99ca --- /dev/null +++ b/bash/void/train_kbnet_orb.sh @@ -0,0 +1,59 @@ +#!/bin/bash + +export CUDA_VISIBLE_DEVICES=0 + +python src/train_kbnet.py \ +--train_image_path training/orb/train_image.txt \ +--train_sparse_depth_path training/orb/train_sparse_depth.txt \ +--train_pose_path training/orb/train_pose.txt \ +--train_intrinsics_path training/orb/train_intrinsics.txt \ +--val_image_path validation/orb/val_image.txt \ +--val_sparse_depth_path validation/orb/val_sparse_depth.txt \ +--val_intrinsics_path validation/orb/val_intrinsics.txt \ +--val_ground_truth_path validation/orb/val_ground_truth.txt \ +--is_orb_data 1 \ +--pose_in_world_frame 0 \ +--to_scale_depth 0 \ +--n_batch 4 \ +--n_height 480 \ +--n_width 848 \ +--input_channels_image 3 \ +--input_channels_depth 2 \ +--normalized_image_range 0 1 \ +--outlier_removal_kernel_size 7 \ +--outlier_removal_threshold 1.5 \ +--min_pool_sizes_sparse_to_dense_pool 25 27 29 \ +--max_pool_sizes_sparse_to_dense_pool 33 37 \ +--n_convolution_sparse_to_dense_pool 3 \ +--n_filter_sparse_to_dense_pool 8 \ +--n_filters_encoder_image 48 96 192 384 384 \ +--n_filters_encoder_depth 16 32 64 128 128 \ +--resolutions_backprojection 0 1 2 3 \ +--n_filters_decoder 256 128 128 64 12 \ +--deconv_type up \ +--min_predict_depth 0.01 \ +--max_predict_depth 8.0 \ +--weight_initializer xavier_normal \ +--activation_func leaky_relu \ +--learning_rates 1e-4 5e-5 \ +--learning_schedule 100 150 \ +--augmentation_probabilities 0.00 \ +--augmentation_schedule -1 \ +--augmentation_random_crop_type horizontal vertical anchored \ +--augmentation_random_remove_points 0.30 0.60 \ +--augmentation_random_noise_type none \ +--augmentation_random_noise_spread -1 \ +--w_color 0.15 \ +--w_structure 0.95 \ +--w_sparse_depth 2.0 \ +--w_smoothness 2.0 \ +--w_weight_decay_depth 0.00 \ +--w_weight_decay_pose 0.00 \ +--min_evaluate_depth 0.2 \ +--max_evaluate_depth 5.0 \ +--n_summary 1000 \ +--n_checkpoint 1000 \ +--validation_start 1000 \ +--checkpoint_path trained_kbnet/orb/kbnet_model \ +--device gpu \ +--n_thread 16 diff --git a/bash/void/train_kbnet_void1500.sh b/bash/void/train_kbnet_void1500.sh index a89ea99..737668c 100644 --- a/bash/void/train_kbnet_void1500.sh +++ b/bash/void/train_kbnet_void1500.sh @@ -6,10 +6,12 @@ python src/train_kbnet.py \ --train_image_path training/void/void_train_image_1500.txt \ --train_sparse_depth_path training/void/void_train_sparse_depth_1500.txt \ --train_intrinsics_path training/void/void_train_intrinsics_1500.txt \ +--train_pose_path training/void/void_train_pose_1500.txt \ --val_image_path testing/void/void_test_image_1500.txt \ --val_sparse_depth_path testing/void/void_test_sparse_depth_1500.txt \ --val_intrinsics_path testing/void/void_test_intrinsics_1500.txt \ --val_ground_truth_path testing/void/void_test_ground_truth_1500.txt \ +--pose_in_world_frame 0 \ --n_batch 8 \ --n_height 480 \ --n_width 640 \ @@ -52,4 +54,4 @@ python src/train_kbnet.py \ --validation_start 5000 \ --checkpoint_path trained_kbnet/void1500/kbnet_model \ --device gpu \ ---n_thread 8 +--n_thread 16 diff --git a/run_kbnet_void1500.sh b/run_kbnet_void1500.sh new file mode 100644 index 0000000..3aae403 --- /dev/null +++ b/run_kbnet_void1500.sh @@ -0,0 +1,35 @@ +#!/bin/bash + +export CUDA_VISIBLE_DEVICES=0 + +python src/run_kbnet.py \ +--image_path /run/user/1000/gvfs/sftp:host=data.ciirc.cvut.cz,user=madharak/nfs/datasets/SPRING/ARI/Depth\ Completion\ Datasets/void-dataset/data/void_release/void_1500/data/birthplace_of_internet/image/1552097915.5257.png \ +--sparse_depth_path /run/user/1000/gvfs/sftp:host=data.ciirc.cvut.cz,user=madharak/nfs/datasets/SPRING/ARI/Depth\ Completion\ Datasets/void-dataset/data/void_release/void_1500/data/birthplace_of_internet/sparse_depth/1552097915.5257.png \ +--intrinsics_path /run/user/1000/gvfs/sftp:host=data.ciirc.cvut.cz,user=madharak/nfs/datasets/SPRING/ARI/Depth\ Completion\ Datasets/void-dataset/data/void_release/void_1500/data/birthplace_of_internet/K.txt \ +--ground_truth_path /run/user/1000/gvfs/sftp:host=data.ciirc.cvut.cz,user=madharak/nfs/datasets/SPRING/ARI/Depth\ Completion\ Datasets/void-dataset/data/void_release/void_1500/data/birthplace_of_internet/ground_truth/1552097915.5257.png \ +--input_channels_image 3 \ +--input_channels_depth 2 \ +--normalized_image_range 0 1 \ +--outlier_removal_kernel_size 7 \ +--outlier_removal_threshold 1.5 \ +--min_pool_sizes_sparse_to_dense_pool 15 17 \ +--max_pool_sizes_sparse_to_dense_pool 23 27 29 \ +--n_convolution_sparse_to_dense_pool 3 \ +--n_filter_sparse_to_dense_pool 8 \ +--n_filters_encoder_image 48 96 192 384 384 \ +--n_filters_encoder_depth 16 32 64 128 128 \ +--resolutions_backprojection 0 1 2 3 \ +--n_filters_decoder 256 128 128 64 12 \ +--deconv_type up \ +--weight_initializer xavier_normal \ +--activation_func leaky_relu \ +--min_predict_depth 0.1 \ +--max_predict_depth 8.0 \ +--min_evaluate_depth 0.2 \ +--max_evaluate_depth 5.0 \ +--save_outputs \ +--depth_model_restore_path \ +pretrained/pretrained_models/void/kbnet-void1500.pth \ +--output_path \ +pretrained_models/void/evaluation_results/void1500 \ +--device gpu diff --git a/setup/get_training_data.py b/setup/get_training_data.py new file mode 100644 index 0000000..537b6a7 --- /dev/null +++ b/setup/get_training_data.py @@ -0,0 +1,64 @@ +import cv2 +import numpy as np +import glob +import os +import sys +from pathlib import Path +from sklearn.cluster import MiniBatchKMeans +sys.path.append('../') +import data_utils + +N_INIT_CORNER = 1500 + +outer_folder = Path(sys.argv[1]) +d435_image_folder = Path(str(outer_folder)+'/d435_color') +color_image_list = glob.glob(str(d435_image_folder)+"*.png") + +validity_map_folder = Path(str(outer_folder)+'/validity_map') +validity_map_folder.mkdir(parents=True, exist_ok=True) + +sparse_depth_folder = Path(str(outer_folder)+'/sparse_depth') +sparse_depth_folder.mkdir(parents=True, exist_ok=True) + + +depth_folder = Path(str(outer_folder) + '/d435_depth') +depth_image_list = glob.glob(str(depth_folder)+"*.png") + +assert len(color_image_list) == len(depth_image_list), "Not same images for depth and color" + +for it,image_name in enumerate(color_image_list): + sparse_depth_filename_without_ext = Path(image_name).stem + sparse_depth_filename = str(sparse_depth_folder) + '/' + sparse_depth_filename_without_ext + '.png' + + image = cv2.imread(image_name, cv2.IMREAD_COLOR) + depth_image = data_utils.load_depth(depth_image_list[it]) + sparse_depth = np.zeros_like(depth_image) + + corners = cv2.cornerHarris(image, blockSize=5, ksize=3, k=0.04) + corners = corners.ravel() + corner_locs = np.argsort(corners)[0:N_INIT_CORNER] + + corner_map = np.zeros_like(corners) + corner_map[corner_locs] = 1 + + corner_locs = np.unravel_index(corner_locs, (image.shape[0], image.shape[1])) + corner_locs = np.transpose(np.array([corner_locs[0], corner_locs[1]])) + + + kmeans = MiniBatchKMeans( + n_clusters=384, + max_iter=2, + n_init=1, + init_size=None, + random_state=1, + reassignment_ratio=1e-11) + + kmeans.fit(corner_locs) + corner_locs = kmeans.cluster_centers_.astype(np.uint16) + + validity_map = np.zeros_like(image).astype(np.int16) + validity_map[corner_locs[:, 0], corner_locs[:, 1]] = 1 + + sparse_depth[np.where(validity_map==1)] = dense_depth[np.where(validity_map==1)] + data_utils.save_depth(sparse_depth, sparse_depth_filename) + diff --git a/setup/new_setup_captured_orb_data.py b/setup/new_setup_captured_orb_data.py new file mode 100644 index 0000000..b7ae48a --- /dev/null +++ b/setup/new_setup_captured_orb_data.py @@ -0,0 +1,698 @@ +''' +Author: Alex Wong + +If you use this code, please cite the following paper: + +A. Wong, and S. Soatto. Unsupervised Depth Completion with Calibrated Backprojection Layers. +https://arxiv.org/pdf/2108.10531.pdf + +@inproceedings{wong2021unsupervised, + title={Unsupervised Depth Completion with Calibrated Backprojection Layers}, + author={Wong, Alex and Soatto, Stefano}, + booktitle={Proceedings of the IEEE/CVF International Conference on Computer Vision}, + pages={12747--12756}, + year={2021} +} +''' +import os, sys, glob, argparse +import multiprocessing as mp +import numpy as np +import cv2 +sys.path.insert(0,'src') +from pathlib import Path as path +import data_utils +from tqdm import tqdm +from natsort import natsorted + +paths_only = False +skip = 5 + +def create_imagesPaths_files(DATA_DIRPATH, train_fileNames, test_fileNames, val_fileNames): + train_image_fileName, train_pose_fileName, train_sparse_depth_fileName, train_validity_map_fileName, train_ground_truth_fileName, train_intrinsics_fileName = train_fileNames + test_image_fileName, test_pose_fileName, test_sparse_depth_fileName, test_validity_map_fileName, test_ground_truth_fileName, test_intrinsics_fileName = test_fileNames + val_image_fileName, val_sparse_depth_fileName, val_validity_map_fileName, val_ground_truth_fileName, val_intrinsics_fileName = val_fileNames + + train_image_filepath = os.path.join(DATA_DIRPATH, train_image_fileName) + train_pose_filepath = os.path.join(DATA_DIRPATH, train_pose_fileName) + train_sparse_depth_filepath = os.path.join(DATA_DIRPATH, train_sparse_depth_fileName) + train_validity_map_filepath = os.path.join(DATA_DIRPATH, train_validity_map_fileName) + train_ground_truth_filepath = os.path.join(DATA_DIRPATH, train_ground_truth_fileName) + train_intrinsics_filepath = os.path.join(DATA_DIRPATH, train_intrinsics_fileName) + + test_image_filepath = os.path.join(DATA_DIRPATH, test_image_fileName) + test_pose_filepath = os.path.join(DATA_DIRPATH, test_pose_fileName) + test_sparse_depth_filepath = os.path.join(DATA_DIRPATH, test_sparse_depth_fileName) + test_validity_map_filepath = os.path.join(DATA_DIRPATH, test_validity_map_fileName) + test_ground_truth_filepath = os.path.join(DATA_DIRPATH, test_ground_truth_fileName) + test_intrinsics_filepath = os.path.join(DATA_DIRPATH, test_intrinsics_fileName) + + val_image_filepath = os.path.join(DATA_DIRPATH, val_image_fileName) + val_sparse_depth_filepath = os.path.join(DATA_DIRPATH, val_sparse_depth_fileName) + val_validity_map_filepath = os.path.join(DATA_DIRPATH, val_validity_map_fileName) + val_ground_truth_filepath = os.path.join(DATA_DIRPATH, val_ground_truth_fileName) + val_intrinsics_filepath = os.path.join(DATA_DIRPATH, val_intrinsics_fileName) + + train_imageF = open(train_image_filepath, "w") + train_poseF = open(train_pose_filepath, "w") + train_sparseF = open(train_sparse_depth_filepath, "w") + train_valF = open(train_validity_map_filepath, "w") + train_gtF = open(train_ground_truth_filepath, "w") + train_KF = open(train_intrinsics_filepath, "w") + + test_imageF = open(test_image_filepath, "w") + test_poseF = open(test_pose_filepath, "w") + test_sparseF = open(test_sparse_depth_filepath, "w") + test_valF = open(test_validity_map_filepath, "w") + test_gtF = open(test_ground_truth_filepath, "w") + test_KF = open(test_intrinsics_filepath, "w") + + val_imageF = open(val_image_filepath, "w") + val_sparseF = open(val_sparse_depth_filepath, "w") + val_valF = open(val_validity_map_filepath, "w") + val_gtF = open(val_ground_truth_filepath, "w") + val_KF = open(val_intrinsics_filepath, "w") + + images_folder = os.path.join(DATA_DIRPATH, "images") + poses_folder = os.path.join(DATA_DIRPATH, "poses") + depths_folder = os.path.join(DATA_DIRPATH, "depths") + val_folder = os.path.join(DATA_DIRPATH, "validity_maps") + gt_folder = os.path.join(DATA_DIRPATH, "raw_depths") + + numImages = len([i for i in os.listdir(images_folder)]) + train_imagePaths = [] + train_posePaths = [] + train_sparsePaths = [] + train_valPaths = [] + train_GTPaths = [] + train_intrinsicPaths = [] + + test_imagePaths = [] + test_posePaths = [] + test_sparsePaths = [] + test_valPaths = [] + test_GTPaths = [] + test_intrinsicPaths = [] + + val_imagePaths = [] + val_posePaths = [] + val_sparsePaths = [] + val_valPaths = [] + val_GTPaths = [] + val_intrinsicPaths = [] + + val_indices = np.random.choice(numImages, int(0.1*numImages), replace=False) + for i in range(numImages): + if i< skip: + continue + # if i in training_indices: + train_imagePaths.append( os.path.join(images_folder, "keyframe"+str(i)+".png\n") ) + train_posePaths.append( os.path.join(poses_folder, "pose"+str(i)+".txt\n") ) + train_sparsePaths.append( os.path.join(depths_folder, "depth"+str(i)+".yml\n") ) + train_valPaths.append( os.path.join(val_folder, "validity_map"+str(i)+".png\n") ) + train_GTPaths.append( os.path.join(gt_folder, "rawDepth"+str(i)+".yml\n") ) + train_intrinsicPaths.append( os.path.join(DATA_DIRPATH, "intrinsics.yml\n") ) + + test_imagePaths.append(os.path.join(images_folder, "keyframe"+str(i)+".png\n")) + test_posePaths.append( os.path.join(poses_folder, "pose"+str(i)+".txt\n") ) + test_sparsePaths.append( os.path.join(depths_folder, "depth"+str(i)+".yml\n") ) + test_valPaths.append( os.path.join(val_folder, "validity_map"+str(i)+".png\n") ) + test_GTPaths.append( os.path.join(gt_folder, "rawDepth"+str(i)+".yml\n") ) + test_intrinsicPaths.append( os.path.join(DATA_DIRPATH, "intrinsics.yml\n") ) + + if i in val_indices: + val_imagePaths.append(os.path.join(images_folder, "keyframe"+str(i)+".png\n")) + val_sparsePaths.append( os.path.join(depths_folder, "depth"+str(i)+".yml\n") ) + val_valPaths.append( os.path.join(val_folder, "validity_map"+str(i)+".png\n") ) + val_GTPaths.append( os.path.join(gt_folder, "rawDepth"+str(i)+".yml\n") ) + val_intrinsicPaths.append( os.path.join(DATA_DIRPATH, "intrinsics.yml\n") ) + + train_imageF.writelines(train_imagePaths) + train_poseF.writelines(train_posePaths) + train_sparseF.writelines(train_sparsePaths) + train_valF.writelines(train_valPaths) + train_gtF.writelines(train_GTPaths) + train_KF.writelines(train_intrinsicPaths) + + test_imageF.writelines(test_imagePaths) + test_poseF.writelines(test_posePaths) + test_sparseF.writelines(test_sparsePaths) + test_valF.writelines(test_valPaths) + test_gtF.writelines(test_GTPaths) + test_KF.writelines(test_intrinsicPaths) + + val_imageF.writelines(val_imagePaths) + val_sparseF.writelines(val_sparsePaths) + val_valF.writelines(val_valPaths) + val_gtF.writelines(val_GTPaths) + val_KF.writelines(val_intrinsicPaths) + + train_imageF.close() + train_poseF.close() + train_sparseF.close() + train_valF.close() + train_gtF.close() + train_KF.close() + test_imageF.close() + test_poseF.close() + test_sparseF.close() + test_valF.close() + test_gtF.close() + test_KF.close() + val_imageF.close() + val_sparseF.close() + val_valF.close() + val_gtF.close() + val_KF.close() + + +DATA_DIRPATH = sys.argv[1] +OUTPUT_DIRPATH = sys.argv[2] + +TRAIN_IMAGE_FILENAME = 'train_image.txt' +TRAIN_POSE_FILENAME = 'train_pose.txt' +TRAIN_SPARSE_DEPTH_FILENAME = 'train_sparse_depth.txt' +TRAIN_VALIDITY_MAP_FILENAME = 'train_validity_map.txt' +TRAIN_GROUND_TRUTH_FILENAME = 'train_ground_truth.txt' +TRAIN_INTRINSICS_FILENAME = 'train_intrinsics.txt' +TEST_IMAGE_FILENAME = 'test_image.txt' +TEST_POSE_FILENAME = 'test_pose.txt' +TEST_SPARSE_DEPTH_FILENAME = 'test_sparse_depth.txt' +TEST_VALIDITY_MAP_FILENAME = 'test_validity_map.txt' +TEST_GROUND_TRUTH_FILENAME = 'test_ground_truth.txt' +TEST_INTRINSICS_FILENAME = 'test_intrinsics.txt' +VAL_IMAGE_FILENAME = 'val_image.txt' +VAL_SPARSE_DEPTH_FILENAME = 'val_sparse_depth.txt' +VAL_VALIDITY_MAP_FILENAME = 'val_validity_map.txt' +VAL_GROUND_TRUTH_FILENAME = 'val_ground_truth.txt' +VAL_INTRINSICS_FILENAME = 'val_intrinsics.txt' + + + +TRAIN_REFS_DIRPATH = os.path.join('training', 'orb') +TEST_REFS_DIRPATH = os.path.join('testing', 'orb') +VAL_REFS_DIRPATH = os.path.join('validation', 'orb') + + +# ORB training set +TRAIN_IMAGE_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_image.txt') +TRAIN_POSE_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_pose.txt') +TRAIN_SPARSE_DEPTH_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_sparse_depth.txt') +TRAIN_VALIDITY_MAP_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_validity_map.txt') +TRAIN_GROUND_TRUTH_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_ground_truth.txt') +TRAIN_INTRINSICS_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_intrinsics.txt') +# ORB testing +TEST_IMAGE_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_image.txt') +TEST_POSE_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_pose.txt') +TEST_SPARSE_DEPTH_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_sparse_depth.txt') +TEST_VALIDITY_MAP_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_validity_map.txt') +TEST_GROUND_TRUTH_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_ground_truth.txt') +TEST_INTRINSICS_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_intrinsics.txt') +# ORB validation +VAL_IMAGE_FILEPATH = os.path.join(VAL_REFS_DIRPATH, 'val_image.txt') +VAL_SPARSE_DEPTH_FILEPATH = os.path.join(VAL_REFS_DIRPATH, 'val_sparse_depth.txt') +VAL_VALIDITY_MAP_FILEPATH = os.path.join(VAL_REFS_DIRPATH, 'val_validity_map.txt') +VAL_GROUND_TRUTH_FILEPATH = os.path.join(VAL_REFS_DIRPATH, 'val_ground_truth.txt') +VAL_INTRINSICS_FILEPATH = os.path.join(VAL_REFS_DIRPATH, 'val_intrinsics.txt') + + +training_names = [TRAIN_IMAGE_FILENAME, TRAIN_POSE_FILENAME, TRAIN_SPARSE_DEPTH_FILENAME, TRAIN_VALIDITY_MAP_FILENAME, TRAIN_GROUND_TRUTH_FILENAME, TRAIN_INTRINSICS_FILENAME] +testing_names = [TEST_IMAGE_FILENAME, TEST_POSE_FILENAME, TEST_SPARSE_DEPTH_FILENAME, TEST_VALIDITY_MAP_FILENAME, TEST_GROUND_TRUTH_FILENAME, TEST_INTRINSICS_FILENAME] +val_names = [VAL_IMAGE_FILENAME, VAL_SPARSE_DEPTH_FILENAME, VAL_VALIDITY_MAP_FILENAME, VAL_GROUND_TRUTH_FILENAME, VAL_INTRINSICS_FILENAME] +create_imagesPaths_files(DATA_DIRPATH, training_names,testing_names, val_names) + +def process_frame(inputs): + ''' + Processes a single frame + + Arg(s): + inputs : tuple + image path at time t=0, + image path at time t=1, + image path at time t=-1, + pose path at time t=0 + pose path at time t=1 + pose path at time t=-1 + sparse depth path at time t=0, + validity map path at time t=0, + ground truth path at time t=0, + boolean flag if set then create paths only + Returns: + str : image reference directory path + str : output concatenated image path at time t=0 + str : output sparse depth path at time t=0 + str : output validity map path at time t=0 + str : output ground truth path at time t=0 + ''' + + image_path1, \ + image_path0, \ + image_path2, \ + pose_path1, \ + pose_path0, \ + pose_path2, \ + sparse_depth_path, \ + validity_map_path, \ + ground_truth_path, \ + paths_only = inputs + + if not paths_only: + # Create image composite of triplets + image1 = cv2.imread(image_path1) + image0 = cv2.imread(image_path0) + image2 = cv2.imread(image_path2) + imagec = np.concatenate([image1, image0, image2], axis=1) + # Store poses of image triplet + + if path(pose_path0).suffix.lower()==".txt": + pose1 = np.loadtxt(pose_path1) + pose0 = np.loadtxt(pose_path0) + pose2 = np.loadtxt(pose_path2) + posec = np.stack((pose1, pose0, pose2)) + posec = posec.reshape(posec.shape[0],-1) + elif path(pose_path0).suffix.lower()==".yml": + s = cv2.FileStorage() + _ = s.open(pose_path1, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose1 = pnode.mat() + _ = s.open(pose_path0, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose0 = pnode.mat() + _ = s.open(pose_path2, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose2 = pnode.mat() + posec = np.stack((pose1, pose0, pose2)) + posec = posec.reshape(posec.shape[0],-1) + + # Get validity map + sparse_depth, validity_map = data_utils.load_depth_with_validity_map(sparse_depth_path, file_format='yml') + + # image_refpath = os.path.join(*image_path0.split(os.sep)[8:]) + # pose_refpath = os.path.join(*pose_path0.split(os.sep)[8:]) + imageName = path(image_path0).name + poseName = path(pose_path0).name.replace("yml", "txt") + + # Set output paths + image_outpath = path.joinpath(path(OUTPUT_DIRPATH), "images", imageName) + pose_outpath = path.joinpath(path(OUTPUT_DIRPATH), "poses", poseName) + sparse_depth_outpath = sparse_depth_path + validity_map_outpath = validity_map_path + ground_truth_outpath = ground_truth_path + + # Verify that all filenames match + image_filename = imageName + pose_filename = poseName + sparse_depth_filename = path(sparse_depth_outpath).name + validity_map_filename = path(validity_map_outpath).name + ground_truth_filename = path(ground_truth_outpath).name + assert "".join(filter(str.isdigit, path(image_filename).name.replace(path(image_filename).suffix, " ")) ) == "".join(filter(str.isdigit, path(pose_filename).name.replace(path(pose_filename).suffix, " ")) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, sparse_depth_filename) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, validity_map_filename) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, ground_truth_filename) ) + + if not paths_only: + cv2.imwrite(str(image_outpath), imagec) + np.savetxt(str(pose_outpath), posec) + return (imageName, + image_outpath, + poseName, + pose_outpath, + sparse_depth_outpath, + validity_map_outpath, + ground_truth_outpath) + + +# parser = argparse.ArgumentParser() +# parser.add_argument('--paths_only', action='store_true') + +# args = parser.parse_args() + +data_dirpaths = [ + DATA_DIRPATH +] + +train_output_filepaths = [ + [ + TRAIN_IMAGE_FILEPATH, + TRAIN_POSE_FILEPATH, + TRAIN_SPARSE_DEPTH_FILEPATH, + TRAIN_VALIDITY_MAP_FILEPATH, + TRAIN_GROUND_TRUTH_FILEPATH, + TRAIN_INTRINSICS_FILEPATH + ] +] +test_output_filepaths = [ + [ + TEST_IMAGE_FILEPATH, + TEST_POSE_FILEPATH, + TEST_SPARSE_DEPTH_FILEPATH, + TEST_VALIDITY_MAP_FILEPATH, + TEST_GROUND_TRUTH_FILEPATH, + TEST_INTRINSICS_FILEPATH + ] +] + +val_output_filepaths = [ + [ + VAL_IMAGE_FILEPATH, + VAL_SPARSE_DEPTH_FILEPATH, + VAL_VALIDITY_MAP_FILEPATH, + VAL_GROUND_TRUTH_FILEPATH, + VAL_INTRINSICS_FILEPATH + ] +] + + + +for dirpath in tqdm([TRAIN_REFS_DIRPATH, TEST_REFS_DIRPATH, VAL_REFS_DIRPATH]): + if not os.path.exists(dirpath): + os.makedirs(dirpath) + +data_filepaths = \ + zip(data_dirpaths, train_output_filepaths, test_output_filepaths, val_output_filepaths) + +for data_dirpath, train_filepaths, test_filepaths, val_filepaths in tqdm(data_filepaths): + # Training set + train_image_filepath = os.path.join(data_dirpath, TRAIN_IMAGE_FILENAME) + train_pose_filepath = os.path.join(data_dirpath, TRAIN_POSE_FILENAME) + train_sparse_depth_filepath = os.path.join(data_dirpath, TRAIN_SPARSE_DEPTH_FILENAME) + train_validity_map_filepath = os.path.join(data_dirpath, TRAIN_VALIDITY_MAP_FILENAME) + train_ground_truth_filepath = os.path.join(data_dirpath, TRAIN_GROUND_TRUTH_FILENAME) + train_intrinsics_filepath = os.path.join(data_dirpath, TRAIN_INTRINSICS_FILENAME) + + # Read training paths + train_image_paths = data_utils.read_paths(train_image_filepath) + # combined_trainImage = '\t'.join(train_image_paths) + train_pose_paths = data_utils.read_paths(train_pose_filepath) + # combined_trainPose = '\t'.join(train_pose_paths) + train_sparse_depth_paths = data_utils.read_paths(train_sparse_depth_filepath) + # combined_trainSparse = '\t'.join(train_sparse_depth_paths) + train_validity_map_paths = data_utils.read_paths(train_validity_map_filepath) + # combined_trainVal = '\t'.join(train_validity_map_paths) + train_ground_truth_paths = data_utils.read_paths(train_ground_truth_filepath) + # combined_trainGT = '\t'.join(train_ground_truth_paths) + train_intrinsics_paths = data_utils.read_paths(train_intrinsics_filepath) + # combined_trainIn = '\t'.join(train_intrinsics_paths) + + assert len(train_image_paths) == len(train_pose_paths) + assert len(train_image_paths) == len(train_sparse_depth_paths) + assert len(train_image_paths) == len(train_validity_map_paths) + assert len(train_image_paths) == len(train_ground_truth_paths) + assert len(train_image_paths) == len(train_intrinsics_paths) + + # Testing set + test_image_filepath = os.path.join(data_dirpath, TEST_IMAGE_FILENAME) + test_pose_filepath = os.path.join(data_dirpath, TEST_POSE_FILENAME) + test_sparse_depth_filepath = os.path.join(data_dirpath, TEST_SPARSE_DEPTH_FILENAME) + test_validity_map_filepath = os.path.join(data_dirpath, TEST_VALIDITY_MAP_FILENAME) + test_ground_truth_filepath = os.path.join(data_dirpath, TEST_GROUND_TRUTH_FILENAME) + test_intrinsics_filepath = os.path.join(data_dirpath, TEST_INTRINSICS_FILENAME) + + # Read testing paths + test_image_paths = data_utils.read_paths(test_image_filepath) + test_pose_paths = data_utils.read_paths(test_pose_filepath) + test_sparse_depth_paths = data_utils.read_paths(test_sparse_depth_filepath) + test_validity_map_paths = data_utils.read_paths(test_validity_map_filepath) + test_ground_truth_paths = data_utils.read_paths(test_ground_truth_filepath) + test_intrinsics_paths = data_utils.read_paths(test_intrinsics_filepath) + + assert len(test_image_paths) == len(test_pose_paths) + assert len(test_image_paths) == len(test_sparse_depth_paths) + assert len(test_image_paths) == len(test_validity_map_paths) + assert len(test_image_paths) == len(test_ground_truth_paths) + assert len(test_image_paths) == len(test_intrinsics_paths) + + # Get test set directories + test_seq_dirpaths = set( + [test_image_paths[idx].split(os.sep)[-3] for idx in range(len(test_image_paths))]) + + # Validation set + val_image_filepath = os.path.join(data_dirpath, VAL_IMAGE_FILENAME) + val_sparse_depth_filepath = os.path.join(data_dirpath, VAL_SPARSE_DEPTH_FILENAME) + val_validity_map_filepath = os.path.join(data_dirpath, VAL_VALIDITY_MAP_FILENAME) + val_ground_truth_filepath = os.path.join(data_dirpath, VAL_GROUND_TRUTH_FILENAME) + val_intrinsics_filepath = os.path.join(data_dirpath, VAL_INTRINSICS_FILENAME) + + # Read testing paths + val_image_paths = data_utils.read_paths(val_image_filepath) + val_sparse_depth_paths = data_utils.read_paths(val_sparse_depth_filepath) + val_validity_map_paths = data_utils.read_paths(val_validity_map_filepath) + val_ground_truth_paths = data_utils.read_paths(val_ground_truth_filepath) + val_intrinsics_paths = data_utils.read_paths(val_intrinsics_filepath) + + assert len(val_image_paths) == len(val_sparse_depth_paths) + assert len(val_image_paths) == len(val_validity_map_paths) + assert len(val_image_paths) == len(val_ground_truth_paths) + assert len(val_image_paths) == len(val_intrinsics_paths) + + # Get val set directories + val_seq_dirpaths = set( + [val_image_paths[idx].split(os.sep)[-3] for idx in range(len(val_image_paths))]) + + # Initialize placeholders for training output paths + train_image_outpaths = [] + train_pose_outpaths = [] + train_sparse_depth_outpaths = [] + train_validity_map_outpaths = [] + train_ground_truth_outpaths = [] + train_intrinsics_outpaths = [] + + # Initialize placeholders for testing output paths + test_image_outpaths = [] + test_pose_outpaths = [] + test_sparse_depth_outpaths = [] + test_validity_map_outpaths = [] + test_ground_truth_outpaths = [] + test_intrinsics_outpaths = [] + + # Initialize placeholders for val output paths + val_image_outpaths = [] + val_sparse_depth_outpaths = [] + val_validity_map_outpaths = [] + val_ground_truth_outpaths = [] + val_intrinsics_outpaths = [] + + + # For each dataset density, grab the sequences + seq_dirpaths = [DATA_DIRPATH]#glob.glob(os.path.join(data_dirpath, 'data', '*')) + n_sample = 0 + + for seq_dirpath in tqdm(seq_dirpaths): + # For each sequence, grab the images, sparse depths and valid maps + image_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'images', '*.png'))) + pose_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'poses', '*.yml'))) + sparse_depth_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'depths', '*.yml'))) + validity_map_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'validity_maps', '*.png'))) + ground_truth_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'raw_depths', '*.yml'))) + intrinsics_path = os.path.join(seq_dirpath, 'intrinsics.yml') + + assert len(image_paths) == len(pose_paths) + assert len(image_paths) == len(sparse_depth_paths) + assert len(image_paths) == len(validity_map_paths) + + # Load intrinsics + if path(intrinsics_path).suffix.lower()==".txt": + kin = np.loadtxt(intrinsics_path) + elif path(intrinsics_path).suffix.lower()==".yml": + s = cv2.FileStorage() + _ = s.open(intrinsics_path, cv2.FileStorage_READ) + Knode = s.getNode('camera_intrinsics') + kin = Knode.mat() + if kin.shape[1]==4: + kin = kin[:,:-1] + intrinsics_path_obj = path(intrinsics_path) + intrinsics_file = intrinsics_path_obj.name + intrinsics_refpath = \ + intrinsics_file + intrinsics_outpath = \ + os.path.join(OUTPUT_DIRPATH, intrinsics_refpath[:-3]+'npy') + image_out_dirpath = \ + os.path.join(os.path.dirname(intrinsics_outpath), 'images') + pose_out_dirpath = \ + os.path.join(os.path.dirname(intrinsics_outpath), 'poses') + + if not os.path.exists(image_out_dirpath): + os.makedirs(image_out_dirpath) + if not os.path.exists(pose_out_dirpath): + os.makedirs(pose_out_dirpath) + + # Save intrinsics + np.save(intrinsics_outpath, kin) + + if seq_dirpath.split(os.sep)[-1] in test_seq_dirpaths: + start_idx = skip + offset_idx = 2 + else: + # Skip first stationary 30 frames (1 second) and skip every 10 + start_idx = skip + offset_idx = 2 + + pool_input = [] + for idx in tqdm(range(start_idx, len(image_paths)-offset_idx-start_idx)): + pool_input.append(( + image_paths[idx-offset_idx], + image_paths[idx], + image_paths[idx+offset_idx], + pose_paths[idx-offset_idx], + pose_paths[idx], + pose_paths[idx+offset_idx], + sparse_depth_paths[idx], + validity_map_paths[idx], + ground_truth_paths[idx], + paths_only)) + + train_image_paths = [path(train_image_path).name for train_image_path in train_image_paths] + test_image_paths = [path(test_image_path).name for test_image_path in test_image_paths] + val_image_paths = [path(val_image_path).name for val_image_path in val_image_paths] + + with mp.Pool() as pool: + pool_results = pool.map(process_frame, pool_input) + + for result in tqdm(pool_results): + image_refpath, \ + image_outpath, \ + pose_refpath, \ + pose_outpath, \ + sparse_depth_outpath, \ + validity_map_outpath, \ + ground_truth_outpath = result + # Split into training, testing and unused testing sets + if image_refpath in train_image_paths: + train_image_outpaths.append(str(image_outpath)) + train_pose_outpaths.append(str(pose_outpath)) + train_sparse_depth_outpaths.append(str(sparse_depth_outpath)) + train_validity_map_outpaths.append(str(validity_map_outpath)) + train_ground_truth_outpaths.append(str(ground_truth_outpath)) + train_intrinsics_outpaths.append(str(intrinsics_outpath)) + if image_refpath in test_image_paths: + test_image_outpaths.append(str(image_outpath)) + test_pose_outpaths.append(str(pose_outpath)) + test_sparse_depth_outpaths.append(str(sparse_depth_outpath)) + test_validity_map_outpaths.append(str(validity_map_outpath)) + test_ground_truth_outpaths.append(str(ground_truth_outpath)) + test_intrinsics_outpaths.append(str(intrinsics_outpath)) + if image_refpath in val_image_paths: + val_image_outpaths.append(str(image_outpath)) + val_sparse_depth_outpaths.append(str(sparse_depth_outpath)) + val_validity_map_outpaths.append(str(validity_map_outpath)) + val_ground_truth_outpaths.append(str(ground_truth_outpath)) + val_intrinsics_outpaths.append(str(intrinsics_outpath)) + + n_sample = n_sample + len(pool_input) + + print('Completed processing {} examples for sequence={}'.format( + len(pool_input), seq_dirpath)) + + print('Completed processing {} examples for density={}'.format(n_sample, data_dirpath)) + orb_train_image_filepath, \ + orb_train_pose_filepath, \ + orb_train_sparse_depth_filepath, \ + orb_train_validity_map_filepath, \ + orb_train_ground_truth_filepath, \ + orb_train_intrinsics_filepath = train_filepaths + + print('Storing {} training image file paths into: {}'.format( + len(train_image_outpaths), orb_train_image_filepath)) + data_utils.write_paths( + orb_train_image_filepath, train_image_outpaths) + + print('Storing {} training pose file paths into: {}'.format( + len(train_pose_outpaths), orb_train_pose_filepath)) + data_utils.write_paths( + orb_train_pose_filepath, train_pose_outpaths) + + print('Storing {} training sparse depth file paths into: {}'.format( + len(train_sparse_depth_outpaths), orb_train_sparse_depth_filepath)) + data_utils.write_paths( + orb_train_sparse_depth_filepath, train_sparse_depth_outpaths) + + print('Storing {} training validity map file paths into: {}'.format( + len(train_validity_map_outpaths), orb_train_validity_map_filepath)) + data_utils.write_paths( + orb_train_validity_map_filepath, train_validity_map_outpaths) + + print('Storing {} training groundtruth depth file paths into: {}'.format( + len(train_ground_truth_outpaths), orb_train_ground_truth_filepath)) + data_utils.write_paths( + orb_train_ground_truth_filepath, train_ground_truth_outpaths) + + print('Storing {} training camera intrinsics file paths into: {}'.format( + len(train_intrinsics_outpaths), orb_train_intrinsics_filepath)) + data_utils.write_paths( + orb_train_intrinsics_filepath, train_intrinsics_outpaths) + + orb_test_image_filepath, \ + orb_test_pose_filepath, \ + orb_test_sparse_depth_filepath, \ + orb_test_validity_map_filepath, \ + orb_test_ground_truth_filepath, \ + orb_test_intrinsics_filepath = test_filepaths + + print('Storing {} testing image file paths into: {}'.format( + len(test_image_outpaths), orb_test_image_filepath)) + data_utils.write_paths( + orb_test_image_filepath, test_image_outpaths) + + print('Storing {} testing pose file paths into: {}'.format( + len(test_pose_outpaths), orb_test_pose_filepath)) + data_utils.write_paths( + orb_test_pose_filepath, test_pose_outpaths) + + print('Storing {} testing sparse depth file paths into: {}'.format( + len(test_sparse_depth_outpaths), orb_test_sparse_depth_filepath)) + data_utils.write_paths( + orb_test_sparse_depth_filepath, test_sparse_depth_outpaths) + + print('Storing {} testing validity map file paths into: {}'.format( + len(test_validity_map_outpaths), orb_test_validity_map_filepath)) + data_utils.write_paths( + orb_test_validity_map_filepath, test_validity_map_outpaths) + + print('Storing {} testing groundtruth depth file paths into: {}'.format( + len(test_ground_truth_outpaths), orb_test_ground_truth_filepath)) + data_utils.write_paths( + orb_test_ground_truth_filepath, test_ground_truth_outpaths) + + print('Storing {} testing camera intrinsics file paths into: {}'.format( + len(test_intrinsics_outpaths), orb_test_intrinsics_filepath)) + data_utils.write_paths( + orb_test_intrinsics_filepath, test_intrinsics_outpaths) + + + orb_val_image_filepath, \ + orb_val_sparse_depth_filepath, \ + orb_val_validity_map_filepath, \ + orb_val_ground_truth_filepath, \ + orb_val_intrinsics_filepath = val_filepaths + + print('Storing {} validation image file paths into: {}'.format( + len(val_image_outpaths), orb_val_image_filepath)) + data_utils.write_paths( + orb_val_image_filepath, val_image_outpaths) + + print('Storing {} validation sparse depth file paths into: {}'.format( + len(val_sparse_depth_outpaths), orb_val_sparse_depth_filepath)) + data_utils.write_paths( + orb_val_sparse_depth_filepath, val_sparse_depth_outpaths) + + print('Storing {} validation validity map file paths into: {}'.format( + len(val_validity_map_outpaths), orb_val_validity_map_filepath)) + data_utils.write_paths( + orb_val_validity_map_filepath, val_validity_map_outpaths) + + print('Storing {} validation groundtruth depth file paths into: {}'.format( + len(val_ground_truth_outpaths), orb_val_ground_truth_filepath)) + data_utils.write_paths( + orb_val_ground_truth_filepath, val_ground_truth_outpaths) + + print('Storing {} validation camera intrinsics file paths into: {}'.format( + len(val_intrinsics_outpaths), orb_val_intrinsics_filepath)) + data_utils.write_paths( + orb_val_intrinsics_filepath, val_intrinsics_outpaths) \ No newline at end of file diff --git a/setup/save_captured_data.py b/setup/save_captured_data.py new file mode 100644 index 0000000..4a93bb7 --- /dev/null +++ b/setup/save_captured_data.py @@ -0,0 +1,44 @@ +import numpy as np +import rosbag +import sys +from pathlib import Path +import cv2 +from tqdm import tqdm + +def save_data(messages, folder): + folder = str(folder) + for topic,msg,time in tqdm(messages): + np_arr = np.frombuffer(msg.data, np.uint8) + image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + cv2.imwrite(folder+'/'+str(time)+'.png', image) + +bag_file = sys.argv[1] +save_folder = Path(sys.argv[2]) + +fisheye_folder = Path(str(save_folder) + '/fisheye_color') +fisheye_folder.mkdir(parents=True, exist_ok=True) + +d435_image_folder = Path(str(save_folder)+'/d435_color') +d435_image_folder.mkdir(parents=True, exist_ok=True) + +depth_folder = Path(str(save_folder) + '/d435_depth') +depth_folder.mkdir(parents=True, exist_ok=True) + +print('created approp folders') + +fisheye_color_topic = '/front_fisheye_camera/image_raw/compressed' +d435_color_topic = '/torso_front_camera/color/image_raw/compressed' +depth_topic = '/torso_front_camera/aligned_depth_to_color/image_raw/compressed' + +print('Reading bag messages') +bag = rosbag.Bag(bag_file, 'r') +fisheye_color_messages = bag.read_messages(topics=[fisheye_color_topic]) +d435_color_messages = bag.read_messages(topics=[d435_color_topic]) +depth_messages = bag.read_messages(topics=[depth_topic]) + +print('saving fisheye images') +save_data(fisheye_color_messages, fisheye_folder) +print('saving d435 images') +save_data(d435_color_messages, d435_image_folder) +print('saving depth images') +save_data(depth_messages, depth_folder) \ No newline at end of file diff --git a/setup/save_captured_data2.py b/setup/save_captured_data2.py new file mode 100755 index 0000000..1daf5e0 --- /dev/null +++ b/setup/save_captured_data2.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python +import rospy +import numpy as np +import sys +from pathlib import Path +import tf +import cv2 +from sensor_msgs.msg import CompressedImage +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped +from cv_bridge import CvBridge +import subprocess as sp +from PIL import Image as Im + +current_pose = PoseWithCovariance() +last_pose = PoseWithCovariance() + +bag_file = sys.argv[1] +try: + player_proc = sp.Popen(['rosbag', 'play', bag_file]) +except: + rospy.logerr("Unable to play bag file. Exiting") + exit() + +save_folder = Path(sys.argv[2]) + +def pose_callback(msg): + global current_pose + current_pose = msg.pose + + +class Camera(): + def __init__(self, camera): + self.br = CvBridge() + self.camera_name = camera[0] + # rospy.resolve_name(self.camera_name, caller_id=None) + self.inner_folder = str(save_folder)+'/'+self.camera_name + self.inner_folder_path = Path(self.inner_folder) + try: + self.inner_folder_path.mkdir(parents=True,exist_ok=True) + except: + self.inner_folder_path.mkdir(parents=True) + + self.topic = camera[1][0] + self.compressed = camera[1][1] + if self.compressed==1: + self.topic = self.topic+'/compressed' + + if self.camera_name == 'torso_front_depth': + self.sub = rospy.Subscriber(self.topic, Image, self.compressed_callback, queue_size=10) + else: + self.sub = rospy.Subscriber(self.topic, CompressedImage, self.compressed_callback, queue_size=10) + self.time_beg=False + + + + def compressed_callback(self,msg): + global current_pose, last_pose + + current_position = np.asarray([current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z]) + last_position = np.asarray([last_pose.pose.position.x, last_pose.pose.position.y, last_pose.pose.position.z]) + + current_orientation = [current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w] + current_orientation = np.asarray(tf.transformations.euler_from_quaternion(current_orientation)) + + last_orientation = [last_pose.pose.orientation.x, last_pose.pose.orientation.y, last_pose.pose.orientation.z, last_pose.pose.orientation.w] + last_orientation = np.asarray(tf.transformations.euler_from_quaternion(last_orientation)) + + time = msg.header.stamp + if self.time_beg == False: + self.start_time=time + self.time_beg=True + + # time = time - self.start_time + if (np.linalg.norm(current_position-last_position,2) >= 0.01) or np.rad2deg(np.linalg.norm(current_orientation-last_orientation))>=10: + try: + np_arr = None + if self.camera_name != 'torso_front_depth': + np_arr = np.frombuffer(msg.data, np.uint8) + image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + cv2.imwrite(self.inner_folder+'/'+str(time)+'.jpg', image) + else: + depth_image = self.br.imgmsg_to_cv2(msg, desired_encoding="passthrough") + depth_array = np.array(depth_image, dtype=np.uint32)*256 # The *256 is needed to preserve floating point accuracy; while loading depth, it is multiplied by 256. + z = Im.fromarray(depth_array, mode='I') + z.save(self.inner_folder+'/'+str(time)+'.png') + # cv2.imwrite(self.inner_folder+'/'+str(time)+'.png', depth_image) + + except: + rospy.logwarn("Some issue with: ", self.camera_name) + pass + + last_pose = current_pose + +cameras = { + 'head_front':('/head_front_camera/image_raw',1), + 'front_fisheye':('/front_fisheye_camera/image_raw',1), + 'torso_front_color':('/torso_front_camera/color/image_raw',1), + 'torso_frontIR1':('/torso_front_camera/infra1/image_rect_raw',1), + 'torso_frontIR2':('/torso_front_camera/infra2/image_rect_raw',1), + 'torso_front_depth':('/torso_front_camera/aligned_depth_to_color/image_raw',0) + } +cameras_list = list(cameras.keys()) + + +if __name__=='__main__': + rospy.init_node('image_extractor') + + pose_sub = rospy.Subscriber('/robot_pose', PoseWithCovarianceStamped, pose_callback, queue_size=10) + + for i in range(len(cameras_list)): + camera = cameras_list[i] + topic, compressed = cameras[camera] + cam = Camera((camera, (topic,compressed))) + + rospy.spin() \ No newline at end of file diff --git a/setup/setup_captured_orb_data.py b/setup/setup_captured_orb_data.py new file mode 100644 index 0000000..5d28d61 --- /dev/null +++ b/setup/setup_captured_orb_data.py @@ -0,0 +1,563 @@ +''' +Author: Alex Wong + +If you use this code, please cite the following paper: + +A. Wong, and S. Soatto. Unsupervised Depth Completion with Calibrated Backprojection Layers. +https://arxiv.org/pdf/2108.10531.pdf + +@inproceedings{wong2021unsupervised, + title={Unsupervised Depth Completion with Calibrated Backprojection Layers}, + author={Wong, Alex and Soatto, Stefano}, + booktitle={Proceedings of the IEEE/CVF International Conference on Computer Vision}, + pages={12747--12756}, + year={2021} +} +''' +import os, sys, glob, argparse +import multiprocessing as mp +import numpy as np +import cv2 +sys.path.insert(0,'src') +from pathlib import Path as path +import data_utils +from tqdm import tqdm +from natsort import natsorted + +paths_only = False +skip = 5 + +def create_imagesPaths_files(DATA_DIRPATH, train_fileNames, test_fileNames): + train_image_fileName, train_pose_fileName, train_sparse_depth_fileName, train_validity_map_fileName, train_ground_truth_fileName, train_intrinsics_fileName = train_fileNames + test_image_fileName, test_pose_fileName, test_sparse_depth_fileName, test_validity_map_fileName, test_ground_truth_fileName, test_intrinsics_fileName = test_fileNames + + train_image_filepath = os.path.join(DATA_DIRPATH, train_image_fileName) + train_pose_filepath = os.path.join(DATA_DIRPATH, train_pose_fileName) + train_sparse_depth_filepath = os.path.join(DATA_DIRPATH, train_sparse_depth_fileName) + train_validity_map_filepath = os.path.join(DATA_DIRPATH, train_validity_map_fileName) + train_ground_truth_filepath = os.path.join(DATA_DIRPATH, train_ground_truth_fileName) + train_intrinsics_filepath = os.path.join(DATA_DIRPATH, train_intrinsics_fileName) + + test_image_filepath = os.path.join(DATA_DIRPATH, test_image_fileName) + test_pose_filepath = os.path.join(DATA_DIRPATH, test_pose_fileName) + test_sparse_depth_filepath = os.path.join(DATA_DIRPATH, test_sparse_depth_fileName) + test_validity_map_filepath = os.path.join(DATA_DIRPATH, test_validity_map_fileName) + test_ground_truth_filepath = os.path.join(DATA_DIRPATH, test_ground_truth_fileName) + test_intrinsics_filepath = os.path.join(DATA_DIRPATH, test_intrinsics_fileName) + + + train_imageF = open(train_image_filepath, "w") + train_poseF = open(train_pose_filepath, "w") + train_sparseF = open(train_sparse_depth_filepath, "w") + train_valF = open(train_validity_map_filepath, "w") + train_gtF = open(train_ground_truth_filepath, "w") + train_KF = open(train_intrinsics_filepath, "w") + + test_imageF = open(test_image_filepath, "w") + test_poseF = open(test_pose_filepath, "w") + test_sparseF = open(test_sparse_depth_filepath, "w") + test_valF = open(test_validity_map_filepath, "w") + test_gtF = open(test_ground_truth_filepath, "w") + test_KF = open(test_intrinsics_filepath, "w") + + images_folder = os.path.join(DATA_DIRPATH, "images") + poses_folder = os.path.join(DATA_DIRPATH, "poses") + depths_folder = os.path.join(DATA_DIRPATH, "depths") + val_folder = os.path.join(DATA_DIRPATH, "validity_maps") + gt_folder = os.path.join(DATA_DIRPATH, "raw_depths") + + numImages = len([i for i in os.listdir(images_folder)]) + train_imagePaths = [] + train_posePaths = [] + train_sparsePaths = [] + train_valPaths = [] + train_GTPaths = [] + train_intrinsicPaths = [] + + test_imagePaths = [] + test_posePaths = [] + test_sparsePaths = [] + test_valPaths = [] + test_GTPaths = [] + test_intrinsicPaths = [] + + + training_indices = np.random.choice(numImages, int(0.9*numImages), replace=False) + for i in range(numImages): + if i< skip: + continue + if i in training_indices: + train_imagePaths.append( os.path.join(images_folder, "keyframe"+str(i)+".png\n") ) + train_posePaths.append( os.path.join(poses_folder, "pose"+str(i)+".txt\n") ) + train_sparsePaths.append( os.path.join(depths_folder, "depth"+str(i)+".yml\n") ) + train_valPaths.append( os.path.join(val_folder, "validity_map"+str(i)+".png\n") ) + train_GTPaths.append( os.path.join(gt_folder, "rawDepth"+str(i)+".yml\n") ) + train_intrinsicPaths.append( os.path.join(DATA_DIRPATH, "intrinsics.yml\n") ) + else: + test_imagePaths.append(os.path.join(images_folder, "keyframe"+str(i)+".png\n")) + test_posePaths.append( os.path.join(poses_folder, "pose"+str(i)+".txt\n") ) + test_sparsePaths.append( os.path.join(depths_folder, "depth"+str(i)+".yml\n") ) + test_valPaths.append( os.path.join(val_folder, "validity_map"+str(i)+".png\n") ) + test_GTPaths.append( os.path.join(gt_folder, "rawDepth"+str(i)+".yml\n") ) + test_intrinsicPaths.append( os.path.join(DATA_DIRPATH, "intrinsics.yml\n") ) + + train_imageF.writelines(train_imagePaths) + train_poseF.writelines(train_posePaths) + train_sparseF.writelines(train_sparsePaths) + train_valF.writelines(train_valPaths) + train_gtF.writelines(train_GTPaths) + train_KF.writelines(train_intrinsicPaths) + + test_imageF.writelines(test_imagePaths) + test_poseF.writelines(test_posePaths) + test_sparseF.writelines(test_sparsePaths) + test_valF.writelines(test_valPaths) + test_gtF.writelines(test_GTPaths) + test_KF.writelines(test_intrinsicPaths) + + train_imageF.close() + train_poseF.close() + train_sparseF.close() + train_valF.close() + train_gtF.close() + train_KF.close() + test_imageF.close() + test_poseF.close() + test_sparseF.close() + test_valF.close() + test_gtF.close() + test_KF.close() + +DATA_DIRPATH = sys.argv[1] +OUTPUT_DIRPATH = sys.argv[2] + +TRAIN_IMAGE_FILENAME = 'train_image.txt' +TRAIN_POSE_FILENAME = 'train_pose.txt' +TRAIN_SPARSE_DEPTH_FILENAME = 'train_sparse_depth.txt' +TRAIN_VALIDITY_MAP_FILENAME = 'train_validity_map.txt' +TRAIN_GROUND_TRUTH_FILENAME = 'train_ground_truth.txt' +TRAIN_INTRINSICS_FILENAME = 'train_intrinsics.txt' +TEST_IMAGE_FILENAME = 'test_image.txt' +TEST_POSE_FILENAME = 'test_pose.txt' +TEST_SPARSE_DEPTH_FILENAME = 'test_sparse_depth.txt' +TEST_VALIDITY_MAP_FILENAME = 'test_validity_map.txt' +TEST_GROUND_TRUTH_FILENAME = 'test_ground_truth.txt' +TEST_INTRINSICS_FILENAME = 'test_intrinsics.txt' + + +TRAIN_REFS_DIRPATH = os.path.join('training', 'orb') +TEST_REFS_DIRPATH = os.path.join('testing', 'orb') + + +# ORB training set +TRAIN_IMAGE_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_image.txt') +TRAIN_POSE_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_pose.txt') +TRAIN_SPARSE_DEPTH_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_sparse_depth.txt') +TRAIN_VALIDITY_MAP_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_validity_map.txt') +TRAIN_GROUND_TRUTH_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_ground_truth.txt') +TRAIN_INTRINSICS_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'train_intrinsics.txt') +# ORB testing +TEST_IMAGE_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_image.txt') +TEST_POSE_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_pose.txt') +TEST_SPARSE_DEPTH_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_sparse_depth.txt') +TEST_VALIDITY_MAP_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_validity_map.txt') +TEST_GROUND_TRUTH_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_ground_truth.txt') +TEST_INTRINSICS_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'test_intrinsics.txt') + +training_names = [TRAIN_IMAGE_FILENAME, TRAIN_POSE_FILENAME, TRAIN_SPARSE_DEPTH_FILENAME, TRAIN_VALIDITY_MAP_FILENAME, TRAIN_GROUND_TRUTH_FILENAME, TRAIN_INTRINSICS_FILENAME] +testing_names = [TEST_IMAGE_FILENAME, TEST_POSE_FILENAME, TEST_SPARSE_DEPTH_FILENAME, TEST_VALIDITY_MAP_FILENAME, TEST_GROUND_TRUTH_FILENAME, TEST_INTRINSICS_FILENAME] +create_imagesPaths_files(DATA_DIRPATH, training_names,testing_names) + +def process_frame(inputs): + ''' + Processes a single frame + + Arg(s): + inputs : tuple + image path at time t=0, + image path at time t=1, + image path at time t=-1, + pose path at time t=0 + pose path at time t=1 + pose path at time t=-1 + sparse depth path at time t=0, + validity map path at time t=0, + ground truth path at time t=0, + boolean flag if set then create paths only + Returns: + str : image reference directory path + str : output concatenated image path at time t=0 + str : output sparse depth path at time t=0 + str : output validity map path at time t=0 + str : output ground truth path at time t=0 + ''' + + image_path1, \ + image_path0, \ + image_path2, \ + pose_path1, \ + pose_path0, \ + pose_path2, \ + sparse_depth_path, \ + validity_map_path, \ + ground_truth_path, \ + paths_only = inputs + + if not paths_only: + # Create image composite of triplets + image1 = cv2.imread(image_path1) + image0 = cv2.imread(image_path0) + image2 = cv2.imread(image_path2) + imagec = np.concatenate([image1, image0, image2], axis=1) + # Store poses of image triplet + + if path(pose_path0).suffix.lower()==".txt": + pose1 = np.loadtxt(pose_path1) + pose0 = np.loadtxt(pose_path0) + pose2 = np.loadtxt(pose_path2) + posec = np.stack((pose1, pose0, pose2)) + posec = posec.reshape(posec.shape[0],-1) + elif path(pose_path0).suffix.lower()==".yml": + s = cv2.FileStorage() + _ = s.open(pose_path1, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose1 = pnode.mat() + _ = s.open(pose_path0, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose0 = pnode.mat() + _ = s.open(pose_path2, cv2.FileStorage_READ) + assert _ == True + pnode = s.getNode('camera_pose') + pose2 = pnode.mat() + posec = np.stack((pose1, pose0, pose2)) + posec = posec.reshape(posec.shape[0],-1) + + # Get validity map + sparse_depth, validity_map = data_utils.load_depth_with_validity_map(sparse_depth_path, file_format='yml') + + # image_refpath = os.path.join(*image_path0.split(os.sep)[8:]) + # pose_refpath = os.path.join(*pose_path0.split(os.sep)[8:]) + imageName = path(image_path0).name + poseName = path(pose_path0).name.replace("yml", "txt") + + # Set output paths + image_outpath = path.joinpath(path(OUTPUT_DIRPATH), "images", imageName) + pose_outpath = path.joinpath(path(OUTPUT_DIRPATH), "poses", poseName) + sparse_depth_outpath = sparse_depth_path + validity_map_outpath = validity_map_path + ground_truth_outpath = ground_truth_path + + # Verify that all filenames match + image_filename = imageName + pose_filename = poseName + sparse_depth_filename = path(sparse_depth_outpath).name + validity_map_filename = path(validity_map_outpath).name + ground_truth_filename = path(ground_truth_outpath).name + assert "".join(filter(str.isdigit, path(image_filename).name.replace(path(image_filename).suffix, " ")) ) == "".join(filter(str.isdigit, path(pose_filename).name.replace(path(pose_filename).suffix, " ")) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, sparse_depth_filename) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, validity_map_filename) ) + assert "".join(filter(str.isdigit, image_filename) ) == "".join(filter(str.isdigit, ground_truth_filename) ) + + if not paths_only: + cv2.imwrite(str(image_outpath), imagec) + np.savetxt(str(pose_outpath), posec) + return (imageName, + image_outpath, + poseName, + pose_outpath, + sparse_depth_outpath, + validity_map_outpath, + ground_truth_outpath) + + +# parser = argparse.ArgumentParser() +# parser.add_argument('--paths_only', action='store_true') + +# args = parser.parse_args() + +data_dirpaths = [ + DATA_DIRPATH +] + +train_output_filepaths = [ + [ + TRAIN_IMAGE_FILEPATH, + TRAIN_POSE_FILEPATH, + TRAIN_SPARSE_DEPTH_FILEPATH, + TRAIN_VALIDITY_MAP_FILEPATH, + TRAIN_GROUND_TRUTH_FILEPATH, + TRAIN_INTRINSICS_FILEPATH + ] +] +test_output_filepaths = [ + [ + TEST_IMAGE_FILEPATH, + TEST_POSE_FILEPATH, + TEST_SPARSE_DEPTH_FILEPATH, + TEST_VALIDITY_MAP_FILEPATH, + TEST_GROUND_TRUTH_FILEPATH, + TEST_INTRINSICS_FILEPATH + ] +] + +for dirpath in tqdm([TRAIN_REFS_DIRPATH, TEST_REFS_DIRPATH]): + if not os.path.exists(dirpath): + os.makedirs(dirpath) + +data_filepaths = \ + zip(data_dirpaths, train_output_filepaths, test_output_filepaths) + +for data_dirpath, train_filepaths, test_filepaths in tqdm(data_filepaths): + # Training set + train_image_filepath = os.path.join(data_dirpath, TRAIN_IMAGE_FILENAME) + train_pose_filepath = os.path.join(data_dirpath, TRAIN_POSE_FILENAME) + train_sparse_depth_filepath = os.path.join(data_dirpath, TRAIN_SPARSE_DEPTH_FILENAME) + train_validity_map_filepath = os.path.join(data_dirpath, TRAIN_VALIDITY_MAP_FILENAME) + train_ground_truth_filepath = os.path.join(data_dirpath, TRAIN_GROUND_TRUTH_FILENAME) + train_intrinsics_filepath = os.path.join(data_dirpath, TRAIN_INTRINSICS_FILENAME) + + # Read training paths + train_image_paths = data_utils.read_paths(train_image_filepath) + # combined_trainImage = '\t'.join(train_image_paths) + train_pose_paths = data_utils.read_paths(train_pose_filepath) + # combined_trainPose = '\t'.join(train_pose_paths) + train_sparse_depth_paths = data_utils.read_paths(train_sparse_depth_filepath) + # combined_trainSparse = '\t'.join(train_sparse_depth_paths) + train_validity_map_paths = data_utils.read_paths(train_validity_map_filepath) + # combined_trainVal = '\t'.join(train_validity_map_paths) + train_ground_truth_paths = data_utils.read_paths(train_ground_truth_filepath) + # combined_trainGT = '\t'.join(train_ground_truth_paths) + train_intrinsics_paths = data_utils.read_paths(train_intrinsics_filepath) + # combined_trainIn = '\t'.join(train_intrinsics_paths) + + assert len(train_image_paths) == len(train_pose_paths) + assert len(train_image_paths) == len(train_sparse_depth_paths) + assert len(train_image_paths) == len(train_validity_map_paths) + assert len(train_image_paths) == len(train_ground_truth_paths) + assert len(train_image_paths) == len(train_intrinsics_paths) + + # Testing set + test_image_filepath = os.path.join(data_dirpath, TEST_IMAGE_FILENAME) + test_pose_filepath = os.path.join(data_dirpath, TEST_POSE_FILENAME) + test_sparse_depth_filepath = os.path.join(data_dirpath, TEST_SPARSE_DEPTH_FILENAME) + test_validity_map_filepath = os.path.join(data_dirpath, TEST_VALIDITY_MAP_FILENAME) + test_ground_truth_filepath = os.path.join(data_dirpath, TEST_GROUND_TRUTH_FILENAME) + test_intrinsics_filepath = os.path.join(data_dirpath, TEST_INTRINSICS_FILENAME) + + # Read testing paths + test_image_paths = data_utils.read_paths(test_image_filepath) + test_pose_paths = data_utils.read_paths(test_pose_filepath) + test_sparse_depth_paths = data_utils.read_paths(test_sparse_depth_filepath) + test_validity_map_paths = data_utils.read_paths(test_validity_map_filepath) + test_ground_truth_paths = data_utils.read_paths(test_ground_truth_filepath) + test_intrinsics_paths = data_utils.read_paths(test_intrinsics_filepath) + + assert len(test_image_paths) == len(test_pose_paths) + assert len(test_image_paths) == len(test_sparse_depth_paths) + assert len(test_image_paths) == len(test_validity_map_paths) + assert len(test_image_paths) == len(test_ground_truth_paths) + assert len(test_image_paths) == len(test_intrinsics_paths) + + # Get test set directories + test_seq_dirpaths = set( + [test_image_paths[idx].split(os.sep)[-3] for idx in range(len(test_image_paths))]) + + # Initialize placeholders for training output paths + train_image_outpaths = [] + train_pose_outpaths = [] + train_sparse_depth_outpaths = [] + train_validity_map_outpaths = [] + train_ground_truth_outpaths = [] + train_intrinsics_outpaths = [] + + # Initialize placeholders for testing output paths + test_image_outpaths = [] + test_pose_outpaths = [] + test_sparse_depth_outpaths = [] + test_validity_map_outpaths = [] + test_ground_truth_outpaths = [] + test_intrinsics_outpaths = [] + + # For each dataset density, grab the sequences + seq_dirpaths = [DATA_DIRPATH]#glob.glob(os.path.join(data_dirpath, 'data', '*')) + n_sample = 0 + + for seq_dirpath in tqdm(seq_dirpaths): + # For each sequence, grab the images, sparse depths and valid maps + image_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'images', '*.png'))) + pose_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'poses', '*.yml'))) + sparse_depth_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'depths', '*.yml'))) + validity_map_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'validity_maps', '*.png'))) + ground_truth_paths = \ + natsorted(glob.glob(os.path.join(seq_dirpath, 'raw_depths', '*.yml'))) + intrinsics_path = os.path.join(seq_dirpath, 'intrinsics.yml') + + assert len(image_paths) == len(pose_paths) + assert len(image_paths) == len(sparse_depth_paths) + assert len(image_paths) == len(validity_map_paths) + + # Load intrinsics + if path(intrinsics_path).suffix.lower()==".txt": + kin = np.loadtxt(intrinsics_path) + elif path(intrinsics_path).suffix.lower()==".yml": + s = cv2.FileStorage() + _ = s.open(intrinsics_path, cv2.FileStorage_READ) + Knode = s.getNode('camera_intrinsics') + kin = Knode.mat() + if kin.shape[1]==4: + kin = kin[:,:-1] + intrinsics_path_obj = path(intrinsics_path) + intrinsics_file = intrinsics_path_obj.name + intrinsics_refpath = \ + intrinsics_file + intrinsics_outpath = \ + os.path.join(OUTPUT_DIRPATH, intrinsics_refpath[:-3]+'npy') + image_out_dirpath = \ + os.path.join(os.path.dirname(intrinsics_outpath), 'images') + pose_out_dirpath = \ + os.path.join(os.path.dirname(intrinsics_outpath), 'poses') + + if not os.path.exists(image_out_dirpath): + os.makedirs(image_out_dirpath) + if not os.path.exists(pose_out_dirpath): + os.makedirs(pose_out_dirpath) + + # Save intrinsics + np.save(intrinsics_outpath, kin) + + if seq_dirpath.split(os.sep)[-1] in test_seq_dirpaths: + start_idx = skip + offset_idx = 2 + else: + # Skip first stationary 30 frames (1 second) and skip every 10 + start_idx = skip + offset_idx = 2 + + pool_input = [] + for idx in tqdm(range(start_idx, len(image_paths)-offset_idx-start_idx)): + pool_input.append(( + image_paths[idx-offset_idx], + image_paths[idx], + image_paths[idx+offset_idx], + pose_paths[idx-offset_idx], + pose_paths[idx], + pose_paths[idx+offset_idx], + sparse_depth_paths[idx], + validity_map_paths[idx], + ground_truth_paths[idx], + paths_only)) + + train_image_paths = [path(train_image_path).name for train_image_path in train_image_paths] + test_image_paths = [path(test_image_path).name for test_image_path in test_image_paths] + with mp.Pool() as pool: + pool_results = pool.map(process_frame, pool_input) + + for result in tqdm(pool_results): + image_refpath, \ + image_outpath, \ + pose_refpath, \ + pose_outpath, \ + sparse_depth_outpath, \ + validity_map_outpath, \ + ground_truth_outpath = result + # Split into training, testing and unused testing sets + if image_refpath in train_image_paths: + train_image_outpaths.append(str(image_outpath)) + train_pose_outpaths.append(str(pose_outpath)) + train_sparse_depth_outpaths.append(str(sparse_depth_outpath)) + train_validity_map_outpaths.append(str(validity_map_outpath)) + train_ground_truth_outpaths.append(str(ground_truth_outpath)) + train_intrinsics_outpaths.append(str(intrinsics_outpath)) + elif image_refpath in test_image_paths: + test_image_outpaths.append(str(image_outpath)) + test_pose_outpaths.append(str(pose_outpath)) + test_sparse_depth_outpaths.append(str(sparse_depth_outpath)) + test_validity_map_outpaths.append(str(validity_map_outpath)) + test_ground_truth_outpaths.append(str(ground_truth_outpath)) + test_intrinsics_outpaths.append(str(intrinsics_outpath)) + + n_sample = n_sample + len(pool_input) + + print('Completed processing {} examples for sequence={}'.format( + len(pool_input), seq_dirpath)) + + print('Completed processing {} examples for density={}'.format(n_sample, data_dirpath)) + orb_train_image_filepath, \ + orb_train_pose_filepath, \ + orb_train_sparse_depth_filepath, \ + orb_train_validity_map_filepath, \ + orb_train_ground_truth_filepath, \ + orb_train_intrinsics_filepath = train_filepaths + + print('Storing {} training image file paths into: {}'.format( + len(train_image_outpaths), orb_train_image_filepath)) + data_utils.write_paths( + orb_train_image_filepath, train_image_outpaths) + + print('Storing {} training pose file paths into: {}'.format( + len(train_pose_outpaths), orb_train_pose_filepath)) + data_utils.write_paths( + orb_train_pose_filepath, train_pose_outpaths) + + print('Storing {} training sparse depth file paths into: {}'.format( + len(train_sparse_depth_outpaths), orb_train_sparse_depth_filepath)) + data_utils.write_paths( + orb_train_sparse_depth_filepath, train_sparse_depth_outpaths) + + print('Storing {} training validity map file paths into: {}'.format( + len(train_validity_map_outpaths), orb_train_validity_map_filepath)) + data_utils.write_paths( + orb_train_validity_map_filepath, train_validity_map_outpaths) + + print('Storing {} training groundtruth depth file paths into: {}'.format( + len(train_ground_truth_outpaths), orb_train_ground_truth_filepath)) + data_utils.write_paths( + orb_train_ground_truth_filepath, train_ground_truth_outpaths) + + print('Storing {} training camera intrinsics file paths into: {}'.format( + len(train_intrinsics_outpaths), orb_train_intrinsics_filepath)) + data_utils.write_paths( + orb_train_intrinsics_filepath, train_intrinsics_outpaths) + + orb_test_image_filepath, \ + orb_test_pose_filepath, \ + orb_test_sparse_depth_filepath, \ + orb_test_validity_map_filepath, \ + orb_test_ground_truth_filepath, \ + orb_test_intrinsics_filepath = test_filepaths + + print('Storing {} testing image file paths into: {}'.format( + len(test_image_outpaths), orb_test_image_filepath)) + data_utils.write_paths( + orb_test_image_filepath, test_image_outpaths) + + print('Storing {} testing pose file paths into: {}'.format( + len(test_pose_outpaths), orb_test_pose_filepath)) + data_utils.write_paths( + orb_test_pose_filepath, test_pose_outpaths) + + print('Storing {} testing sparse depth file paths into: {}'.format( + len(test_sparse_depth_outpaths), orb_test_sparse_depth_filepath)) + data_utils.write_paths( + orb_test_sparse_depth_filepath, test_sparse_depth_outpaths) + + print('Storing {} testing validity map file paths into: {}'.format( + len(test_validity_map_outpaths), orb_test_validity_map_filepath)) + data_utils.write_paths( + orb_test_validity_map_filepath, test_validity_map_outpaths) + + print('Storing {} testing groundtruth depth file paths into: {}'.format( + len(test_ground_truth_outpaths), orb_test_ground_truth_filepath)) + data_utils.write_paths( + orb_test_ground_truth_filepath, test_ground_truth_outpaths) + + print('Storing {} testing camera intrinsics file paths into: {}'.format( + len(test_intrinsics_outpaths), orb_test_intrinsics_filepath)) + data_utils.write_paths( + orb_test_intrinsics_filepath, test_intrinsics_outpaths) diff --git a/setup/setup_dataset_void.py b/setup/setup_dataset_void.py index d941661..130d4dc 100644 --- a/setup/setup_dataset_void.py +++ b/setup/setup_dataset_void.py @@ -18,23 +18,24 @@ import multiprocessing as mp import numpy as np import cv2 -sys.path.insert(0, 'src') +sys.path.insert(0,'src') +# print(sys.path) import data_utils +from tqdm import tqdm - -VOID_ROOT_DIRPATH = os.path.join('data', 'void_release') -VOID_DATA_150_DIRPATH = os.path.join(VOID_ROOT_DIRPATH, 'void_150') -VOID_DATA_500_DIRPATH = os.path.join(VOID_ROOT_DIRPATH, 'void_500') +VOID_ROOT_DIRPATH = os.path.join('/local1/datasets/Depth_Completion_Datasets/void_set2/void-dataset/data', 'void_release') VOID_DATA_1500_DIRPATH = os.path.join(VOID_ROOT_DIRPATH, 'void_1500') -VOID_OUTPUT_DIRPATH = os.path.join('data', 'void_kbnet') +VOID_OUTPUT_DIRPATH = os.path.join('/local1/datasets/Depth_Completion_Datasets/void_set2/void-dataset/data', 'void_kbnet') VOID_TRAIN_IMAGE_FILENAME = 'train_image.txt' +VOID_TRAIN_POSE_FILENAME = 'train_absolute_pose.txt' VOID_TRAIN_SPARSE_DEPTH_FILENAME = 'train_sparse_depth.txt' VOID_TRAIN_VALIDITY_MAP_FILENAME = 'train_validity_map.txt' VOID_TRAIN_GROUND_TRUTH_FILENAME = 'train_ground_truth.txt' VOID_TRAIN_INTRINSICS_FILENAME = 'train_intrinsics.txt' VOID_TEST_IMAGE_FILENAME = 'test_image.txt' +VOID_TEST_POSE_FILENAME = 'test_absolute_pose.txt' VOID_TEST_SPARSE_DEPTH_FILENAME = 'test_sparse_depth.txt' VOID_TEST_VALIDITY_MAP_FILENAME = 'test_validity_map.txt' VOID_TEST_GROUND_TRUTH_FILENAME = 'test_ground_truth.txt' @@ -43,56 +44,23 @@ TRAIN_REFS_DIRPATH = os.path.join('training', 'void') TEST_REFS_DIRPATH = os.path.join('testing', 'void') -# VOID training set 150 density -VOID_TRAIN_IMAGE_150_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_image_150.txt') -VOID_TRAIN_SPARSE_DEPTH_150_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_sparse_depth_150.txt') -VOID_TRAIN_VALIDITY_MAP_150_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_validity_map_150.txt') -VOID_TRAIN_GROUND_TRUTH_150_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_ground_truth_150.txt') -VOID_TRAIN_INTRINSICS_150_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_intrinsics_150.txt') -# VOID training set 500 density -VOID_TRAIN_IMAGE_500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_image_500.txt') -VOID_TRAIN_SPARSE_DEPTH_500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_sparse_depth_500.txt') -VOID_TRAIN_VALIDITY_MAP_500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_validity_map_500.txt') -VOID_TRAIN_GROUND_TRUTH_500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_ground_truth_500.txt') -VOID_TRAIN_INTRINSICS_500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_intrinsics_500.txt') # VOID training set 1500 density VOID_TRAIN_IMAGE_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_image_1500.txt') +VOID_TRAIN_POSE_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_pose_1500.txt') VOID_TRAIN_SPARSE_DEPTH_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_sparse_depth_1500.txt') VOID_TRAIN_VALIDITY_MAP_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_validity_map_1500.txt') VOID_TRAIN_GROUND_TRUTH_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_ground_truth_1500.txt') VOID_TRAIN_INTRINSICS_1500_FILEPATH = os.path.join(TRAIN_REFS_DIRPATH, 'void_train_intrinsics_1500.txt') -# VOID testing set 150 density -VOID_TEST_IMAGE_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_image_150.txt') -VOID_TEST_SPARSE_DEPTH_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_sparse_depth_150.txt') -VOID_TEST_VALIDITY_MAP_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_validity_map_150.txt') -VOID_TEST_GROUND_TRUTH_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_ground_truth_150.txt') -VOID_TEST_INTRINSICS_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_intrinsics_150.txt') -# VOID testing set 500 density -VOID_TEST_IMAGE_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_image_500.txt') -VOID_TEST_SPARSE_DEPTH_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_sparse_depth_500.txt') -VOID_TEST_VALIDITY_MAP_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_validity_map_500.txt') -VOID_TEST_GROUND_TRUTH_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_ground_truth_500.txt') -VOID_TEST_INTRINSICS_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_intrinsics_500.txt') # VOID testing set 1500 density VOID_TEST_IMAGE_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_image_1500.txt') +VOID_TEST_POSE_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_pose_1500.txt') VOID_TEST_SPARSE_DEPTH_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_sparse_depth_1500.txt') VOID_TEST_VALIDITY_MAP_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_validity_map_1500.txt') VOID_TEST_GROUND_TRUTH_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_ground_truth_1500.txt') VOID_TEST_INTRINSICS_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_test_intrinsics_1500.txt') -# VOID unused testing set 150 density -VOID_UNUSED_IMAGE_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_image_150.txt') -VOID_UNUSED_SPARSE_DEPTH_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_sparse_depth_150.txt') -VOID_UNUSED_VALIDITY_MAP_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_validity_map_150.txt') -VOID_UNUSED_GROUND_TRUTH_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_ground_truth_150.txt') -VOID_UNUSED_INTRINSICS_150_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_intrinsics_150.txt') -# VOID unused testing set 500 density -VOID_UNUSED_IMAGE_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_image_500.txt') -VOID_UNUSED_SPARSE_DEPTH_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_sparse_depth_500.txt') -VOID_UNUSED_VALIDITY_MAP_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_validity_map_500.txt') -VOID_UNUSED_GROUND_TRUTH_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_ground_truth_500.txt') -VOID_UNUSED_INTRINSICS_500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_intrinsics_500.txt') # VOID unused testing set 1500 density VOID_UNUSED_IMAGE_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_image_1500.txt') +VOID_UNUSED_POSE_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_pose_1500.txt') VOID_UNUSED_SPARSE_DEPTH_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_sparse_depth_1500.txt') VOID_UNUSED_VALIDITY_MAP_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_validity_map_1500.txt') VOID_UNUSED_GROUND_TRUTH_1500_FILEPATH = os.path.join(TEST_REFS_DIRPATH, 'void_unused_ground_truth_1500.txt') @@ -108,6 +76,9 @@ def process_frame(inputs): image path at time t=0, image path at time t=1, image path at time t=-1, + pose path at time t=0 + pose path at time t=1 + pose path at time t=-1 sparse depth path at time t=0, validity map path at time t=0, ground truth path at time t=0, @@ -123,6 +94,9 @@ def process_frame(inputs): image_path1, \ image_path0, \ image_path2, \ + pose_path1, \ + pose_path0, \ + pose_path2, \ sparse_depth_path, \ validity_map_path, \ ground_truth_path, \ @@ -134,33 +108,44 @@ def process_frame(inputs): image0 = cv2.imread(image_path0) image2 = cv2.imread(image_path2) imagec = np.concatenate([image1, image0, image2], axis=1) - + # Store poses of image triplet + pose1 = np.loadtxt(pose_path1) + pose0 = np.loadtxt(pose_path0) + pose2 = np.loadtxt(pose_path2) + posec = np.stack((pose1, pose0, pose2)) + posec = posec.reshape(posec.shape[0],-1) # Get validity map sparse_depth, validity_map = data_utils.load_depth_with_validity_map(sparse_depth_path) - image_refpath = os.path.join(*image_path0.split(os.sep)[2:]) - + # print("IMAGE PATH: ", *image_path0.split(os.sep)[8:]) + image_refpath = os.path.join(*image_path0.split(os.sep)[8:]) + pose_refpath = os.path.join(*pose_path0.split(os.sep)[8:]) # Set output paths image_outpath = os.path.join(VOID_OUTPUT_DIRPATH, image_refpath) + pose_outpath = os.path.join(VOID_OUTPUT_DIRPATH, pose_refpath) sparse_depth_outpath = sparse_depth_path validity_map_outpath = validity_map_path ground_truth_outpath = ground_truth_path # Verify that all filenames match image_out_dirpath, image_filename = os.path.split(image_outpath) + pose_out_dirpath, pose_filename = os.path.split(pose_outpath) sparse_depth_filename = os.path.basename(sparse_depth_outpath) validity_map_filename = os.path.basename(validity_map_outpath) ground_truth_filename = os.path.basename(ground_truth_outpath) + assert os.path.splitext(image_filename)[0] == os.path.splitext(pose_filename)[0] assert image_filename == sparse_depth_filename assert image_filename == validity_map_filename assert image_filename == ground_truth_filename if not paths_only: cv2.imwrite(image_outpath, imagec) - + np.savetxt(pose_outpath, posec) return (image_refpath, image_outpath, + pose_refpath, + pose_outpath, sparse_depth_outpath, validity_map_outpath, ground_truth_outpath) @@ -174,28 +159,13 @@ def process_frame(inputs): data_dirpaths = [ - VOID_DATA_150_DIRPATH, - VOID_DATA_500_DIRPATH, VOID_DATA_1500_DIRPATH ] train_output_filepaths = [ - [ - VOID_TRAIN_IMAGE_150_FILEPATH, - VOID_TRAIN_SPARSE_DEPTH_150_FILEPATH, - VOID_TRAIN_VALIDITY_MAP_150_FILEPATH, - VOID_TRAIN_GROUND_TRUTH_150_FILEPATH, - VOID_TRAIN_INTRINSICS_150_FILEPATH - ], - [ - VOID_TRAIN_IMAGE_500_FILEPATH, - VOID_TRAIN_SPARSE_DEPTH_500_FILEPATH, - VOID_TRAIN_VALIDITY_MAP_500_FILEPATH, - VOID_TRAIN_GROUND_TRUTH_500_FILEPATH, - VOID_TRAIN_INTRINSICS_500_FILEPATH - ], [ VOID_TRAIN_IMAGE_1500_FILEPATH, + VOID_TRAIN_POSE_1500_FILEPATH, VOID_TRAIN_SPARSE_DEPTH_1500_FILEPATH, VOID_TRAIN_VALIDITY_MAP_1500_FILEPATH, VOID_TRAIN_GROUND_TRUTH_1500_FILEPATH, @@ -203,22 +173,9 @@ def process_frame(inputs): ] ] test_output_filepaths = [ - [ - VOID_TEST_IMAGE_150_FILEPATH, - VOID_TEST_SPARSE_DEPTH_150_FILEPATH, - VOID_TEST_VALIDITY_MAP_150_FILEPATH, - VOID_TEST_GROUND_TRUTH_150_FILEPATH, - VOID_TEST_INTRINSICS_150_FILEPATH - ], - [ - VOID_TEST_IMAGE_500_FILEPATH, - VOID_TEST_SPARSE_DEPTH_500_FILEPATH, - VOID_TEST_VALIDITY_MAP_500_FILEPATH, - VOID_TEST_GROUND_TRUTH_500_FILEPATH, - VOID_TEST_INTRINSICS_500_FILEPATH - ], [ VOID_TEST_IMAGE_1500_FILEPATH, + VOID_TEST_POSE_1500_FILEPATH, VOID_TEST_SPARSE_DEPTH_1500_FILEPATH, VOID_TEST_VALIDITY_MAP_1500_FILEPATH, VOID_TEST_GROUND_TRUTH_1500_FILEPATH, @@ -226,22 +183,9 @@ def process_frame(inputs): ] ] unused_output_filepaths = [ - [ - VOID_UNUSED_IMAGE_150_FILEPATH, - VOID_UNUSED_SPARSE_DEPTH_150_FILEPATH, - VOID_UNUSED_VALIDITY_MAP_150_FILEPATH, - VOID_UNUSED_GROUND_TRUTH_150_FILEPATH, - VOID_UNUSED_INTRINSICS_150_FILEPATH - ], - [ - VOID_UNUSED_IMAGE_500_FILEPATH, - VOID_UNUSED_SPARSE_DEPTH_500_FILEPATH, - VOID_UNUSED_VALIDITY_MAP_500_FILEPATH, - VOID_UNUSED_GROUND_TRUTH_500_FILEPATH, - VOID_UNUSED_INTRINSICS_500_FILEPATH - ], [ VOID_UNUSED_IMAGE_1500_FILEPATH, + VOID_UNUSED_POSE_1500_FILEPATH, VOID_UNUSED_SPARSE_DEPTH_1500_FILEPATH, VOID_UNUSED_VALIDITY_MAP_1500_FILEPATH, VOID_UNUSED_GROUND_TRUTH_1500_FILEPATH, @@ -249,16 +193,17 @@ def process_frame(inputs): ] ] -for dirpath in [TRAIN_REFS_DIRPATH, TEST_REFS_DIRPATH]: +for dirpath in tqdm([TRAIN_REFS_DIRPATH, TEST_REFS_DIRPATH]): if not os.path.exists(dirpath): os.makedirs(dirpath) data_filepaths = \ zip(data_dirpaths, train_output_filepaths, test_output_filepaths, unused_output_filepaths) -for data_dirpath, train_filepaths, test_filepaths, unused_filepaths in data_filepaths: +for data_dirpath, train_filepaths, test_filepaths, unused_filepaths in tqdm(data_filepaths): # Training set train_image_filepath = os.path.join(data_dirpath, VOID_TRAIN_IMAGE_FILENAME) + train_pose_filepath = os.path.join(data_dirpath, VOID_TRAIN_POSE_FILENAME) train_sparse_depth_filepath = os.path.join(data_dirpath, VOID_TRAIN_SPARSE_DEPTH_FILENAME) train_validity_map_filepath = os.path.join(data_dirpath, VOID_TRAIN_VALIDITY_MAP_FILENAME) train_ground_truth_filepath = os.path.join(data_dirpath, VOID_TRAIN_GROUND_TRUTH_FILENAME) @@ -266,11 +211,19 @@ def process_frame(inputs): # Read training paths train_image_paths = data_utils.read_paths(train_image_filepath) + # combined_trainImage = '\t'.join(train_image_paths) + train_pose_paths = data_utils.read_paths(train_pose_filepath) + # combined_trainPose = '\t'.join(train_pose_paths) train_sparse_depth_paths = data_utils.read_paths(train_sparse_depth_filepath) + # combined_trainSparse = '\t'.join(train_sparse_depth_paths) train_validity_map_paths = data_utils.read_paths(train_validity_map_filepath) + # combined_trainVal = '\t'.join(train_validity_map_paths) train_ground_truth_paths = data_utils.read_paths(train_ground_truth_filepath) + # combined_trainGT = '\t'.join(train_ground_truth_paths) train_intrinsics_paths = data_utils.read_paths(train_intrinsics_filepath) + # combined_trainIn = '\t'.join(train_intrinsics_paths) + assert len(train_image_paths) == len(train_pose_paths) assert len(train_image_paths) == len(train_sparse_depth_paths) assert len(train_image_paths) == len(train_validity_map_paths) assert len(train_image_paths) == len(train_ground_truth_paths) @@ -278,6 +231,7 @@ def process_frame(inputs): # Testing set test_image_filepath = os.path.join(data_dirpath, VOID_TEST_IMAGE_FILENAME) + test_pose_filepath = os.path.join(data_dirpath, VOID_TEST_POSE_FILENAME) test_sparse_depth_filepath = os.path.join(data_dirpath, VOID_TEST_SPARSE_DEPTH_FILENAME) test_validity_map_filepath = os.path.join(data_dirpath, VOID_TEST_VALIDITY_MAP_FILENAME) test_ground_truth_filepath = os.path.join(data_dirpath, VOID_TEST_GROUND_TRUTH_FILENAME) @@ -285,11 +239,13 @@ def process_frame(inputs): # Read testing paths test_image_paths = data_utils.read_paths(test_image_filepath) + test_pose_paths = data_utils.read_paths(test_pose_filepath) test_sparse_depth_paths = data_utils.read_paths(test_sparse_depth_filepath) test_validity_map_paths = data_utils.read_paths(test_validity_map_filepath) test_ground_truth_paths = data_utils.read_paths(test_ground_truth_filepath) test_intrinsics_paths = data_utils.read_paths(test_intrinsics_filepath) + assert len(test_image_paths) == len(test_pose_paths) assert len(test_image_paths) == len(test_sparse_depth_paths) assert len(test_image_paths) == len(test_validity_map_paths) assert len(test_image_paths) == len(test_ground_truth_paths) @@ -301,6 +257,7 @@ def process_frame(inputs): # Initialize placeholders for training output paths train_image_outpaths = [] + train_pose_outpaths = [] train_sparse_depth_outpaths = [] train_validity_map_outpaths = [] train_ground_truth_outpaths = [] @@ -308,6 +265,7 @@ def process_frame(inputs): # Initialize placeholders for testing output paths test_image_outpaths = [] + test_pose_outpaths = [] test_sparse_depth_outpaths = [] test_validity_map_outpaths = [] test_ground_truth_outpaths = [] @@ -315,6 +273,7 @@ def process_frame(inputs): # Initialize placeholders for unused testing output paths unused_image_outpaths = [] + unused_pose_outpaths = [] unused_sparse_depth_outpaths = [] unused_validity_map_outpaths = [] unused_ground_truth_outpaths = [] @@ -324,10 +283,12 @@ def process_frame(inputs): seq_dirpaths = glob.glob(os.path.join(data_dirpath, 'data', '*')) n_sample = 0 - for seq_dirpath in seq_dirpaths: + for seq_dirpath in tqdm(seq_dirpaths): # For each sequence, grab the images, sparse depths and valid maps image_paths = \ sorted(glob.glob(os.path.join(seq_dirpath, 'image', '*.png'))) + pose_paths = \ + sorted(glob.glob(os.path.join(seq_dirpath, 'absolute_pose', '*.txt'))) sparse_depth_paths = \ sorted(glob.glob(os.path.join(seq_dirpath, 'sparse_depth', '*.png'))) validity_map_paths = \ @@ -336,6 +297,7 @@ def process_frame(inputs): sorted(glob.glob(os.path.join(seq_dirpath, 'ground_truth', '*.png'))) intrinsics_path = os.path.join(seq_dirpath, 'K.txt') + assert len(image_paths) == len(pose_paths) assert len(image_paths) == len(sparse_depth_paths) assert len(image_paths) == len(validity_map_paths) @@ -343,14 +305,20 @@ def process_frame(inputs): kin = np.loadtxt(intrinsics_path) intrinsics_refpath = \ - os.path.join(*intrinsics_path.split(os.sep)[2:]) + os.path.join(*intrinsics_path.split(os.sep)[8:]) intrinsics_outpath = \ os.path.join(VOID_OUTPUT_DIRPATH, intrinsics_refpath[:-3]+'npy') image_out_dirpath = \ os.path.join(os.path.dirname(intrinsics_outpath), 'image') + pose_out_dirpath = \ + os.path.join(os.path.dirname(intrinsics_outpath), 'absolute_pose') + + # print(image_out_dirpath) if not os.path.exists(image_out_dirpath): os.makedirs(image_out_dirpath) + if not os.path.exists(pose_out_dirpath): + os.makedirs(pose_out_dirpath) # Save intrinsics np.save(intrinsics_outpath, kin) @@ -364,11 +332,14 @@ def process_frame(inputs): offset_idx = 10 pool_input = [] - for idx in range(start_idx, len(image_paths)-offset_idx-start_idx): + for idx in tqdm(range(start_idx, len(image_paths)-offset_idx-start_idx)): pool_input.append(( image_paths[idx-offset_idx], image_paths[idx], image_paths[idx+offset_idx], + pose_paths[idx-offset_idx], + pose_paths[idx], + pose_paths[idx+offset_idx], sparse_depth_paths[idx], validity_map_paths[idx], ground_truth_paths[idx], @@ -377,9 +348,11 @@ def process_frame(inputs): with mp.Pool() as pool: pool_results = pool.map(process_frame, pool_input) - for result in pool_results: + for result in tqdm(pool_results): image_refpath, \ image_outpath, \ + pose_refpath, \ + pose_outpath, \ sparse_depth_outpath, \ validity_map_outpath, \ ground_truth_outpath = result @@ -387,18 +360,21 @@ def process_frame(inputs): # Split into training, testing and unused testing sets if image_refpath in train_image_paths: train_image_outpaths.append(image_outpath) + train_pose_outpaths.append(pose_outpath) train_sparse_depth_outpaths.append(sparse_depth_outpath) train_validity_map_outpaths.append(validity_map_outpath) train_ground_truth_outpaths.append(ground_truth_outpath) train_intrinsics_outpaths.append(intrinsics_outpath) elif image_refpath in test_image_paths: test_image_outpaths.append(image_outpath) + test_pose_outpaths.append(pose_outpath) test_sparse_depth_outpaths.append(sparse_depth_outpath) test_validity_map_outpaths.append(validity_map_outpath) test_ground_truth_outpaths.append(ground_truth_outpath) test_intrinsics_outpaths.append(intrinsics_outpath) else: unused_image_outpaths.append(image_outpath) + unused_pose_outpaths.append(pose_outpath) unused_sparse_depth_outpaths.append(sparse_depth_outpath) unused_validity_map_outpaths.append(validity_map_outpath) unused_ground_truth_outpaths.append(ground_truth_outpath) @@ -412,6 +388,7 @@ def process_frame(inputs): print('Completed processing {} examples for density={}'.format(n_sample, data_dirpath)) void_train_image_filepath, \ + void_train_pose_filepath, \ void_train_sparse_depth_filepath, \ void_train_validity_map_filepath, \ void_train_ground_truth_filepath, \ @@ -422,6 +399,11 @@ def process_frame(inputs): data_utils.write_paths( void_train_image_filepath, train_image_outpaths) + print('Storing {} training pose file paths into: {}'.format( + len(train_pose_outpaths), void_train_pose_filepath)) + data_utils.write_paths( + void_train_pose_filepath, train_pose_outpaths) + print('Storing {} training sparse depth file paths into: {}'.format( len(train_sparse_depth_outpaths), void_train_sparse_depth_filepath)) data_utils.write_paths( @@ -443,6 +425,7 @@ def process_frame(inputs): void_train_intrinsics_filepath, train_intrinsics_outpaths) void_test_image_filepath, \ + void_test_pose_filepath, \ void_test_sparse_depth_filepath, \ void_test_validity_map_filepath, \ void_test_ground_truth_filepath, \ @@ -453,6 +436,11 @@ def process_frame(inputs): data_utils.write_paths( void_test_image_filepath, test_image_outpaths) + print('Storing {} testing pose file paths into: {}'.format( + len(test_pose_outpaths), void_test_pose_filepath)) + data_utils.write_paths( + void_test_pose_filepath, test_pose_outpaths) + print('Storing {} testing sparse depth file paths into: {}'.format( len(test_sparse_depth_outpaths), void_test_sparse_depth_filepath)) data_utils.write_paths( @@ -474,6 +462,7 @@ def process_frame(inputs): void_test_intrinsics_filepath, test_intrinsics_outpaths) void_unused_image_filepath, \ + void_unused_pose_filepath, \ void_unused_sparse_depth_filepath, \ void_unused_validity_map_filepath, \ void_unused_ground_truth_filepath, \ @@ -484,6 +473,11 @@ def process_frame(inputs): data_utils.write_paths( void_unused_image_filepath, unused_image_outpaths) + print('Storing {} unused testing pose file paths into: {}'.format( + len(unused_pose_outpaths), void_unused_pose_filepath)) + data_utils.write_paths( + void_unused_pose_filepath, unused_pose_outpaths) + print('Storing {} unused testing sparse depth file paths into: {}'.format( len(unused_sparse_depth_outpaths), void_unused_sparse_depth_filepath)) data_utils.write_paths( @@ -502,4 +496,4 @@ def process_frame(inputs): print('Storing {} unused testing camera intrinsics file paths into: {}'.format( len(unused_intrinsics_outpaths), void_unused_intrinsics_filepath)) data_utils.write_paths( - void_unused_intrinsics_filepath, unused_intrinsics_outpaths) + void_unused_intrinsics_filepath, unused_intrinsics_outpaths) \ No newline at end of file diff --git a/setup/store_corresponding_data.py b/setup/store_corresponding_data.py new file mode 100644 index 0000000..d4f9305 --- /dev/null +++ b/setup/store_corresponding_data.py @@ -0,0 +1,64 @@ +import sys +import pathlib +import numpy as np +from numpy.core.fromnumeric import argmin +import shutil +from natsort import natsorted +from tqdm import tqdm + +outer_folder = sys.argv[1] +outer_folder_path = pathlib.Path(outer_folder) + +# list(pathlib.Path('your_directory').glob('*.txt')) +subFolders = list( outer_folder_path.glob('*') ) + +subFolder_images = [] +subFolder_times = [] +subFolder_images_lens = [] + +for subF in subFolders: + corres_folder = pathlib.Path(str(subF)+'/corresponding_images') + try: + corres_folder.mkdir(parents=True,exist_ok=True) + except: + corres_folder.mkdir(parents=True) + + images_paths_list = natsorted(list( subF.glob('*jpg') )) + pngs = natsorted([image for image in subF.glob('*png')]) + for png in pngs: + images_paths_list.append(png) + + times = natsorted([int(image_path.stem) for image_path in images_paths_list]) + subFolder_images.append(images_paths_list) + subFolder_images_lens.append(len(images_paths_list)) + subFolder_times.append(times) + +min_folder = np.argmin(subFolder_images_lens) + +print('Minimum images in folder: {}'.format(subFolders[min_folder].stem )) +count = 1 +for iter,image_path in tqdm (enumerate(subFolder_images[min_folder]), total=len(subFolder_images[min_folder])): + image_time = subFolder_times[min_folder][iter] + subFolder = image_path.parent + + new_filename = '/corresponding_images/' + str(count) + image_path.suffix + new_file_str = str(subFolder)+new_filename + new_file_path = pathlib.Path(new_file_str) + + if not new_file_path.exists(): + shutil.copy(image_path, new_file_path) + + for i in range(len(subFolders)): + if i == min_folder: + continue + diff = [abs(subF_time - image_time) for subF_time in subFolder_times[i]] + min_arg = np.argmin(diff) + subF_image_path = subFolder_images[i][min_arg] + + new_filename = '/corresponding_images/'+str(count) + subF_image_path.suffix + new_file_str = str(subFolders[i])+new_filename + new_file_path = pathlib.Path(new_file_str) + if not new_file_path.exists(): + shutil.copy(subF_image_path, new_file_path) + count+=1 + diff --git a/setup/vlslam_pb2.py b/setup/vlslam_pb2.py new file mode 100755 index 0000000..8b3d062 --- /dev/null +++ b/setup/vlslam_pb2.py @@ -0,0 +1,898 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: vlslam.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +from google.protobuf import descriptor_pb2 +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='vlslam.proto', + package='vlslam_pb', + serialized_pb=_b('\n\x0cvlslam.proto\x12\tvlslam_pb\"\xe7\x04\n\nCameraInfo\x12\x0c\n\x04rows\x18\x01 \x02(\x05\x12\x0c\n\x04\x63ols\x18\x02 \x02(\x05\x12\x30\n\x07pinhole\x18\x05 \x01(\x0b\x32\x1d.vlslam_pb.CameraInfo.PinholeH\x00\x12\x30\n\x03\x66ov\x18\x06 \x01(\x0b\x32!.vlslam_pb.CameraInfo.FieldOfViewH\x00\x12.\n\x06radtan\x18\x07 \x01(\x0b\x32\x1c.vlslam_pb.CameraInfo.RadTanH\x00\x12\x31\n\x04\x65qui\x18\x08 \x01(\x0b\x32!.vlslam_pb.CameraInfo.EquidistantH\x00\x1a\x39\n\x07Pinhole\x12\n\n\x02\x66x\x18\x01 \x02(\x01\x12\n\n\x02\x66y\x18\x02 \x02(\x01\x12\n\n\x02\x63x\x18\x03 \x02(\x01\x12\n\n\x02\x63y\x18\x04 \x02(\x01\x1aH\n\x0b\x46ieldOfView\x12\n\n\x02\x66x\x18\x01 \x02(\x01\x12\n\n\x02\x66y\x18\x02 \x02(\x01\x12\n\n\x02\x63x\x18\x03 \x02(\x01\x12\n\n\x02\x63y\x18\x04 \x02(\x01\x12\t\n\x01w\x18\x05 \x02(\x01\x1at\n\x06RadTan\x12\n\n\x02\x66x\x18\x01 \x02(\x01\x12\n\n\x02\x66y\x18\x02 \x02(\x01\x12\n\n\x02\x63x\x18\x03 \x02(\x01\x12\n\n\x02\x63y\x18\x04 \x02(\x01\x12\n\n\x02r0\x18\x05 \x02(\x01\x12\n\n\x02r1\x18\x06 \x02(\x01\x12\n\n\x02k0\x18\x07 \x02(\x01\x12\n\n\x02k1\x18\x08 \x02(\x01\x12\n\n\x02k2\x18\t \x02(\x01\x1am\n\x0b\x45quidistant\x12\n\n\x02\x66x\x18\x01 \x02(\x01\x12\n\n\x02\x66y\x18\x02 \x02(\x01\x12\n\n\x02\x63x\x18\x03 \x02(\x01\x12\n\n\x02\x63y\x18\x04 \x02(\x01\x12\n\n\x02k0\x18\x07 \x02(\x01\x12\n\n\x02k1\x18\x08 \x02(\x01\x12\n\n\x02k2\x18\t \x02(\x01\x12\n\n\x02k3\x18\n \x02(\x01\x42\x0c\n\ndistortion\"\xc6\x01\n\x07\x46\x65\x61ture\x12\n\n\x02id\x18\x01 \x02(\x03\x12)\n\x06status\x18\x02 \x02(\x0e\x32\x19.vlslam_pb.Feature.Status\x12\n\n\x02xp\x18\x03 \x03(\x01\x12\n\n\x02xw\x18\x04 \x03(\x01\x12\t\n\x01z\x18\x05 \x01(\x01\"a\n\x06Status\x12\t\n\x05\x45MPTY\x10\x00\x12\x0c\n\x08GOODDROP\x10\x01\x12\x08\n\x04KEEP\x10\x02\x12\n\n\x06REJECT\x10\x03\x12\x10\n\x0cINITIALIZING\x10\x04\x12\t\n\x05READY\x10\x05\x12\x0b\n\x07INSTATE\x10\x06\"S\n\x06Packet\x12\n\n\x02ts\x18\x01 \x02(\x01\x12\x0b\n\x03gwc\x18\x02 \x03(\x01\x12$\n\x08\x66\x65\x61tures\x18\x03 \x03(\x0b\x32\x12.vlslam_pb.Feature\x12\n\n\x02wg\x18\x04 \x03(\x01\":\n\x05Track\x12\n\n\x02ts\x18\x01 \x02(\x01\x12%\n\ttracklets\x18\x02 \x03(\x0b\x32\x12.vlslam_pb.Feature\"\x8b\x01\n\x07\x44\x61taset\x12\x13\n\x0b\x64\x65scription\x18\x01 \x01(\t\x12%\n\x06\x63\x61mera\x18\x02 \x02(\x0b\x32\x15.vlslam_pb.CameraInfo\x12\"\n\x07packets\x18\x03 \x03(\x0b\x32\x11.vlslam_pb.Packet\x12 \n\x06tracks\x18\x04 \x03(\x0b\x32\x10.vlslam_pb.Track\"H\n\x07\x45\x64geMap\x12\x13\n\x0b\x64\x65scription\x18\x01 \x01(\t\x12\x0c\n\x04rows\x18\x02 \x02(\x05\x12\x0c\n\x04\x63ols\x18\x03 \x02(\x05\x12\x0c\n\x04\x64\x61ta\x18\x04 \x03(\x02\"\xa9\x01\n\x0b\x42oundingBox\x12\x12\n\ntop_left_x\x18\x01 \x02(\x02\x12\x12\n\ntop_left_y\x18\x02 \x02(\x02\x12\x16\n\x0e\x62ottom_right_x\x18\x03 \x02(\x02\x12\x16\n\x0e\x62ottom_right_y\x18\x04 \x02(\x02\x12\x0e\n\x06scores\x18\x05 \x03(\x02\x12\x12\n\nclass_name\x18\x06 \x01(\t\x12\r\n\x05label\x18\x07 \x01(\x05\x12\x0f\n\x07\x61zimuth\x18\x08 \x01(\x02\"V\n\x0f\x42oundingBoxList\x12\x13\n\x0b\x64\x65scription\x18\x01 \x01(\t\x12.\n\x0e\x62ounding_boxes\x18\x02 \x03(\x0b\x32\x16.vlslam_pb.BoundingBox') +) +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + + + +_FEATURE_STATUS = _descriptor.EnumDescriptor( + name='Status', + full_name='vlslam_pb.Feature.Status', + filename=None, + file=DESCRIPTOR, + values=[ + _descriptor.EnumValueDescriptor( + name='EMPTY', index=0, number=0, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='GOODDROP', index=1, number=1, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='KEEP', index=2, number=2, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='REJECT', index=3, number=3, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='INITIALIZING', index=4, number=4, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='READY', index=5, number=5, + options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='INSTATE', index=6, number=6, + options=None, + type=None), + ], + containing_type=None, + options=None, + serialized_start=747, + serialized_end=844, +) +_sym_db.RegisterEnumDescriptor(_FEATURE_STATUS) + + +_CAMERAINFO_PINHOLE = _descriptor.Descriptor( + name='Pinhole', + full_name='vlslam_pb.CameraInfo.Pinhole', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='fx', full_name='vlslam_pb.CameraInfo.Pinhole.fx', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='fy', full_name='vlslam_pb.CameraInfo.Pinhole.fy', index=1, + number=2, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cx', full_name='vlslam_pb.CameraInfo.Pinhole.cx', index=2, + number=3, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cy', full_name='vlslam_pb.CameraInfo.Pinhole.cy', index=3, + number=4, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=269, + serialized_end=326, +) + +_CAMERAINFO_FIELDOFVIEW = _descriptor.Descriptor( + name='FieldOfView', + full_name='vlslam_pb.CameraInfo.FieldOfView', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='fx', full_name='vlslam_pb.CameraInfo.FieldOfView.fx', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='fy', full_name='vlslam_pb.CameraInfo.FieldOfView.fy', index=1, + number=2, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cx', full_name='vlslam_pb.CameraInfo.FieldOfView.cx', index=2, + number=3, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cy', full_name='vlslam_pb.CameraInfo.FieldOfView.cy', index=3, + number=4, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='w', full_name='vlslam_pb.CameraInfo.FieldOfView.w', index=4, + number=5, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=328, + serialized_end=400, +) + +_CAMERAINFO_RADTAN = _descriptor.Descriptor( + name='RadTan', + full_name='vlslam_pb.CameraInfo.RadTan', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='fx', full_name='vlslam_pb.CameraInfo.RadTan.fx', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='fy', full_name='vlslam_pb.CameraInfo.RadTan.fy', index=1, + number=2, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cx', full_name='vlslam_pb.CameraInfo.RadTan.cx', index=2, + number=3, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cy', full_name='vlslam_pb.CameraInfo.RadTan.cy', index=3, + number=4, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='r0', full_name='vlslam_pb.CameraInfo.RadTan.r0', index=4, + number=5, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='r1', full_name='vlslam_pb.CameraInfo.RadTan.r1', index=5, + number=6, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k0', full_name='vlslam_pb.CameraInfo.RadTan.k0', index=6, + number=7, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k1', full_name='vlslam_pb.CameraInfo.RadTan.k1', index=7, + number=8, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k2', full_name='vlslam_pb.CameraInfo.RadTan.k2', index=8, + number=9, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=402, + serialized_end=518, +) + +_CAMERAINFO_EQUIDISTANT = _descriptor.Descriptor( + name='Equidistant', + full_name='vlslam_pb.CameraInfo.Equidistant', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='fx', full_name='vlslam_pb.CameraInfo.Equidistant.fx', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='fy', full_name='vlslam_pb.CameraInfo.Equidistant.fy', index=1, + number=2, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cx', full_name='vlslam_pb.CameraInfo.Equidistant.cx', index=2, + number=3, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cy', full_name='vlslam_pb.CameraInfo.Equidistant.cy', index=3, + number=4, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k0', full_name='vlslam_pb.CameraInfo.Equidistant.k0', index=4, + number=7, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k1', full_name='vlslam_pb.CameraInfo.Equidistant.k1', index=5, + number=8, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k2', full_name='vlslam_pb.CameraInfo.Equidistant.k2', index=6, + number=9, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='k3', full_name='vlslam_pb.CameraInfo.Equidistant.k3', index=7, + number=10, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=520, + serialized_end=629, +) + +_CAMERAINFO = _descriptor.Descriptor( + name='CameraInfo', + full_name='vlslam_pb.CameraInfo', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='rows', full_name='vlslam_pb.CameraInfo.rows', index=0, + number=1, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cols', full_name='vlslam_pb.CameraInfo.cols', index=1, + number=2, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='pinhole', full_name='vlslam_pb.CameraInfo.pinhole', index=2, + number=5, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='fov', full_name='vlslam_pb.CameraInfo.fov', index=3, + number=6, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='radtan', full_name='vlslam_pb.CameraInfo.radtan', index=4, + number=7, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='equi', full_name='vlslam_pb.CameraInfo.equi', index=5, + number=8, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[_CAMERAINFO_PINHOLE, _CAMERAINFO_FIELDOFVIEW, _CAMERAINFO_RADTAN, _CAMERAINFO_EQUIDISTANT, ], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + _descriptor.OneofDescriptor( + name='distortion', full_name='vlslam_pb.CameraInfo.distortion', + index=0, containing_type=None, fields=[]), + ], + serialized_start=28, + serialized_end=643, +) + + +_FEATURE = _descriptor.Descriptor( + name='Feature', + full_name='vlslam_pb.Feature', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='id', full_name='vlslam_pb.Feature.id', index=0, + number=1, type=3, cpp_type=2, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='status', full_name='vlslam_pb.Feature.status', index=1, + number=2, type=14, cpp_type=8, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='xp', full_name='vlslam_pb.Feature.xp', index=2, + number=3, type=1, cpp_type=5, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='xw', full_name='vlslam_pb.Feature.xw', index=3, + number=4, type=1, cpp_type=5, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='z', full_name='vlslam_pb.Feature.z', index=4, + number=5, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + _FEATURE_STATUS, + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=646, + serialized_end=844, +) + + +_PACKET = _descriptor.Descriptor( + name='Packet', + full_name='vlslam_pb.Packet', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='ts', full_name='vlslam_pb.Packet.ts', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='gwc', full_name='vlslam_pb.Packet.gwc', index=1, + number=2, type=1, cpp_type=5, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='features', full_name='vlslam_pb.Packet.features', index=2, + number=3, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='wg', full_name='vlslam_pb.Packet.wg', index=3, + number=4, type=1, cpp_type=5, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=846, + serialized_end=929, +) + + +_TRACK = _descriptor.Descriptor( + name='Track', + full_name='vlslam_pb.Track', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='ts', full_name='vlslam_pb.Track.ts', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='tracklets', full_name='vlslam_pb.Track.tracklets', index=1, + number=2, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=931, + serialized_end=989, +) + + +_DATASET = _descriptor.Descriptor( + name='Dataset', + full_name='vlslam_pb.Dataset', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='description', full_name='vlslam_pb.Dataset.description', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=_b("").decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='camera', full_name='vlslam_pb.Dataset.camera', index=1, + number=2, type=11, cpp_type=10, label=2, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='packets', full_name='vlslam_pb.Dataset.packets', index=2, + number=3, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='tracks', full_name='vlslam_pb.Dataset.tracks', index=3, + number=4, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=992, + serialized_end=1131, +) + + +_EDGEMAP = _descriptor.Descriptor( + name='EdgeMap', + full_name='vlslam_pb.EdgeMap', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='description', full_name='vlslam_pb.EdgeMap.description', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=_b("").decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='rows', full_name='vlslam_pb.EdgeMap.rows', index=1, + number=2, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='cols', full_name='vlslam_pb.EdgeMap.cols', index=2, + number=3, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='data', full_name='vlslam_pb.EdgeMap.data', index=3, + number=4, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=1133, + serialized_end=1205, +) + + +_BOUNDINGBOX = _descriptor.Descriptor( + name='BoundingBox', + full_name='vlslam_pb.BoundingBox', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='top_left_x', full_name='vlslam_pb.BoundingBox.top_left_x', index=0, + number=1, type=2, cpp_type=6, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='top_left_y', full_name='vlslam_pb.BoundingBox.top_left_y', index=1, + number=2, type=2, cpp_type=6, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='bottom_right_x', full_name='vlslam_pb.BoundingBox.bottom_right_x', index=2, + number=3, type=2, cpp_type=6, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='bottom_right_y', full_name='vlslam_pb.BoundingBox.bottom_right_y', index=3, + number=4, type=2, cpp_type=6, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='scores', full_name='vlslam_pb.BoundingBox.scores', index=4, + number=5, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='class_name', full_name='vlslam_pb.BoundingBox.class_name', index=5, + number=6, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=_b("").decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='label', full_name='vlslam_pb.BoundingBox.label', index=6, + number=7, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='azimuth', full_name='vlslam_pb.BoundingBox.azimuth', index=7, + number=8, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=1208, + serialized_end=1377, +) + + +_BOUNDINGBOXLIST = _descriptor.Descriptor( + name='BoundingBoxList', + full_name='vlslam_pb.BoundingBoxList', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='description', full_name='vlslam_pb.BoundingBoxList.description', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=_b("").decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + _descriptor.FieldDescriptor( + name='bounding_boxes', full_name='vlslam_pb.BoundingBoxList.bounding_boxes', index=1, + number=2, type=11, cpp_type=10, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + options=None), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + options=None, + is_extendable=False, + extension_ranges=[], + oneofs=[ + ], + serialized_start=1379, + serialized_end=1465, +) + +_CAMERAINFO_PINHOLE.containing_type = _CAMERAINFO +_CAMERAINFO_FIELDOFVIEW.containing_type = _CAMERAINFO +_CAMERAINFO_RADTAN.containing_type = _CAMERAINFO +_CAMERAINFO_EQUIDISTANT.containing_type = _CAMERAINFO +_CAMERAINFO.fields_by_name['pinhole'].message_type = _CAMERAINFO_PINHOLE +_CAMERAINFO.fields_by_name['fov'].message_type = _CAMERAINFO_FIELDOFVIEW +_CAMERAINFO.fields_by_name['radtan'].message_type = _CAMERAINFO_RADTAN +_CAMERAINFO.fields_by_name['equi'].message_type = _CAMERAINFO_EQUIDISTANT +_CAMERAINFO.oneofs_by_name['distortion'].fields.append( + _CAMERAINFO.fields_by_name['pinhole']) +_CAMERAINFO.fields_by_name['pinhole'].containing_oneof = _CAMERAINFO.oneofs_by_name['distortion'] +_CAMERAINFO.oneofs_by_name['distortion'].fields.append( + _CAMERAINFO.fields_by_name['fov']) +_CAMERAINFO.fields_by_name['fov'].containing_oneof = _CAMERAINFO.oneofs_by_name['distortion'] +_CAMERAINFO.oneofs_by_name['distortion'].fields.append( + _CAMERAINFO.fields_by_name['radtan']) +_CAMERAINFO.fields_by_name['radtan'].containing_oneof = _CAMERAINFO.oneofs_by_name['distortion'] +_CAMERAINFO.oneofs_by_name['distortion'].fields.append( + _CAMERAINFO.fields_by_name['equi']) +_CAMERAINFO.fields_by_name['equi'].containing_oneof = _CAMERAINFO.oneofs_by_name['distortion'] +_FEATURE.fields_by_name['status'].enum_type = _FEATURE_STATUS +_FEATURE_STATUS.containing_type = _FEATURE +_PACKET.fields_by_name['features'].message_type = _FEATURE +_TRACK.fields_by_name['tracklets'].message_type = _FEATURE +_DATASET.fields_by_name['camera'].message_type = _CAMERAINFO +_DATASET.fields_by_name['packets'].message_type = _PACKET +_DATASET.fields_by_name['tracks'].message_type = _TRACK +_BOUNDINGBOXLIST.fields_by_name['bounding_boxes'].message_type = _BOUNDINGBOX +DESCRIPTOR.message_types_by_name['CameraInfo'] = _CAMERAINFO +DESCRIPTOR.message_types_by_name['Feature'] = _FEATURE +DESCRIPTOR.message_types_by_name['Packet'] = _PACKET +DESCRIPTOR.message_types_by_name['Track'] = _TRACK +DESCRIPTOR.message_types_by_name['Dataset'] = _DATASET +DESCRIPTOR.message_types_by_name['EdgeMap'] = _EDGEMAP +DESCRIPTOR.message_types_by_name['BoundingBox'] = _BOUNDINGBOX +DESCRIPTOR.message_types_by_name['BoundingBoxList'] = _BOUNDINGBOXLIST + +CameraInfo = _reflection.GeneratedProtocolMessageType('CameraInfo', (_message.Message,), dict( + + Pinhole = _reflection.GeneratedProtocolMessageType('Pinhole', (_message.Message,), dict( + DESCRIPTOR = _CAMERAINFO_PINHOLE, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.CameraInfo.Pinhole) + )) + , + + FieldOfView = _reflection.GeneratedProtocolMessageType('FieldOfView', (_message.Message,), dict( + DESCRIPTOR = _CAMERAINFO_FIELDOFVIEW, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.CameraInfo.FieldOfView) + )) + , + + RadTan = _reflection.GeneratedProtocolMessageType('RadTan', (_message.Message,), dict( + DESCRIPTOR = _CAMERAINFO_RADTAN, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.CameraInfo.RadTan) + )) + , + + Equidistant = _reflection.GeneratedProtocolMessageType('Equidistant', (_message.Message,), dict( + DESCRIPTOR = _CAMERAINFO_EQUIDISTANT, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.CameraInfo.Equidistant) + )) + , + DESCRIPTOR = _CAMERAINFO, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.CameraInfo) + )) +_sym_db.RegisterMessage(CameraInfo) +_sym_db.RegisterMessage(CameraInfo.Pinhole) +_sym_db.RegisterMessage(CameraInfo.FieldOfView) +_sym_db.RegisterMessage(CameraInfo.RadTan) +_sym_db.RegisterMessage(CameraInfo.Equidistant) + +Feature = _reflection.GeneratedProtocolMessageType('Feature', (_message.Message,), dict( + DESCRIPTOR = _FEATURE, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.Feature) + )) +_sym_db.RegisterMessage(Feature) + +Packet = _reflection.GeneratedProtocolMessageType('Packet', (_message.Message,), dict( + DESCRIPTOR = _PACKET, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.Packet) + )) +_sym_db.RegisterMessage(Packet) + +Track = _reflection.GeneratedProtocolMessageType('Track', (_message.Message,), dict( + DESCRIPTOR = _TRACK, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.Track) + )) +_sym_db.RegisterMessage(Track) + +Dataset = _reflection.GeneratedProtocolMessageType('Dataset', (_message.Message,), dict( + DESCRIPTOR = _DATASET, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.Dataset) + )) +_sym_db.RegisterMessage(Dataset) + +EdgeMap = _reflection.GeneratedProtocolMessageType('EdgeMap', (_message.Message,), dict( + DESCRIPTOR = _EDGEMAP, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.EdgeMap) + )) +_sym_db.RegisterMessage(EdgeMap) + +BoundingBox = _reflection.GeneratedProtocolMessageType('BoundingBox', (_message.Message,), dict( + DESCRIPTOR = _BOUNDINGBOX, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.BoundingBox) + )) +_sym_db.RegisterMessage(BoundingBox) + +BoundingBoxList = _reflection.GeneratedProtocolMessageType('BoundingBoxList', (_message.Message,), dict( + DESCRIPTOR = _BOUNDINGBOXLIST, + __module__ = 'vlslam_pb2' + # @@protoc_insertion_point(class_scope:vlslam_pb.BoundingBoxList) + )) +_sym_db.RegisterMessage(BoundingBoxList) + + +# @@protoc_insertion_point(module_scope) diff --git a/src/data_utils.py b/src/data_utils.py index abe6dcb..5026ada 100644 --- a/src/data_utils.py +++ b/src/data_utils.py @@ -237,4 +237,4 @@ def load_calibration(path): [float(x) for x in value.split(' ')]) except ValueError: pass - return data + return data \ No newline at end of file diff --git a/src/datasets.py b/src/datasets.py index f8cdb41..c2d6980 100644 --- a/src/datasets.py +++ b/src/datasets.py @@ -15,9 +15,51 @@ } ''' import numpy as np +import cv2 import torch.utils.data import data_utils +def load_pose_triplet(path,format='txt'): + ''' + Load and returns 4x4 pose matrices at timt t-1,t, t+1 + ''' + pose1=None + pose0=None + pose2=None + if format.lower()=='txt': + poses = np.loadtxt(path) + if poses.shape[1]==12: + poses = poses.reshape(poses.shape[0], poses.shape[1]//4, 4) + affine_row = np.asarray([0,0,0,1], dtype=poses.dtype) + + pose1 = poses[0,:,:].copy() + pose1 = np.asarray(pose1, dtype=np.float32) + pose1 = np.vstack((pose1, affine_row)) + pose0 = poses[1,:,:].copy() + pose0 = np.asarray(pose0, dtype=np.float32) + pose0 = np.vstack((pose0, affine_row)) + pose2 = poses[2,:,:].copy() + pose2 = np.asarray(pose2, dtype=np.float32) + pose2 = np.vstack((pose2, affine_row)) + elif poses.shape[1]==16: + poses = poses.reshape(poses.shape[0], poses.shape[1]//4, 4) + pose1 = poses[0,:,:].copy() + pose0 = poses[1,:,:].copy() + pose2 = poses[2,:,:].copy() + + + elif format.lower()=='cv_yml': + s = cv2.FileStorage() + _ = s.open(path, cv2.FileStorage_READ) + pnode = s.getNode('Poses') + poses = pnode.mat() + poses = poses.reshape(poses.shape[0], poses.shape[1]//4, 4) + if poses.shape[1]==4 and poses.shape[2]==4: + pose1 = poses[0,:,:].copy() + pose0 = poses[1,:,:].copy() + pose2 = poses[2,:,:].copy() + + return pose1, pose0, pose2 def load_image_triplet(path, normalize=True): ''' @@ -182,10 +224,12 @@ def __init__(self, image_paths, sparse_depth_paths, intrinsics_paths, + pose_paths=None, shape=None, random_crop_type=['none']): self.image_paths = image_paths + self.pose_paths = pose_paths self.sparse_depth_paths = sparse_depth_paths self.intrinsics_paths = intrinsics_paths @@ -222,8 +266,12 @@ def __getitem__(self, index): for T in [image0, image1, image2, sparse_depth0, intrinsics] ] - return image0, image1, image2, sparse_depth0, intrinsics - + if self.pose_paths is None: + return image0, image1, image2, sparse_depth0, intrinsics + else: + pose1, pose0, pose2 = load_pose_triplet(self.pose_paths[index]) + return image0, image1, image2, pose0, pose1, pose2, sparse_depth0, intrinsics + def __len__(self): return len(self.image_paths) diff --git a/src/global_constants.py b/src/global_constants.py index 8b7874c..a282a42 100644 --- a/src/global_constants.py +++ b/src/global_constants.py @@ -38,8 +38,8 @@ RESOLUTIONS_BACKPROJECTION = [0, 1, 2, 3] N_FILTERS_DECODER = [256, 128, 128, 64, 12] DECONV_TYPE = 'up' -MIN_PREDICT_DEPTH = 1.5 -MAX_PREDICT_DEPTH = 100.0 +MIN_PREDICT_DEPTH = 0.2 +MAX_PREDICT_DEPTH = 1000.0 # Weight settings WEIGHT_INITIALIZER = 'xavier_normal' diff --git a/src/kbnet.py b/src/kbnet.py index 886e4bc..d05a232 100644 --- a/src/kbnet.py +++ b/src/kbnet.py @@ -29,12 +29,14 @@ def train(train_image_path, + train_pose_path, train_sparse_depth_path, train_intrinsics_path, val_image_path, val_sparse_depth_path, val_intrinsics_path, val_ground_truth_path, + pose_in_world_frame = False, # Batch settings n_batch=settings.N_BATCH, n_height=settings.N_HEIGHT, @@ -119,12 +121,16 @@ def train(train_image_path, Load input paths and set up dataloaders ''' # Read paths for training + train_pose_paths = None + if not train_pose_path==" ": + train_pose_paths = data_utils.read_paths(train_pose_path) train_image_paths = data_utils.read_paths(train_image_path) train_sparse_depth_paths = data_utils.read_paths(train_sparse_depth_path) train_intrinsics_paths = data_utils.read_paths(train_intrinsics_path) n_train_sample = len(train_image_paths) - + if train_pose_paths is not None: + assert len(train_pose_paths) == n_train_sample assert len(train_sparse_depth_paths) == n_train_sample assert len(train_intrinsics_paths) == n_train_sample @@ -136,6 +142,7 @@ def train(train_image_path, image_paths=train_image_paths, sparse_depth_paths=train_sparse_depth_paths, intrinsics_paths=train_intrinsics_paths, + pose_paths=train_pose_paths, shape=(n_height, n_width), random_crop_type=augmentation_random_crop_type), batch_size=n_batch, @@ -216,24 +223,25 @@ def train(train_image_path, parameters_depth_model = depth_model.parameters() depth_model.train() - - # Bulid PoseNet (only needed for training) network - pose_model = PoseNetModel( - encoder_type='resnet18', - rotation_parameterization='axis', - weight_initializer=weight_initializer, - activation_func='relu', - device=device) - - parameters_pose_model = pose_model.parameters() - - pose_model.train() + parameters_pose_model = None + if train_pose_paths is None: + # Bulid PoseNet (only needed for training) network + pose_model = PoseNetModel( + encoder_type='resnet18', + rotation_parameterization='axis', + weight_initializer=weight_initializer, + activation_func='relu', + device=device) + + parameters_pose_model = pose_model.parameters() + pose_model.train() + + if pose_model_restore_path is not None and pose_model_restore_path != '': + pose_model.restore_model(pose_model_restore_path) if depth_model_restore_path is not None and depth_model_restore_path != '': depth_model.restore_model(depth_model_restore_path) - if pose_model_restore_path is not None and pose_model_restore_path != '': - pose_model.restore_model(pose_model_restore_path) # Set up tensorboard summary writers train_summary_writer = SummaryWriter(event_path + '-train') @@ -357,16 +365,25 @@ def train(train_image_path, augmentation_schedule_pos = 0 augmentation_probability = augmentation_probabilities[0] - optimizer = torch.optim.Adam([ - { - 'params' : parameters_depth_model, - 'weight_decay' : w_weight_decay_depth - }, - { - 'params' : parameters_pose_model, - 'weight_decay' : w_weight_decay_pose - }], - lr=learning_rate) + if train_pose_paths is None: + optimizer = torch.optim.Adam([ + { + 'params' : parameters_depth_model, + 'weight_decay' : w_weight_decay_depth + }, + { + 'params' : parameters_pose_model, + 'weight_decay' : w_weight_decay_pose + }], + lr=learning_rate) + else: + optimizer = torch.optim.Adam([ + { + 'params' : parameters_depth_model, + 'weight_decay' : w_weight_decay_depth + } + ], + lr=learning_rate) # Start training train_step = 0 @@ -398,7 +415,10 @@ def train(train_image_path, in_.to(device) for in_ in inputs ] - image0, image1, image2, sparse_depth0, intrinsics = inputs + if train_pose_paths is None: + image0, image1, image2, sparse_depth0, intrinsics = inputs + else: + image0, image1, image2, pose0, pose1, pose2, sparse_depth0, intrinsics = inputs # Validity map is where sparse depth is available validity_map_depth0 = torch.where( @@ -428,8 +448,17 @@ def train(train_image_path, validity_map_depth=filtered_validity_map_depth0, intrinsics=intrinsics) - pose01 = pose_model.forward(image0, image1) - pose02 = pose_model.forward(image0, image2) + if train_pose_paths is None: + pose01 = pose_model.forward(image0, image1) + pose02 = pose_model.forward(image0, image2) + else: + if pose_in_world_frame: + # Below 3 lines are in case 'absolute pose' is in world frame, and not camera frame. + pose0 = torch.inverse(pose0) + pose1 = torch.inverse(pose1) + pose2 = torch.inverse(pose2) + pose01 = pose1@torch.inverse(pose0) + pose02 = pose2@torch.inverse(pose0) # Compute loss function loss, loss_info = depth_model.compute_loss( @@ -506,16 +535,17 @@ def train(train_image_path, # Save checkpoints depth_model.save_model( depth_model_checkpoint_path.format(train_step), train_step, optimizer) - - pose_model.save_model( - pose_model_checkpoint_path.format(train_step), train_step, optimizer) + if train_pose_paths is None: + pose_model.save_model( + pose_model_checkpoint_path.format(train_step), train_step, optimizer) # Save checkpoints depth_model.save_model( depth_model_checkpoint_path.format(train_step), train_step, optimizer) - pose_model.save_model( - pose_model_checkpoint_path.format(train_step), train_step, optimizer) + if train_pose_paths is None: + pose_model.save_model( + pose_model_checkpoint_path.format(train_step), train_step, optimizer) def validate(depth_model, dataloader, @@ -1098,7 +1128,9 @@ def log_network_settings(log_path, # Computer number of parameters n_parameter_depth = sum(p.numel() for p in parameters_depth_model) - n_parameter_pose = sum(p.numel() for p in parameters_pose_model) + n_parameter_pose = 0 + if parameters_pose_model is not None: + n_parameter_pose = sum(p.numel() for p in parameters_pose_model) n_parameter = n_parameter_depth + n_parameter_pose diff --git a/src/kbnet_model.py b/src/kbnet_model.py index c614655..c8fdfe9 100644 --- a/src/kbnet_model.py +++ b/src/kbnet_model.py @@ -647,4 +647,4 @@ def log_summary(self, summary_writer.add_image( display_summary_depth_text, torchvision.utils.make_grid(display_summary_depth, nrow=n_display), - global_step=step) + global_step=step) \ No newline at end of file diff --git a/src/net_utils.py b/src/net_utils.py index 6a163a3..a08f2a2 100644 --- a/src/net_utils.py +++ b/src/net_utils.py @@ -1694,7 +1694,8 @@ def project_to_pixel(points, pose, intrinsics, shape): intrinsics = torch.cat([intrinsics, row], dim=1) # Apply the transformation and project: \pi K g p - T = torch.matmul(intrinsics, pose) + # print(type(pose)) + T = torch.matmul(intrinsics, pose.float()) T = T[:, 0:3, :] points = torch.matmul(T, points) points = points / (torch.unsqueeze(points[:, 2, :], dim=1) + 1e-7) diff --git a/src/run_kbnet.py b/src/run_kbnet.py index a023ead..2b10013 100644 --- a/src/run_kbnet.py +++ b/src/run_kbnet.py @@ -146,4 +146,4 @@ save_outputs=args.save_outputs, keep_input_filenames=args.keep_input_filenames, # Hardware settings - device=args.device) + device=args.device) \ No newline at end of file diff --git a/src/train_kbnet.py b/src/train_kbnet.py index 1aaa6c9..bbf75a1 100644 --- a/src/train_kbnet.py +++ b/src/train_kbnet.py @@ -25,6 +25,8 @@ # Training and validation input filepaths parser.add_argument('--train_image_path', type=str, required=True, help='Path to list of training image paths') +parser.add_argument('--train_pose_path', + type=str, required=False, help='Path to list of training pose paths', default=" ") parser.add_argument('--train_sparse_depth_path', type=str, required=True, help='Path to list of training sparse depth paths') parser.add_argument('--train_intrinsics_path', @@ -37,6 +39,8 @@ type=str, default='', help='Path to list of validation camera intrinsics paths') parser.add_argument('--val_ground_truth_path', type=str, default='', help='Path to list of validation ground truth depth paths') +parser.add_argument('--pose_in_world_frame', + type=str, default='0', help='Set to 1 if pose is in world frame. Default 0 implies poses are in camera(s) frame(s)') # Batch parameters parser.add_argument('--n_batch', type=int, default=settings.N_BATCH, help='Number of samples per batch') @@ -180,14 +184,15 @@ args.device = settings.CUDA if torch.cuda.is_available() else settings.CPU args.device = settings.CUDA if args.device == settings.GPU else args.device - train(train_image_path=args.train_image_path, + train_pose_path=args.train_pose_path, train_sparse_depth_path=args.train_sparse_depth_path, train_intrinsics_path=args.train_intrinsics_path, val_image_path=args.val_image_path, val_sparse_depth_path=args.val_sparse_depth_path, val_intrinsics_path=args.val_intrinsics_path, val_ground_truth_path=args.val_ground_truth_path, + pose_in_world_frame=bool(int(args.pose_in_world_frame)), # Batch settings n_batch=args.n_batch, n_height=args.n_height,