├── srv ├── TrackerStatus.srv ├── PredictDepthmap.srv ├── DepthFusion.srv └── TrackImage.srv ├── networks └── depth_fusion │ ├── jobs │ ├── run_lsd_depth.sh │ ├── run_single_image_depth.sh │ ├── lsd_depth.sh │ └── single_image_depth.sh │ ├── net │ ├── __pycache__ │ │ ├── my_losses.cpython-35.pyc │ │ └── my_networks.cpython-35.pyc │ ├── helpers.py │ └── my_losses.py │ ├── test_rgbd.py │ ├── trainer_singleImage.py │ └── trainer_rgbd.py ├── msg ├── keyframeGraphMsg.msg ├── keyframeMsgSiasa.msg └── keyframeMsg.msg ├── launch ├── training_data_gen.launch ├── lsd_slam_dataset.launch ├── live_lsd_deeptam.launch ├── dataset_lsd_deeptam.launch ├── dataset_lsd_deeptam_depthcompletion.launch └── dataset_reinforced_lsd_slam.launch ├── bin ├── write_folder_contents_to_txt.py ├── training_data_generator.py ├── evaluate_depth.py ├── training_data_validator.py ├── associate_ov.py ├── associate.py └── slam_system_tester.py ├── scripts ├── helpers.py ├── depthmap_predictor.py ├── single_image_depth_predictor.py └── depthmap_fuser.py ├── notebooks ├── .ipynb_checkpoints │ ├── single_image_depth_predictor-checkpoint.ipynb │ ├── opencv-cvbridge trial-checkpoint.ipynb │ ├── LSDDepth dataset reader-checkpoint.ipynb │ └── LSDDepth dataset generator-checkpoint.ipynb ├── opencv-cvbridge trial.ipynb ├── single_image_depth_predictor.ipynb ├── check_training_data.ipynb ├── LSDDepth dataset reader.ipynb ├── LSDDepth dataset generator.ipynb └── depth_fusion_network_test.ipynb ├── package.xml └── CMakeLists.txt /srv/TrackerStatus.srv: -------------------------------------------------------------------------------- 1 | std_msgs/String name 2 | --- 3 | std_msgs/Bool status -------------------------------------------------------------------------------- /srv/PredictDepthmap.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image rgb_image 2 | --- 3 | sensor_msgs/Image depthmap 4 | sensor_msgs/Image residual -------------------------------------------------------------------------------- /networks/depth_fusion/jobs/run_lsd_depth.sh: -------------------------------------------------------------------------------- 1 | cd /misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/jobs 2 | pew in demon_venv ./lsd_depth.sh 3 | -------------------------------------------------------------------------------- /srv/DepthFusion.srv: -------------------------------------------------------------------------------- 1 | float32 scale 2 | sensor_msgs/Image rgb_image 3 | float32[] idepth 4 | float32[] idepth_var 5 | --- 6 | sensor_msgs/Image depthmap 7 | sensor_msgs/Image residual -------------------------------------------------------------------------------- /networks/depth_fusion/jobs/run_single_image_depth.sh: -------------------------------------------------------------------------------- 1 | cd /misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/jobs 2 | pew in demon_venv ./single_image_depth.sh 3 | -------------------------------------------------------------------------------- /networks/depth_fusion/net/__pycache__/my_losses.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akhilthomas17/reinforced_visual_slam/HEAD/networks/depth_fusion/net/__pycache__/my_losses.cpython-35.pyc -------------------------------------------------------------------------------- /networks/depth_fusion/net/__pycache__/my_networks.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akhilthomas17/reinforced_visual_slam/HEAD/networks/depth_fusion/net/__pycache__/my_networks.cpython-35.pyc -------------------------------------------------------------------------------- /msg/keyframeGraphMsg.msg: -------------------------------------------------------------------------------- 1 | # data as serialization of sim(3)'s: (int id, float[7] camToWorld) 2 | uint32 numFrames 3 | uint8[] frameData 4 | 5 | 6 | # constraints (int from, int to, float err) 7 | uint32 numConstraints 8 | uint8[] constraintsData 9 | -------------------------------------------------------------------------------- /srv/TrackImage.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image keyframe_image 2 | sensor_msgs/Image keyframe_depth 3 | sensor_msgs/Image current_image 4 | float32[4] intrinsics 5 | float32[3] rotation_prior 6 | float32[3] translation_prior 7 | --- 8 | geometry_msgs/Transform transform 9 | -------------------------------------------------------------------------------- /networks/depth_fusion/jobs/lsd_depth.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /misc/software/cuda/add_environment_cuda9.0.176_cudnnv7.sh 3 | export LMBSPECIALOPS_LIB=/misc/lmbraid19/thomasa/deep-networks/lmbspecialops/build_tf1.8/lib/lmbspecialops.so 4 | #roscd reinforced_visual_slam/networks/depth_fusion/ 5 | cd /misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion 6 | python trainer_rgbd.py --num_epochs 70 --model_name NetV02Res_L1Sig4L1ExpResL1_tr1 7 | -------------------------------------------------------------------------------- /networks/depth_fusion/jobs/single_image_depth.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /misc/software/cuda/add_environment_cuda9.0.176_cudnnv7.sh 3 | export LMBSPECIALOPS_LIB=/misc/lmbraid19/thomasa/deep-networks/lmbspecialops/build_tf1.8/lib/lmbspecialops.so 4 | #roscd reinforced_visual_slam/networks/depth_fusion/ 5 | cd /misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion 6 | python trainer_singleImage.py --num_epochs 70 --model_name NetV0lRes_L1SigL1ExpResL1_tr2 -------------------------------------------------------------------------------- /msg/keyframeMsgSiasa.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | float64 time 3 | bool isKeyframe 4 | 5 | # camToWorld as serialization of sophus sim(3). 6 | # may change with keyframeGraph-updates. 7 | float32[7] camToWorld 8 | 9 | # camera parameter (fx fy cx cy), width, height 10 | # will never change, but required for display. 11 | float32 fx 12 | float32 fy 13 | float32 cx 14 | float32 cy 15 | uint32 height 16 | uint32 width 17 | 18 | # RGB Image and Depth Image for visualization with Siasa 19 | sensor_msgs/Image rgb 20 | sensor_msgs/Image depth -------------------------------------------------------------------------------- /msg/keyframeMsg.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | float64 time 3 | bool isKeyframe 4 | 5 | # camToWorld as serialization of sophus sim(3). 6 | # may change with keyframeGraph-updates. 7 | float32[7] camToWorld 8 | 9 | 10 | # camera parameter (fx fy cx cy), width, height 11 | # will never change, but required for display. 12 | float32 fx 13 | float32 fy 14 | float32 cx 15 | float32 cy 16 | uint32 height 17 | uint32 width 18 | 19 | 20 | # data as InputPointDense (float idepth, float idepth_var, uchar color[4]), width x height 21 | # may be empty, in that case no associated pointcloud is ever shown. 22 | uint8[] pointcloud 23 | -------------------------------------------------------------------------------- /launch/training_data_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/lsd_slam_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /bin/write_folder_contents_to_txt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | import argparse 5 | 6 | def write_filenames_to_txt(folder, out_filename): 7 | folder_parent = os.path.dirname(os.path.normpath(folder)) 8 | outfile_path = os.path.join(folder_parent, out_filename) 9 | print("Result will be stored in", outfile_path) 10 | with open(outfile_path, "w+") as train_file: 11 | cnt = 0 12 | for root, directories, filenames in os.walk(folder): 13 | for files in sorted(filenames): 14 | cnt += 1 15 | if cnt < 4: 16 | sep = "," 17 | else: 18 | sep = "\r\n" 19 | cnt = 0 20 | #print(files) 21 | #print(os.path.basename(os.path.normpath(root))) 22 | train_file.write( os.path.basename(os.path.normpath(root)) + "/" + files + sep) 23 | 24 | 25 | def run(): 26 | parser = argparse.ArgumentParser(description=( "To walk through subdirectories of the given directory " 27 | "and make a single file containing relative path to each of the samples. Each line in file would be:\n" 28 | "depth_gt,rgb,sparseDepth,sparseDepthVar")) 29 | parser.add_argument("--folder", type=str, required=True, help="Path to the folder which contains required files") 30 | parser.add_argument("--out_filename", type=str, required=True, help="Name of output file") 31 | args = parser.parse_args() 32 | #assert os.path.isfolder(args.folder) 33 | write_filenames_to_txt(args.folder, args.out_filename) 34 | 35 | 36 | if __name__ == "__main__": 37 | run() 38 | sys.exit() -------------------------------------------------------------------------------- /networks/depth_fusion/net/helpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import tensorflow as tf 3 | import lmbspecialops as sops 4 | import numpy as np 5 | 6 | 7 | ################# 8 | ##helpers my_losses 9 | ################# 10 | 11 | def nan_mean(x): 12 | # x will be a numpy array with the contents of the placeholder below 13 | return np.nanmean(np.abs(x)) 14 | 15 | 16 | def res_converter_exp_conf(res_gt, k = 1): 17 | ## plotting mean value to help decide the k 18 | mean_res = tf.py_func(nan_mean, [res_gt], tf.float32) 19 | tf.summary.scalar("mean_residual", mean_res) 20 | ## converting residual to confidence in range (0,1] 21 | res_gt = tf.exp(tf.scalar_mul(-k, res_gt)) 22 | return res_gt 23 | 24 | def invert_finite_depth(x): 25 | # mask is true if input is finite and greater than 0. If condition is false, make it invalid (nan) 26 | mask = tf.logical_and(tf.is_finite(x), tf.greater(x, 0.)) 27 | ix_clean = tf.where(mask, tf.reciprocal(x), tf.fill(x.shape, np.nan)) 28 | return ix_clean 29 | 30 | def replace_nonfinite(x): 31 | mask = tf.is_finite(x) 32 | x_clean = tf.where(mask, x, tf.zeros_like(x)) 33 | return x_clean 34 | 35 | def replace_nonfinite_loss(diff, gt): 36 | mask = tf.logical_and(tf.is_finite(diff), tf.greater(gt, 0.)) 37 | x_clean = tf.where(mask, diff, tf.zeros_like(diff)) 38 | return x_clean 39 | 40 | def get_multi_sig_list(inp, sig_param_list): 41 | """ Get multi sig with the pattern in the sig_param_list, result is concat in axis=1 42 | 43 | inp: Tensor 44 | sig_param_list: a list of sig_param dictionary 45 | """ 46 | sig_list = [] 47 | for sig_param in sig_param_list: 48 | sig_list.append(sops.scale_invariant_gradient(inp,**sig_param)) 49 | #multi_sig = tf.concat(sig_list,axis=1) 50 | return sig_list -------------------------------------------------------------------------------- /scripts/helpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from ctypes import * 4 | import numpy as np 5 | from minieigen import Quaternion 6 | from collections import namedtuple 7 | 8 | Keyframe = namedtuple('Keyframe', ['camToWorld', 'depth', 'rgb']) 9 | 10 | class InputPointCloud(Structure): 11 | _fields_ = [("idepth", c_float), 12 | ("idepth_var", c_float), 13 | ("color", c_ubyte * 4)] 14 | 15 | class FrameData(Structure): 16 | _fields_ = [("id", c_int), 17 | ("camToWorld", c_float * 7)] 18 | 19 | def quaternion_to_rotation_matrix(q_np): 20 | q_mini = Quaternion(*(np.roll(q_np, 1))) 21 | R = q_mini.toRotationMatrix() 22 | return np.asarray(R, dtype=np.float32) 23 | 24 | def invert_transformation(R, t, scale=1): 25 | T = np.eye(4) 26 | T[:3, :3] = R 27 | T[:3, 3] = t 28 | # Uncomment below to convert scale of the current output. 29 | T_scale = np.eye(4) 30 | R_scale = np.eye(3)*scale 31 | T_scale[:3,:3] = R_scale 32 | T = np.dot(T_scale, T) 33 | Tinv = np.linalg.inv(T) 34 | Rinv = Tinv[:3,:3] 35 | tinv = Tinv[:3,3] 36 | return Rinv, tinv, T 37 | 38 | def read_pointcloud_struct(pointcloud_data, arr_len): 39 | """ Reads point cloud struct array from python bytearray (bytes, to be specific) 40 | Struct is defined as InputPointCloud 41 | Args: 42 | pointcloud_data : bytes 43 | arr_len : array length (Input image width * Input image height) """ 44 | ctype_arr = InputPointCloud * arr_len 45 | return ctype_arr.from_buffer_copy(pointcloud_data) 46 | 47 | def read_frameData_struct(frame_data, arr_len): 48 | """ Reads frameData struct array from python bytearray (bytes, to be specific) 49 | Struct is defined as FrameData 50 | Args: 51 | frame_data : bytes 52 | arr_len : array length (Number of frames) """ 53 | ctype_arr = FrameData * arr_len 54 | return ctype_arr.from_buffer_copy(frame_data) -------------------------------------------------------------------------------- /launch/live_lsd_deeptam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /notebooks/.ipynb_checkpoints/single_image_depth_predictor-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import tensorflow as tf\n", 10 | "from deepTAM.helpers2 import *\n", 11 | "from deepTAM.common_netcode.myHelpers import *\n", 12 | "gpu_options = tf.GPUOptions()\n", 13 | "gpu_options.per_process_gpu_memory_fraction=0.8\n", 14 | "sess = tf.InteractiveSession(config=tf.ConfigProto(allow_soft_placement=True, gpu_options=gpu_options))\n", 15 | "\n", 16 | "# Option 1\n", 17 | "saver = tf.train.import_meta_graph('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065.meta')\n", 18 | "saver.restore(sess, '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065')\n", 19 | "\n", 20 | "# Option 2 (Huizhong's way)\n", 21 | "from deepTAM.evaluation.helpers import *\n", 22 | "from tfutils import optimistic_restore\n", 23 | "import cv2\n", 24 | "import numpy as np\n", 25 | "\n", 26 | "mapping_mod = load_myNetworks_module_noname('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/net/myNetworks.py')\n", 27 | "single_image_depth_net = mapping_mod.SingleImageDepthNetwork(batch_size=1, width=320, height=240)\n", 28 | "single_image_depth_outputs = single_image_depth_net.build_net(**single_image_depth_net.placeholders)\n", 29 | "sess.run(tf.global_variables_initializer())\n", 30 | "checkpoint = '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065'\n", 31 | "optimistic_restore(sess,checkpoint,verbose=True)\n", 32 | "\t\n", 33 | "rgb = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb/1305032227.675702.png', -1)\n", 34 | "rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n", 35 | "rgb_new = cv2.resize(rgb_new,(320, 240))\n", 36 | "rgb_new = np.transpose(rgb_new, (2,0,1))\n", 37 | "\n", 38 | "feed_dict = {\n", 39 | " single_image_depth_net.placeholders['image']: rgb_new[np.newaxis, :, :, :],\n", 40 | " }\n", 41 | "\n", 42 | "single_image_depth_out = sess.run(single_image_depth_outputs, feed_dict=feed_dict)\n", 43 | "depth_pr = single_image_depth_out['predict_depth']\n", 44 | "\n", 45 | "cv2.imshow(\"predicted depth\", depth_pr[0,0])\n", 46 | "cv2.imshow(\"input image\", rgb)\n", 47 | "cv2.waitKey(0)" 48 | ] 49 | } 50 | ], 51 | "metadata": { 52 | "kernelspec": { 53 | "display_name": "Python 3", 54 | "language": "python", 55 | "name": "python3" 56 | }, 57 | "language_info": { 58 | "codemirror_mode": { 59 | "name": "ipython", 60 | "version": 3 61 | }, 62 | "file_extension": ".py", 63 | "mimetype": "text/x-python", 64 | "name": "python", 65 | "nbconvert_exporter": "python", 66 | "pygments_lexer": "ipython3", 67 | "version": "3.5.2" 68 | } 69 | }, 70 | "nbformat": 4, 71 | "nbformat_minor": 2 72 | } 73 | -------------------------------------------------------------------------------- /launch/dataset_lsd_deeptam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 53 | 54 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | reinforced_visual_slam 4 | 0.0.0 5 | The reinforced_visual_slam package 6 | 7 | 8 | 9 | 10 | Akhil Thomas 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Akhil Thomas 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | cv_bridge 54 | geometry_msgs 55 | message_generation 56 | rospy 57 | sensor_msgs 58 | std_msgs 59 | 60 | cv_bridge 61 | geometry_msgs 62 | rospy 63 | sensor_msgs 64 | std_msgs 65 | 66 | cv_bridge 67 | geometry_msgs 68 | rospy 69 | sensor_msgs 70 | std_msgs 71 | message_runtime 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /notebooks/opencv-cvbridge trial.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import cv2\n", 10 | "import numpy as np\n", 11 | "from cv_bridge import CvBridge" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "rgb = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb/1305032180.416768.png', -1)\n", 21 | "rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n", 22 | "rgb_new = cv2.resize(rgb_new,(320, 240))\n", 23 | "rgb_new = np.transpose(rgb_new, (2,0,1))\n" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "depth_pr = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/depth/1305032180.405639.png', -1)" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "depth_pr = np.float32(depth_pr) * 0.0002\n", 42 | "print(np.max(depth_pr))\n", 43 | "print(np.min(depth_pr))" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "metadata": {}, 50 | "outputs": [], 51 | "source": [ 52 | "plot_depth = cv2.convertScaleAbs(depth_pr*255./4.)\n", 53 | "print(np.max(plot_depth))\n", 54 | "print(np.min(plot_depth))\n", 55 | "cv2.imshow('Predicted Depthmap', plot_depth)\n", 56 | "cv2.waitKey(0)" 57 | ] 58 | }, 59 | { 60 | "cell_type": "code", 61 | "execution_count": null, 62 | "metadata": {}, 63 | "outputs": [], 64 | "source": [ 65 | "depth_predicted_plot = cv2.cvtColor(plot_depth, cv2.COLOR_GRAY2RGB)\n", 66 | "idepth = np.nan_to_num(1./depth_pr)" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": {}, 73 | "outputs": [], 74 | "source": [ 75 | "depth_predicted_plot[:,:,0] = 255 - cv2.convertScaleAbs((0-idepth)*255.)\n", 76 | "depth_predicted_plot[:,:,1] = 255 - cv2.convertScaleAbs((1-idepth)*255.)\n", 77 | "depth_predicted_plot[:,:,2] = 255 - cv2.convertScaleAbs((2-idepth)*255.)\n", 78 | "cv2.imshow('Predicted Depthmap color', depth_predicted_plot)\n", 79 | "cv2.waitKey(0)" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": null, 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "cv2.destroyAllWindows()" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "_cv_bridge_depth = CvBridge()\n", 98 | "depthmap_predicted_msg = _cv_bridge_depth.cv2_to_imgmsg(depth_pr, \"passthrough\")" 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "de" 108 | ] 109 | }, 110 | { 111 | "cell_type": "code", 112 | "execution_count": null, 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "depth_cv = _cv_bridge_depth.imgmsg_to_cv2(depthmap_predicted_msg, \"passthrough\")" 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": null, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "depth_cv" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [] 134 | } 135 | ], 136 | "metadata": { 137 | "kernelspec": { 138 | "display_name": "Python 3", 139 | "language": "python", 140 | "name": "python3" 141 | }, 142 | "language_info": { 143 | "codemirror_mode": { 144 | "name": "ipython", 145 | "version": 3 146 | }, 147 | "file_extension": ".py", 148 | "mimetype": "text/x-python", 149 | "name": "python", 150 | "nbconvert_exporter": "python", 151 | "pygments_lexer": "ipython3", 152 | "version": "3.5.2" 153 | } 154 | }, 155 | "nbformat": 4, 156 | "nbformat_minor": 2 157 | } 158 | -------------------------------------------------------------------------------- /launch/dataset_lsd_deeptam_depthcompletion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 66 | 67 | 68 | 69 | 72 | 73 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /launch/dataset_reinforced_lsd_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 69 | 70 | 71 | 72 | 75 | 76 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /bin/training_data_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | import argparse 5 | import time 6 | import subprocess 7 | import numpy as np 8 | 9 | def dataset_len(dataset_loc): 10 | with open(dataset_loc) as f: 11 | for i, l in enumerate(f): 12 | pass 13 | return i + 1 14 | 15 | def gen_lsd_depth_training_data(dataset_loc, len_traj=200, keyframe_rate = 10, pause=2.5, test=False, debug=False): 16 | # static params (not to be changed) 17 | bin_loc = "rosrun lsd_slam_core depth_training_data_gen" 18 | hz = "0" 19 | doSlam = "true" 20 | KFDistWeight = 7 21 | 22 | # params that may be changed for different datasets 23 | # uncomment below for tum-fr1 24 | #calib_file = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg" 25 | # uncomment below for tum-fr2 26 | #calib_file = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/tum_rgbd_fr2_calib.cfg" 27 | # uncomment below for tum-fr3 28 | calib_file = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/tum_rgbd_fr3_calib.cfg" 29 | minUseGrad = "3" 30 | 31 | # Finding number of iterations needed: (Assuming "keyframe_rate" frames avg between each KFs) 32 | len_sequence = dataset_len(dataset_loc) 33 | number_itrns = keyframe_rate * int(len_sequence/len_traj) 34 | 35 | # Finding dataset basename 36 | base = "/misc/scratchSSD/ummenhof/rgbd_tum/" 37 | seq_name = os.path.dirname(dataset_loc).split('/')[-1] 38 | basename = base + seq_name 39 | 40 | # Finding start index for each iteration in the trajectory 41 | max_start = len_sequence - len_traj 42 | start_indx_array = np.random.randint(max_start, size=number_itrns) 43 | start_indx_array = start_indx_array.astype(np.unicode_) 44 | 45 | if test: 46 | itrn_max = 1 47 | else: 48 | itrn_max = number_itrns 49 | 50 | if debug: 51 | print("len_sequence: ", len_sequence) 52 | print("number_itrns: ", number_itrns) 53 | print("max_start: ", max_start) 54 | print("start_indx_array: ", start_indx_array) 55 | print("len_traj: ", len_traj) 56 | 57 | process_list= [] 58 | args_prefix = (bin_loc + " _calib:=" + calib_file +" _files:=" + dataset_loc + " _basename:=" + basename + 59 | " _hz:=" + hz + " _doSlam:=" + doSlam + " _len_traj:=" + str(len_traj) + " _minUseGrad:=" 60 | + minUseGrad + " _KFUsageWeight:=3 _KFDistWeight:=" + str(KFDistWeight) + " _start_indx:=") 61 | node_name_remap = " /LSD_SLAM_TRAIN_DATA_GEN:=/LSDDepthtraining" 62 | itr_num = " _itr_num:=" 63 | 64 | # running roscore 65 | command = "roscore" 66 | popen = subprocess.Popen(command, shell=True) 67 | 68 | for ii in range(itrn_max): 69 | start_indx = start_indx_array[ii] 70 | command = args_prefix + start_indx + node_name_remap + str(ii) + itr_num + str(ii) 71 | process_list.append( subprocess.Popen(command, shell=True) ) 72 | time.sleep(pause) 73 | 74 | def run(): 75 | parser = argparse.ArgumentParser(description=( "Generate training data for depth refinement by running LSD Slam on the given dataset.")) 76 | parser.add_argument("--sequence", type=str, required=True, help="Path to input sequence") 77 | parser.add_argument("--len_traj", type=int, help="Length of partial sequence on which LSD Slam runs") 78 | parser.add_argument("--pause", type=float, help="Time to pause between two iterations in seconds") 79 | parser.add_argument("--keyframe_rate", type=int, help="Average number of frames between 2 keyframes") 80 | parser.add_argument('--test', action='store_true', help="start program in test mode") 81 | parser.add_argument('--debug', action='store_true', help="enable debug outputs") 82 | parser.add_argument("--intro", action="store_true", help="print an intro to the i/p dataset and exit") 83 | 84 | args = parser.parse_args() 85 | assert os.path.isfile(args.sequence) 86 | 87 | len_traj = 200 88 | keyframe_rate = 10 89 | 90 | kwargs = dict(dataset_loc=args.sequence) 91 | if args.len_traj: 92 | kwargs['len_traj'] = args.len_traj 93 | len_traj = args.len_traj 94 | if args.keyframe_rate: 95 | kwargs['keyframe_rate'] = args.keyframe_rate 96 | keyframe_rate = args.keyframe_rate 97 | if args.pause: 98 | kwargs['pause'] = args.pause 99 | if args.test: 100 | kwargs['test'] = args.test 101 | if args.debug: 102 | kwargs['debug'] = args.debug 103 | 104 | if args.intro: 105 | len_sequence = dataset_len(args.sequence) 106 | number_itrns = keyframe_rate * int(len_sequence/len_traj) 107 | print("len_sequence: ", len_sequence) 108 | print("number_itrns: ", number_itrns) 109 | else: 110 | gen_lsd_depth_training_data(**kwargs) 111 | 112 | 113 | if __name__ == "__main__": 114 | run() 115 | sys.exit() -------------------------------------------------------------------------------- /bin/evaluate_depth.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import argparse 3 | import numpy as np 4 | import sys 5 | import os 6 | import glob 7 | from enum import Enum 8 | from collections import namedtuple 9 | 10 | #for cv2 11 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 12 | import cv2 13 | 14 | import matplotlib 15 | matplotlib.use('Agg') 16 | import matplotlib.pyplot as plt 17 | from matplotlib import colors 18 | 19 | 20 | debug = False 21 | 22 | read_png = lambda depth_png: cv2.imread(depth_png, -1).astype(np.float32)/5000 23 | read_sparse = lambda sparse_bin: np.reciprocal( 24 | np.fromfile(sparse_bin, dtype=np.float16).astype(np.float32).reshape((480, 640))) 25 | find_id = lambda filename: filename.split('/')[-1].split('_')[1] 26 | 27 | def evaluate_sparse(gt_depths, sparse_depths, ids, eval_folder): 28 | sum_error = 0 29 | plot_dir = os.path.join(eval_folder, "sparse_depths") 30 | os.makedirs(plot_dir, exist_ok=True) 31 | for gt_depth, sparse_depth, iid in zip(gt_depths, sparse_depths, ids): 32 | nan_mask = np.ones_like(gt_depth)*np.nan 33 | diff = np.where( sparse_depth>0, sparse_depth-gt_depth, nan_mask) 34 | sum_error += np.nanmean(diff**2) 35 | if debug: 36 | print("nan_mask:", nan_mask) 37 | print("sparse_depth:", sparse_depth) 38 | print("gt_depth", gt_depth) 39 | print("diff_r:", sparse_depth-gt_depth) 40 | print("diff:", diff) 41 | print("sum_error:", sum_error) 42 | plot_file = os.path.join(plot_dir, iid+".png") 43 | plot_coloured_depths(gt_depth, sparse_depth, plot_file) 44 | rms = np.sqrt(sum_error/len(gt_depths)) 45 | with open(eval_folder+"/summary_sparse.txt", "w") as f: 46 | f.write("number of images compared: %d images\n"%len(gt_depths)) 47 | f.write("RMSE(linear): %f m\n"%rms) 48 | 49 | def evaluate_dense(gt_depths, dense_depths, ids, eval_folder): 50 | sum_error = 0 51 | for gt_depth, dense_depth in zip(gt_depths, dense_depths, ids): 52 | diff_image = dense_depth - gt_depth 53 | sum_error += np.mean((diff_image)*2) 54 | plot_file = os.path.join(plot_dir, iid+".png") 55 | plot_coloured_depths(gt_depth, dense_depth, plot_file) 56 | rms = np.sqrt(sum_error/len(gt_depths)) 57 | with open(eval_folder+"/summary_dense.txt", "w") as f: 58 | f.write("number of images compared: %d images\n"%len(gt_depths)) 59 | f.write("RMSE(linear): %f m\n"%rms) 60 | 61 | def plot_coloured_depths(gt_depth, predicted_depth, plot_file, show_plots=debug): 62 | fig, axs = plt.subplots(1, 2) 63 | norm = colors.Normalize(vmin=0, vmax=6) 64 | cmap = 'hot' 65 | images = [] 66 | images.append(axs[0].imshow(predicted_depth, cmap=cmap)) 67 | images.append(axs[1].imshow(gt_depth, cmap=cmap)) 68 | for im in images: 69 | im.set_norm(norm) 70 | fig.colorbar(images[0], ax=axs, orientation='horizontal', fraction=.1) 71 | fig.savefig(plot_file) 72 | if show_plots: 73 | plt.show() 74 | key = plt.waitforbuttonpress() 75 | plt.close() 76 | 77 | if __name__ == "__main__": 78 | parser = argparse.ArgumentParser(description=( "Evaluate results of SLAM system configurations on datasets.")) 79 | parser.add_argument("--result_folder", type=str, required=True, help="Folder which contains all sparse depth, gt_depth and predicted depths.") 80 | parser.add_argument("--no_dense_depths", action='store_true', help="Select this if no dense_depth_prediction is made.") 81 | parser.add_argument("--eval_folder", type=str, help="Specify the file to save summary of the evaluation.") 82 | parser.add_argument("--show_live_plots", action="store_true", help=("Select true to display plots while running. Active only when --plot_dir is provided")) 83 | args = parser.parse_args() 84 | 85 | assert(os.path.isdir(args.result_folder)) 86 | if args.eval_folder: 87 | assert(os.path.isdir(eval_folder)) 88 | eval_folder = args.eval_folder 89 | else: 90 | eval_folder = os.path.join(args.result_folder, "eval") 91 | os.makedirs(eval_folder, exist_ok=True) 92 | 93 | # read sparse depths and gt depths 94 | sparse_depth_files = sorted(glob.glob(args.result_folder+"/*sparse_depth.bin")) 95 | sparse_depths = list(map(read_sparse, sparse_depth_files)) 96 | gt_depth_files = sorted(glob.glob(args.result_folder+"/*depthGT.png")) 97 | gt_depths = list(map(read_png, gt_depth_files)) 98 | 99 | # read frame ids from filenames 100 | ids = list(map(find_id, gt_depth_files)) 101 | 102 | if args.show_live_plots: 103 | debug = True 104 | 105 | if debug: 106 | print("sparse_depths[0]:", sparse_depths[0]) 107 | print("len(sparse_depths):", len(sparse_depths)) 108 | print("gt_depths[0]:", gt_depths[0]) 109 | print("len(gt_depths):", len(gt_depths)) 110 | print("ids:", ids) 111 | print("eval_folder:", eval_folder) 112 | 113 | 114 | # analyze sparse depths 115 | evaluate_sparse(gt_depths, sparse_depths, ids, eval_folder) 116 | 117 | if not args.no_dense_depths: 118 | # read dense depth predictions 119 | dense_depth_files = sorted(glob.glob(args.result_folder+"/*depthPrediction.png")) 120 | gt_depths = map(read_png, gt_depth_files) 121 | 122 | # analyze dense depths 123 | evaluate_dense(gt_depths, dense_depths, ids, eval_folder) 124 | 125 | print("Finished processing ", args.result_folder) 126 | sys.exit() -------------------------------------------------------------------------------- /bin/training_data_validator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | import argparse 5 | 6 | #for cv2 7 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 8 | import cv2 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from matplotlib import colors 13 | 14 | 15 | def check_validity_LSD_depth_dataset(dataset_folder, basename, matplotlib=True, debug=False): 16 | """ 17 | Function to check validity of the generated LSD Slam depth refinement dataset. 18 | Provide the function with datasetset_folder and basename of the Keyframe batch to be verified. 19 | """ 20 | 21 | #Inside notebook uncomment the following 22 | #matplotlib.rcsetup.interactive_bk 23 | #plt.switch_backend('nbAgg') 24 | 25 | # Setting paths of the keyframe batch 26 | depth_gt_png = dataset_folder + basename + "depthGT.png" 27 | print(depth_gt_png) 28 | rgb_png = dataset_folder + basename + "rgb.png" 29 | sparse_depth_bin = dataset_folder + basename + "sparse_depth.bin" 30 | sparse_variance_bin = dataset_folder + basename + "sparse_depthVar.bin" 31 | 32 | # opening depth_gt and converting it to float 33 | print("GT depth---") 34 | depth_gt = cv2.imread(depth_gt_png, -1).astype(np.float32)/5000 35 | rgb = cv2.imread(rgb_png, -1) 36 | gt_max = np.nanmax(depth_gt) 37 | print("shape: ", depth_gt.shape) 38 | print("max: ", gt_max) 39 | print("dtype: ", depth_gt.dtype) 40 | 41 | # loading sparse idepth from bin (as half-float) and converting it to float32 42 | print("Sparse idepth ---") 43 | sparse_idepth = np.fromfile(sparse_depth_bin, dtype=np.float16) 44 | if(debug): 45 | print("min half-float sparse_idepth: ", np.nanmin(sparse_idepth)) 46 | print("max half-float sparse_idepth: ", np.nanmax(sparse_idepth)) 47 | sparse_idepth = sparse_idepth.astype(np.float32) 48 | sparse_idepth = sparse_idepth.reshape((480, 640)) 49 | print("max: ", np.nanmax(sparse_idepth)) 50 | print("shape: ",sparse_idepth.shape) 51 | print("dtype: ", sparse_idepth.dtype) 52 | 53 | # converting sparse idepth to depth 54 | print("Sparse depth ---") 55 | sparse_depth = (1./sparse_idepth) 56 | print("max: ", np.nanmax(sparse_depth)) 57 | print("shape: ", sparse_depth.shape) 58 | print("dtype: ", sparse_depth.dtype) 59 | 60 | # loading sparse idepthVar from bin (as half-float) and converting it to float32 61 | print("Sparse idepthVar ---") 62 | sparse_idepthVar = np.fromfile(sparse_variance_bin, dtype=np.float16) 63 | if(debug): 64 | print("min half-float sparse_idepthVar: ", np.nanmin(sparse_idepthVar)) 65 | print("max half-float sparse_idepthVar: ", np.nanmax(sparse_idepthVar)) 66 | sparse_idepthVar = sparse_idepthVar.astype(np.float32) 67 | sparse_idepthVar = sparse_idepthVar.reshape((480, 640)) 68 | print("max: ", np.nanmax(sparse_idepthVar)) 69 | print("shape: ", sparse_idepthVar.shape) 70 | print("dtype: ", sparse_idepthVar.dtype) 71 | 72 | # plotting images 73 | if matplotlib: 74 | # plot using matplotlib 75 | fig, axs = plt.subplots(1, 2) 76 | norm = colors.Normalize(vmin=0, vmax=gt_max) 77 | cmap = 'hot' 78 | images = [] 79 | images.append(axs[0].imshow(sparse_depth, cmap=cmap)) 80 | images.append(axs[1].imshow(depth_gt, cmap=cmap)) 81 | for im in images: 82 | im.set_norm(norm) 83 | fig.colorbar(images[0], ax=axs, orientation='horizontal', fraction=.1) 84 | #plt.figure("sparse_depth") 85 | #plt.imshow(sparse_depth, cmap='hot', norm=norm) 86 | #plt.clim(0,gt_max) 87 | #plt.colorbar() 88 | #plt.figure("gt_depth") 89 | #plt.imshow(depth_gt, cmap='hot', norm=norm) 90 | #plt.clim(0,gt_max) 91 | #plt.colorbar() 92 | plt.figure("sparse_depth_variance") 93 | plt.imshow(sparse_idepthVar, cmap='hot') 94 | plt.figure("rgb") 95 | plt.imshow(rgb) 96 | plt.show() 97 | else: 98 | # plot using opencv 99 | sparse_plot = cv2.convertScaleAbs(sparse_depth*255./4.5) 100 | gt_plot = cv2.convertScaleAbs(depth_gt*255./4.5) 101 | cv2.imshow("sparse_depth", sparse_plot) 102 | cv2.imshow("gt_depth", gt_plot) 103 | cv2.imshow("rgb", rgb_gt) 104 | cv2.waitKey(0) 105 | 106 | 107 | if(debug): 108 | print("min depth_gt: ", np.nanmin(depth_gt)) 109 | print("min sparse_idepth: ", np.nanmin(sparse_idepth)) 110 | print("min sparse_depth: ", np.nanmin(sparse_depth)) 111 | print("min sparse_idepthVar: ", np.nanmin(sparse_idepthVar)) 112 | 113 | def run(): 114 | parser = argparse.ArgumentParser(description=( "Checks validity of one Keyframe batch inside training data generated from LSD Slam to refine depth.")) 115 | parser.add_argument("--dataset_dir", type=str, required=True, help="training data directory to be verified") 116 | parser.add_argument("--basename", type=str, required=True, help="basename of the Keyframe batch in training data") 117 | parser.add_argument('--debug', action='store_true', help="enable debug outputs") 118 | 119 | args = parser.parse_args() 120 | assert os.path.isdir(args.dataset_dir) 121 | check_validity_LSD_depth_dataset(dataset_folder=args.dataset_dir+"/", basename=args.basename, debug=args.debug) 122 | 123 | 124 | if __name__ == "__main__": 125 | run() 126 | sys.exit() -------------------------------------------------------------------------------- /scripts/depthmap_predictor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from reinforced_visual_slam.srv import * 4 | import rospy 5 | 6 | import sys 7 | 8 | # for cv_bridge 9 | sys.path.insert(0, '/misc/lmbraid19/thomasa/catkin_ws/install/lib/python3/dist-packages') 10 | #for cv2 11 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 12 | 13 | 14 | from cv_bridge import CvBridge 15 | from std_msgs.msg import Header 16 | 17 | import cv2 18 | 19 | import tensorflow as tf 20 | 21 | from deepTAM.helpers2 import * 22 | from deepTAM.common_netcode.myHelpers import * 23 | from deepTAM.evaluation.helpers import * 24 | from tfutils import optimistic_restore 25 | 26 | class DepthmapPredictor(object): 27 | 28 | def __init__(self, input_img_width=640, input_img_height=480): 29 | rospy.init_node('depthmap_predictor') 30 | self._cv_bridge_rgb = CvBridge() 31 | self._cv_bridge_depth = CvBridge() 32 | rospy.loginfo("Opening tensorflow session") 33 | gpu_options = tf.GPUOptions() 34 | gpu_options.per_process_gpu_memory_fraction=0.3 35 | self.session = tf.InteractiveSession(config=tf.ConfigProto(allow_soft_placement=True, gpu_options=gpu_options)) 36 | 37 | def configure(self, network_module_path, network_checkpoint, width=320, height=240): 38 | self.width = 320 39 | self.height = 240 40 | depthmap_module = load_myNetworks_module_noname(network_module_path) 41 | self.single_image_depth_net = depthmap_module.SingleImageDepthNetwork(batch_size=1, width=self.width, height=self.height) 42 | self.single_image_depth_outputs = self.single_image_depth_net.build_net(**self.single_image_depth_net.placeholders) 43 | self.session.run(tf.global_variables_initializer()) 44 | optimistic_restore(self.session, network_checkpoint, verbose=False) 45 | rospy.logwarn("Finished loading the depthmap predictor network") 46 | 47 | def convertOpenCvRGBToTfTensor(self, rgb): 48 | rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F) 49 | # Resize Keyframe RGB image to (320W,240H) and reshape it to (3, height, width) 50 | rgb_new = cv2.resize(rgb_new, (self.width, self.height)) 51 | rgb_new = np.transpose(rgb_new, (2,0,1)) 52 | return rgb_new 53 | 54 | def predict_depthmap_cb(self, req, debug=False): 55 | rospy.loginfo("Received image to predict depthmap") 56 | # Requirement: RGB has to np array float32, normalized in range (-0.5,0.5), with shape (1,3,240,320) 57 | # Get the Keyframe RGB image from ROS message and convert it to openCV Image format 58 | try: 59 | rgb_image = self._cv_bridge_rgb.imgmsg_to_cv2(req.rgb_image, "passthrough") 60 | except CvBridgeError as e: 61 | print(e) 62 | return PredictDepthmapResponse(0) 63 | if False: 64 | cv2.imshow('Input RGB', rgb_image) 65 | rgb_tensor = self.convertOpenCvRGBToTfTensor(rgb_image) 66 | feed_dict = { 67 | self.single_image_depth_net.placeholders['image']: rgb_tensor[np.newaxis, :, :, :], 68 | } 69 | self.single_image_depth_out = self.session.run(self.single_image_depth_outputs, feed_dict=feed_dict) 70 | # *** Network gives idepth prediction. Invert it if needed! *** 71 | depthmap_predicted = np.nan_to_num(self.single_image_depth_out['predict_depth'][0,0]) 72 | 73 | # Doing cv_bridge conversion 74 | depthmap_predicted_msg = self._cv_bridge_depth.cv2_to_imgmsg(depthmap_predicted, "passthrough") 75 | depthmap_predicted_msg.step = int (depthmap_predicted_msg.step) 76 | if debug: 77 | print("depthmap_predicted max:", np.max(depthmap_predicted)) 78 | print("depthmap_predicted min:", np.min(depthmap_predicted)) 79 | print("depthmap_predicted.dtype", depthmap_predicted.dtype) 80 | print("depthmap_predicted.shape", depthmap_predicted.shape) 81 | plot_depth_gray = cv2.convertScaleAbs(depthmap_predicted*255./np.max(depthmap_predicted)) 82 | depth_predicted_plot = cv2.cvtColor(plot_depth_gray, cv2.COLOR_GRAY2RGB) 83 | idepth = np.nan_to_num(1./depthmap_predicted) 84 | depth_predicted_plot[:,:,0] = 255 - cv2.convertScaleAbs((0-idepth)*255.) 85 | depth_predicted_plot[:,:,1] = 255 - cv2.convertScaleAbs((1-idepth)*255.) 86 | depth_predicted_plot[:,:,2] = 255 - cv2.convertScaleAbs((2-idepth)*255.) 87 | print("depthmap_predicted_msg.header:", depthmap_predicted_msg.header) 88 | print("depthmap_predicted_msg.step:", depthmap_predicted_msg.step) 89 | print("depthmap_predicted_msg.encoding:", depthmap_predicted_msg.encoding) 90 | cv2.imshow('Predicted Depthmap', depth_predicted_plot) 91 | cv2.waitKey(0) 92 | 93 | return PredictDepthmapResponse(depthmap_predicted_msg) 94 | 95 | def run(self): 96 | service = rospy.Service('predict_depthmap', PredictDepthmap, self.predict_depthmap_cb) 97 | print("Ready to predict depthmaps") 98 | try: 99 | rospy.spin() 100 | except KeyboardInterrupt: 101 | print("Shutting down depthmap predictor") 102 | 103 | if __name__ == "__main__": 104 | predictor = DepthmapPredictor() 105 | predictor.configure('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/net/myNetworks.py', '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065') 106 | predictor.run() 107 | -------------------------------------------------------------------------------- /bin/associate_ov.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2013, Juergen Sturm, TUM 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of TUM nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Requirements: 35 | # sudo apt-get install python-argparse 36 | 37 | """ 38 | The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. 39 | 40 | For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. 41 | """ 42 | 43 | import argparse 44 | import sys 45 | import os 46 | import numpy 47 | 48 | 49 | def read_file_list(filename): 50 | """ 51 | Reads a trajectory from a text file. 52 | 53 | File format: 54 | The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) 55 | and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 56 | 57 | Input: 58 | filename -- File name 59 | 60 | Output: 61 | dict -- dictionary of (stamp,data) tuples 62 | 63 | """ 64 | file = open(filename) 65 | data = file.read() 66 | lines = data.replace(","," ").replace("\t"," ").split("\n") 67 | list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] 68 | list = [(float(l[0]),l[1:]) for l in list if len(l)>1] 69 | return dict(list) 70 | 71 | def associate(first_list, second_list,offset=0.0,max_difference=0.02): 72 | """ 73 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 74 | to find the closest match for every input tuple. 75 | 76 | Input: 77 | first_list -- first dictionary of (stamp,data) tuples 78 | second_list -- second dictionary of (stamp,data) tuples 79 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) 80 | max_difference -- search radius for candidate generation 81 | 82 | Output: 83 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) 84 | 85 | """ 86 | first_keys = list(first_list.keys()) 87 | second_keys = list(second_list.keys()) 88 | potential_matches = [(abs(a - (b + offset)), a, b) 89 | for a in first_keys 90 | for b in second_keys 91 | if abs(a - (b + offset)) < max_difference] 92 | potential_matches.sort() 93 | matches = [] 94 | for diff, a, b in potential_matches: 95 | if a in first_keys and b in second_keys: 96 | first_keys.remove(a) 97 | second_keys.remove(b) 98 | matches.append((a, b)) 99 | 100 | matches.sort() 101 | return matches 102 | 103 | if __name__ == '__main__': 104 | 105 | # parse command line 106 | parser = argparse.ArgumentParser(description=''' 107 | This script takes two data files with timestamps and associates them 108 | ''') 109 | parser.add_argument('first_file', help='first text file (format: timestamp data)') 110 | parser.add_argument('second_file', help='second text file (format: timestamp data)') 111 | parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') 112 | parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) 113 | parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) 114 | args = parser.parse_args() 115 | 116 | first_list = read_file_list(args.first_file) 117 | second_list = read_file_list(args.second_file) 118 | 119 | matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) 120 | 121 | if args.first_only: 122 | for a,b in matches: 123 | print("%f %s"%(a," ".join(first_list[a]))) 124 | else: 125 | for a,b in matches: 126 | print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b]))) 127 | 128 | 129 | -------------------------------------------------------------------------------- /notebooks/.ipynb_checkpoints/opencv-cvbridge trial-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 8, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import cv2\n", 10 | "import numpy as np\n", 11 | "from cv_bridge import CvBridge" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": 9, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "rgb = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb/1305032180.416768.png', -1)\n", 21 | "rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n", 22 | "rgb_new = cv2.resize(rgb_new,(320, 240))\n", 23 | "rgb_new = np.transpose(rgb_new, (2,0,1))\n" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": 49, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "depth_pr = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/depth/1305032180.405639.png', -1)" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": 50, 38 | "metadata": {}, 39 | "outputs": [ 40 | { 41 | "name": "stdout", 42 | "output_type": "stream", 43 | "text": [ 44 | "3.6399999\n", 45 | "0.0\n" 46 | ] 47 | } 48 | ], 49 | "source": [ 50 | "depth_pr = np.float32(depth_pr) * 0.0002\n", 51 | "print(np.max(depth_pr))\n", 52 | "print(np.min(depth_pr))" 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": 56, 58 | "metadata": {}, 59 | "outputs": [ 60 | { 61 | "name": "stdout", 62 | "output_type": "stream", 63 | "text": [ 64 | "232\n", 65 | "0\n" 66 | ] 67 | }, 68 | { 69 | "data": { 70 | "text/plain": [ 71 | "10" 72 | ] 73 | }, 74 | "execution_count": 56, 75 | "metadata": {}, 76 | "output_type": "execute_result" 77 | } 78 | ], 79 | "source": [ 80 | "plot_depth = cv2.convertScaleAbs(depth_pr*255./4.)\n", 81 | "print(np.max(plot_depth))\n", 82 | "print(np.min(plot_depth))\n", 83 | "cv2.imshow('Predicted Depthmap', plot_depth)\n", 84 | "cv2.waitKey(0)" 85 | ] 86 | }, 87 | { 88 | "cell_type": "code", 89 | "execution_count": 59, 90 | "metadata": {}, 91 | "outputs": [ 92 | { 93 | "name": "stderr", 94 | "output_type": "stream", 95 | "text": [ 96 | "/misc/lmbraid19/thomasa/virtual-env/demon_venv/lib/python3.5/site-packages/ipykernel_launcher.py:2: RuntimeWarning: divide by zero encountered in true_divide\n", 97 | " \n" 98 | ] 99 | } 100 | ], 101 | "source": [ 102 | "depth_predicted_plot = cv2.cvtColor(plot_depth, cv2.COLOR_GRAY2RGB)" 103 | ] 104 | }, 105 | { 106 | "cell_type": "code", 107 | "execution_count": 61, 108 | "metadata": {}, 109 | "outputs": [ 110 | { 111 | "data": { 112 | "text/plain": [ 113 | "10" 114 | ] 115 | }, 116 | "execution_count": 61, 117 | "metadata": {}, 118 | "output_type": "execute_result" 119 | } 120 | ], 121 | "source": [ 122 | "depth_predicted_plot[:,:,0] = cv2.convertScaleAbs(255 - np.clip(abs(0-idepth)*255, 0, 255))\n", 123 | "depth_predicted_plot[:,:,1] = 255 - np.clip(abs(1-idepth)*255, 0, 255)\n", 124 | "depth_predicted_plot[:,:,2] = 255 - np.clip(abs(2-idepth)*255, 0, 255)\n", 125 | "cv2.imshow('Predicted Depthmap color', depth_predicted_plot)\n", 126 | "cv2.waitKey(0)" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": 24, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "cv2.destroyAllWindows()" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": 26, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "_cv_bridge_depth = CvBridge()\n", 145 | "depthmap_predicted_msg = _cv_bridge_depth.cv2_to_imgmsg(depth_pr, \"passthrough\")" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": 27, 151 | "metadata": {}, 152 | "outputs": [ 153 | { 154 | "data": { 155 | "text/plain": [ 156 | "float" 157 | ] 158 | }, 159 | "execution_count": 27, 160 | "metadata": {}, 161 | "output_type": "execute_result" 162 | } 163 | ], 164 | "source": [ 165 | "de" 166 | ] 167 | }, 168 | { 169 | "cell_type": "code", 170 | "execution_count": 29, 171 | "metadata": {}, 172 | "outputs": [], 173 | "source": [ 174 | "depth_cv = _cv_bridge_depth.imgmsg_to_cv2(depthmap_predicted_msg, \"passthrough\")" 175 | ] 176 | }, 177 | { 178 | "cell_type": "code", 179 | "execution_count": 30, 180 | "metadata": {}, 181 | "outputs": [ 182 | { 183 | "data": { 184 | "text/plain": [ 185 | "array([[ 0., 7569., 7569., ..., 0., 0., 0.],\n", 186 | " [ 0., 0., 0., ..., 0., 0., 0.],\n", 187 | " [ 0., 0., 0., ..., 0., 0., 0.],\n", 188 | " ...,\n", 189 | " [ 0., 0., 0., ..., 0., 0., 0.],\n", 190 | " [ 0., 0., 0., ..., 0., 0., 0.],\n", 191 | " [ 0., 0., 0., ..., 0., 0., 0.]], dtype=float32)" 192 | ] 193 | }, 194 | "execution_count": 30, 195 | "metadata": {}, 196 | "output_type": "execute_result" 197 | } 198 | ], 199 | "source": [ 200 | "depth_cv" 201 | ] 202 | }, 203 | { 204 | "cell_type": "code", 205 | "execution_count": null, 206 | "metadata": {}, 207 | "outputs": [], 208 | "source": [] 209 | } 210 | ], 211 | "metadata": { 212 | "kernelspec": { 213 | "display_name": "Python 3", 214 | "language": "python", 215 | "name": "python3" 216 | }, 217 | "language_info": { 218 | "codemirror_mode": { 219 | "name": "ipython", 220 | "version": 3 221 | }, 222 | "file_extension": ".py", 223 | "mimetype": "text/x-python", 224 | "name": "python", 225 | "nbconvert_exporter": "python", 226 | "pygments_lexer": "ipython3", 227 | "version": "3.5.2" 228 | } 229 | }, 230 | "nbformat": 4, 231 | "nbformat_minor": 2 232 | } 233 | -------------------------------------------------------------------------------- /bin/associate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2013, Juergen Sturm, TUM 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of TUM nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | # 35 | # Requirements: 36 | # sudo apt-get install python-argparse 37 | 38 | """ 39 | The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. 40 | 41 | For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. 42 | """ 43 | 44 | import argparse 45 | import sys 46 | import os 47 | import numpy 48 | 49 | 50 | def read_file_list(filename): 51 | """ 52 | Reads a trajectory from a text file. 53 | 54 | File format: 55 | The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) 56 | and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 57 | 58 | Input: 59 | filename -- File name 60 | 61 | Output: 62 | dict -- dictionary of (stamp,data) tuples 63 | 64 | """ 65 | file = open(filename) 66 | data = file.read() 67 | lines = data.replace(","," ").replace("\t"," ").split("\n") 68 | list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"] 69 | list = [(float(l[0]),l[1:]) for l in list if len(l)>1] 70 | return dict(list) 71 | 72 | def associate(first_list, second_list,offset=0.0,max_difference=0.02): 73 | """ 74 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 75 | to find the closest match for every input tuple. 76 | 77 | Input: 78 | first_list -- first dictionary of (stamp,data) tuples 79 | second_list -- second dictionary of (stamp,data) tuples 80 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) 81 | max_difference -- search radius for candidate generation 82 | 83 | Output: 84 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) 85 | 86 | """ 87 | first_keys = list(first_list.keys()) 88 | second_keys = list(second_list.keys()) 89 | potential_matches = [(abs(a - (b + offset)), a, b) 90 | for a in first_keys 91 | for b in second_keys 92 | if abs(a - (b + offset)) < max_difference] 93 | potential_matches.sort() 94 | matches = [] 95 | for diff, a, b in potential_matches: 96 | if a in first_keys and b in second_keys: 97 | first_keys.remove(a) 98 | second_keys.remove(b) 99 | matches.append((a, b)) 100 | 101 | matches.sort() 102 | return matches 103 | 104 | if __name__ == '__main__': 105 | 106 | # parse command line 107 | parser = argparse.ArgumentParser(description=''' 108 | This script takes two data files with timestamps and associates them 109 | ''') 110 | parser.add_argument('first_file', help='first text file (format: timestamp data)') 111 | parser.add_argument('second_file', help='second text file (format: timestamp data)') 112 | parser.add_argument('out_file_name', help='output file name for associated data (No need of full path)') 113 | parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') 114 | parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0) 115 | parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02) 116 | args = parser.parse_args() 117 | 118 | first_list = read_file_list(args.first_file) 119 | second_list = read_file_list(args.second_file) 120 | 121 | matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) 122 | 123 | base_path = "/misc/lmbraid19/thomasa/datasets/rgbd" 124 | 125 | directory_name = os.path.dirname(args.first_file).split('/')[-1] 126 | directory = base_path + "/" + directory_name 127 | 128 | if not os.path.exists(directory): 129 | os.makedirs(directory) 130 | 131 | out_file_path = directory + "/" + args.out_file_name 132 | 133 | with open(out_file_path, 'w') as f: 134 | 135 | if args.first_only: 136 | for a,b in matches: 137 | print("%f %s"%(a," ".join(first_list[a])), file=f) 138 | else: 139 | for a,b in matches: 140 | print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])), file=f) 141 | 142 | 143 | -------------------------------------------------------------------------------- /networks/depth_fusion/test_rgbd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from reinforced_visual_slam.srv import * 4 | import rospy 5 | 6 | import matplotlib.pyplot as plt 7 | from matplotlib import colors 8 | 9 | import os 10 | import sys 11 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 12 | 13 | import cv2 14 | import numpy as np 15 | from skimage.transform import resize 16 | 17 | import tensorflow as tf 18 | 19 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 20 | from net.my_models import * 21 | 22 | import random 23 | 24 | def predict_input_fn(rgb, sparse_idepth, sparse_idepth_var): 25 | feature_names = [ 26 | 'rgb', 27 | 'sparseInverseDepth', 28 | 'sparseInverseDepthVariance' 29 | ] 30 | input_tensors = [rgb, sparse_idepth[np.newaxis,:,:], sparse_idepth_var[np.newaxis,:,:]] 31 | inputs = dict(zip(feature_names, input_tensors)) 32 | print(inputs['rgb'].shape) 33 | dataset = tf.data.Dataset.from_tensors(inputs) 34 | dataset = dataset.batch(1) 35 | iterator = dataset.make_one_shot_iterator() 36 | features = iterator.get_next() 37 | print(features['rgb'].shape, features['sparseInverseDepth'].shape, 38 | features['sparseInverseDepthVariance'].shape) 39 | return features 40 | 41 | def configure(model_name, model_function): 42 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 43 | model_dir = os.path.join(model_base_dir, model_name) 44 | print(model_dir) 45 | print(model_function) 46 | # loading the network 47 | lsd_depth_fuser = tf.estimator.Estimator(model_fn=model_function, model_dir=model_dir, 48 | params={'data_format':"channels_first", 'multi_gpu':False}) 49 | 50 | def test_network(model_name, model_function): 51 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 52 | model_dir = os.path.join(model_base_dir, model_name) 53 | print(model_dir) 54 | print(model_function) 55 | # loading the network 56 | lsd_depth_fuser = tf.estimator.Estimator(model_fn=model_function, model_dir=model_dir, 57 | params={'data_format':"channels_first", 'multi_gpu':False}) 58 | 59 | test_file = '/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/test/rgbd_lsdDepth_test.txt' 60 | base_path = os.path.dirname(test_file) 61 | with open(test_file) as file: 62 | lines = file.readlines() 63 | random.shuffle(lines) 64 | for line in lines: 65 | # prepare test input 66 | depth_gt_file, rgb_file, sparse_idepth_bin, sparse_idepth_var_bin = line.strip().split(',')[0:] 67 | 68 | print("Sparse idepth ---") 69 | sparse_idepth = np.fromfile(os.path.join(base_path, sparse_idepth_bin), dtype=np.float16) 70 | print("min half-float sparse_idepth: ", np.nanmin(sparse_idepth)) 71 | print("max half-float sparse_idepth: ", np.nanmax(sparse_idepth)) 72 | sparse_idepth = sparse_idepth.astype(np.float32) 73 | sparse_idepth = sparse_idepth.reshape((480, 640)) 74 | #sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0) 75 | sparse_idepth = cv2.resize(sparse_idepth, (320, 240), cv2.INTER_NEAREST) 76 | #sparse_idepth = sparse_idepth.resize() 77 | print("max: ", np.nanmax(sparse_idepth)) 78 | print("shape: ",sparse_idepth.shape) 79 | print("dtype: ", sparse_idepth.dtype) 80 | 81 | print("Sparse idepth var ---") 82 | sparse_idepth_var = np.fromfile(os.path.join(base_path, sparse_idepth_var_bin), dtype=np.float16) 83 | print("min half-float sparse_idepth: ", np.nanmin(sparse_idepth_var)) 84 | print("max half-float sparse_idepth: ", np.nanmax(sparse_idepth_var)) 85 | sparse_idepth_var = sparse_idepth_var.astype(np.float32) 86 | sparse_idepth_var = sparse_idepth_var.reshape((480, 640)) 87 | #sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0) 88 | sparse_idepth_var = cv2.resize(sparse_idepth_var, (320, 240), cv2.INTER_NEAREST) 89 | #sparse_idepth = sparse_idepth.resize() 90 | print("max: ", np.nanmax(sparse_idepth_var)) 91 | print("shape: ",sparse_idepth_var.shape) 92 | print("dtype: ", sparse_idepth_var.dtype) 93 | 94 | print("rgb---") 95 | rgb = cv2.imread(os.path.join(base_path, rgb_file), -1).astype(np.float32)/255 96 | rgb = cv2.resize(rgb, (320, 240)) 97 | rgb = np.transpose(rgb, (2,0,1)) 98 | print("shape: ", rgb.shape) 99 | print("max: ", np.nanmax(rgb)) 100 | print("dtype: ", rgb.dtype) 101 | 102 | depth_gt = cv2.imread(os.path.join(base_path, depth_gt_file), -1).astype(np.float32)/5000 103 | gt_max = np.nanmax(depth_gt) 104 | 105 | depthmap_predicted = lsd_depth_fuser.predict(lambda : predict_input_fn(rgb, sparse_idepth, sparse_idepth_var)) 106 | depthmap_scaled = list(depthmap_predicted)[0]['depth'][0] 107 | 108 | #invrting depthmap 109 | depthmap_scaled = np.where(depthmap_scaled>0.2, 1./depthmap_scaled, np.zeros_like(depthmap_scaled)) 110 | 111 | fig, axs = plt.subplots(1, 2) 112 | norm = colors.Normalize(vmin=0, vmax=gt_max) 113 | cmap = 'hot' 114 | images = [] 115 | images.append(axs[0].imshow(depthmap_scaled, cmap=cmap)) 116 | images.append(axs[1].imshow(depth_gt, cmap=cmap)) 117 | for im in images: 118 | im.set_norm(norm) 119 | fig.colorbar(images[0], ax=axs, orientation='horizontal', fraction=.1) 120 | #plt.figure("sparse_depth") 121 | #plt.imshow(sparse_depth, cmap='hot', norm=norm) 122 | #plt.clim(0,gt_max) 123 | #plt.colorbar() 124 | #plt.figure("gt_depth") 125 | #plt.imshow(depth_gt, cmap='hot', norm=norm) 126 | #plt.clim(0,gt_max) 127 | #plt.colorbar() 128 | #plt.figure("sparse_depth_variance") 129 | #plt.imshow(sparse_idepthVar, cmap='hot') 130 | #plt.figure("rgb") 131 | #plt.imshow(rgb) 132 | plt.show() 133 | 134 | plt.waitforbuttonpress() 135 | 136 | #plt.imshow(depthmap_scaled, cmap='hot') 137 | #plt.colorbar() 138 | #plt.show() 139 | 140 | def run(): 141 | #parser = argparse.ArgumentParser(description=( "Checks validity of one Keyframe batch inside training data generated from LSD Slam to refine depth.")) 142 | #parser.add_argument("--dataset_dir", type=str, required=True, help="training data directory to be verified") 143 | #parser.add_argument("--basename", type=str, required=True, help="basename of the Keyframe batch in training data") 144 | #parser.add_argument('--debug', action='store_true', help="enable debug outputs") 145 | 146 | #args = parser.parse_args() 147 | #assert os.path.isdir(args.dataset_dir) 148 | model_name = "NetV03_L1SigEqL1_down_aug" 149 | model_function = model_fn_Netv3_LossL1SigL1 150 | #configure(model_name, model_function) 151 | test_network(model_name, model_function) 152 | 153 | 154 | if __name__ == "__main__": 155 | run() 156 | sys.exit() -------------------------------------------------------------------------------- /notebooks/single_image_depth_predictor.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import tensorflow as tf\n", 10 | "from deepTAM.helpers2 import *\n", 11 | "from deepTAM.common_netcode.myHelpers import *\n", 12 | "gpu_options = tf.GPUOptions()\n", 13 | "gpu_options.per_process_gpu_memory_fraction=0.8\n", 14 | "sess = tf.InteractiveSession(config=tf.ConfigProto(allow_soft_placement=True, gpu_options=gpu_options))\n", 15 | "\n", 16 | "# Option 1\n", 17 | "saver = tf.train.import_meta_graph('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065.meta')\n", 18 | "saver.restore(sess, '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065')\n", 19 | "\n", 20 | "# Option 2 (Huizhong's way)\n", 21 | "from deepTAM.evaluation.helpers import *\n", 22 | "from tfutils import optimistic_restore\n", 23 | "import cv2\n", 24 | "import numpy as np\n", 25 | "\n", 26 | "mapping_mod = load_myNetworks_module_noname('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/net/myNetworks.py')\n", 27 | "single_image_depth_net = mapping_mod.SingleImageDepthNetwork(batch_size=1, width=320, height=240)\n", 28 | "single_image_depth_outputs = single_image_depth_net.build_net(**single_image_depth_net.placeholders)\n", 29 | "sess.run(tf.global_variables_initializer())\n", 30 | "checkpoint = '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065'\n", 31 | "optimistic_restore(sess,checkpoint,verbose=True)\n", 32 | "\t\n", 33 | "rgb = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb/1305032227.675702.png', -1)\n", 34 | "rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n", 35 | "rgb_new = cv2.resize(rgb_new,(320, 240))\n", 36 | "rgb_new = np.transpose(rgb_new, (2,0,1))\n", 37 | "\n", 38 | "feed_dict = {\n", 39 | " single_image_depth_net.placeholders['image']: rgb_new[np.newaxis, :, :, :],\n", 40 | " }\n", 41 | "\n", 42 | "single_image_depth_out = sess.run(single_image_depth_outputs, feed_dict=feed_dict)\n", 43 | "depth_pr = single_image_depth_out['predict_depth']\n", 44 | "\n", 45 | "cv2.imshow(\"predicted depth\", depth_pr[0,0])\n", 46 | "cv2.imshow(\"input image\", rgb)\n", 47 | "cv2.waitKey(0)" 48 | ] 49 | }, 50 | { 51 | "cell_type": "code", 52 | "execution_count": null, 53 | "metadata": {}, 54 | "outputs": [], 55 | "source": [ 56 | "import tensorflow as tf\n", 57 | "from deepTAM.helpers2 import *\n", 58 | "from deepTAM.common_netcode.myHelpers import *\n", 59 | "from deepTAM.evaluation.helpers import *\n", 60 | "from tfutils import optimistic_restore\n", 61 | "import cv2\n", 62 | "import numpy as np\n", 63 | "from cv_bridge import CvBridge" 64 | ] 65 | }, 66 | { 67 | "cell_type": "code", 68 | "execution_count": null, 69 | "metadata": {}, 70 | "outputs": [], 71 | "source": [ 72 | "gpu_options = tf.GPUOptions()\n", 73 | "gpu_options.per_process_gpu_memory_fraction=0.8\n", 74 | "sess = tf.InteractiveSession(config=tf.ConfigProto(allow_soft_placement=True, gpu_options=gpu_options))\n" 75 | ] 76 | }, 77 | { 78 | "cell_type": "code", 79 | "execution_count": null, 80 | "metadata": {}, 81 | "outputs": [], 82 | "source": [ 83 | "mapping_mod = load_myNetworks_module_noname('/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/net/myNetworks.py')\n", 84 | "single_image_depth_net = mapping_mod.SingleImageDepthNetwork(batch_size=1, width=320, height=240)\n", 85 | "single_image_depth_outputs = single_image_depth_net.build_net(**single_image_depth_net.placeholders)\n", 86 | "sess.run(tf.global_variables_initializer())\n", 87 | "checkpoint = '/misc/lmbraid19/thomasa/deep-networks/deepTAM/nets_multiframes/depth_singleimage/training_v_sun3d/1_d_big/recovery_checkpoints/snapshot-391065'\n", 88 | "optimistic_restore(sess,checkpoint,verbose=True)\n" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "rgb = cv2.imread('/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb/1305032227.675702.png', -1)\n", 98 | "rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F)\n", 99 | "rgb_new = cv2.resize(rgb_new,(320, 240))\n", 100 | "rgb_new = np.transpose(rgb_new, (2,0,1))\n" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "feed_dict = {\n", 110 | " single_image_depth_net.placeholders['image']: rgb_new[np.newaxis, :, :, :],\n", 111 | " }\n", 112 | "\n", 113 | "single_image_depth_out = sess.run(single_image_depth_outputs, feed_dict=feed_dict)" 114 | ] 115 | }, 116 | { 117 | "cell_type": "code", 118 | "execution_count": null, 119 | "metadata": {}, 120 | "outputs": [], 121 | "source": [ 122 | "depth_pr = single_image_depth_out['predict_depth'][0,0]\n" 123 | ] 124 | }, 125 | { 126 | "cell_type": "code", 127 | "execution_count": null, 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "_cv_bridge_depth = CvBridge()" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "depthmap_predicted_msg = _cv_bridge_depth.cv2_to_imgmsg(depth_pr, \"passthrough\")" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "depthmap_predicted_msg.step = int (depthmap_predicted_msg.step)\n", 157 | "depthmap_predicted_msg.step" 158 | ] 159 | }, 160 | { 161 | "cell_type": "code", 162 | "execution_count": null, 163 | "metadata": {}, 164 | "outputs": [], 165 | "source": [] 166 | } 167 | ], 168 | "metadata": { 169 | "kernelspec": { 170 | "display_name": "Python 3", 171 | "language": "python", 172 | "name": "python3" 173 | }, 174 | "language_info": { 175 | "codemirror_mode": { 176 | "name": "ipython", 177 | "version": 3 178 | }, 179 | "file_extension": ".py", 180 | "mimetype": "text/x-python", 181 | "name": "python", 182 | "nbconvert_exporter": "python", 183 | "pygments_lexer": "ipython3", 184 | "version": "3.5.2" 185 | } 186 | }, 187 | "nbformat": 4, 188 | "nbformat_minor": 2 189 | } 190 | -------------------------------------------------------------------------------- /scripts/single_image_depth_predictor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import sys 5 | 6 | # for cv_bridge 7 | sys.path.insert(0, '/misc/lmbraid19/thomasa/catkin_ws/install/lib/python3/dist-packages') 8 | #for cv2 9 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 10 | 11 | import cv2 12 | import numpy as np 13 | 14 | from reinforced_visual_slam.srv import * 15 | import rospy 16 | from cv_bridge import CvBridge 17 | 18 | import tensorflow as tf 19 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 20 | from net.my_models import * 21 | 22 | class DepthmapPredictor(object): 23 | 24 | def __init__(self, input_img_width=640, input_img_height=480): 25 | rospy.init_node('single_image_depthmap_predictor') 26 | self._cv_bridge_rgb = CvBridge() 27 | self._cv_bridge_depth = CvBridge() 28 | 29 | def configure(self, model_dir, model_function, run_config, width=320, height=240): 30 | self.width = width 31 | self.height = height 32 | self.lsd_depth_predictor = tf.estimator.Estimator(model_fn=model_function, model_dir=model_dir, 33 | params={'data_format':"channels_first", 'multi_gpu':False}, 34 | config=run_config) 35 | rospy.logwarn("Finished loading the single image depth predictor network") 36 | 37 | def convertOpenCvRGBToTfTensor(self, rgb, data_format="channels_first"): 38 | #rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F) 39 | rgb_new = rgb.astype(np.float32)/255 40 | # Resize Keyframe RGB image to (320W,240H) and reshape it to (3, height, width) 41 | rgb_new = cv2.resize(rgb_new, (self.width, self.height)) 42 | if data_format=="channels_first": 43 | rgb_new = np.transpose(rgb_new, (2,0,1)) 44 | return rgb_new 45 | 46 | def predict_input_fn(self, rgb): 47 | feature_names = [ 48 | 'rgb' 49 | ] 50 | input_tensors = [rgb] 51 | inputs = dict(zip(feature_names, input_tensors)) 52 | #print(inputs['rgb'].shape) 53 | dataset = tf.data.Dataset.from_tensors(inputs) 54 | dataset = dataset.batch(1) 55 | iterator = dataset.make_one_shot_iterator() 56 | features = iterator.get_next() 57 | #print(features['rgb'].shape, features['sparseInverseDepth'].shape, 58 | #features['sparseInverseDepthVariance'].shape) 59 | return features 60 | 61 | def predict_depthmap_cb(self, req, debug=False): 62 | rospy.loginfo("Received image to predict depth") 63 | # Requirement: RGB has to np array float32, normalized in range (-0.5,0.5), with shape (1,3,H,W) 64 | # Get the Keyframe RGB image from ROS message and convert it to openCV Image format 65 | try: 66 | rgb_image = self._cv_bridge_rgb.imgmsg_to_cv2(req.rgb_image, "passthrough") 67 | except CvBridgeError as e: 68 | print(e) 69 | return PredictDepthmapResponse(0) 70 | if debug: 71 | cv2.imshow('Input RGB', rgb_image) 72 | rgb_tensor = self.convertOpenCvRGBToTfTensor(rgb_image) 73 | 74 | #***Get idepth prediction from net. Scale it back to input sparse idepth scale and invert it*** 75 | idepth_predictions = self.lsd_depth_predictor.predict(lambda : 76 | self.predict_input_fn(rgb_tensor)) 77 | prediction = list(idepth_predictions)[0]['depth'] 78 | depthmap_predicted = prediction[0] 79 | 80 | if prediction.shape[0] > 1: 81 | confidence = prediction[1] 82 | else: 83 | confidence = np.ones_like(depthmap_predicted) 84 | 85 | #** convert confidence to variance for LSD SLAM compatibality **# 86 | #** (approach 1): change k_res according to the confidence function used while training the network **# 87 | #k_res = 2.5 88 | #residual = -np.log(confidence)/k_res 89 | #** (approach 2): change minVar and maxVar as per need **# 90 | minVar = 0.0001 91 | maxVar = 0.125 92 | residual = maxVar - (maxVar - minVar)*confidence 93 | 94 | # Doing cv_bridge conversion 95 | depthmap_predicted_msg = self._cv_bridge_depth.cv2_to_imgmsg(depthmap_predicted, "passthrough") 96 | depthmap_predicted_msg.step = int (depthmap_predicted_msg.step) 97 | residual_msg = self._cv_bridge_depth.cv2_to_imgmsg(residual, "passthrough") 98 | residual_msg.step = int(residual_msg.step) 99 | if debug: 100 | print("depthmap_predicted max:", np.max(depthmap_predicted)) 101 | print("depthmap_predicted min:", np.min(depthmap_predicted)) 102 | print("depthmap_predicted.dtype", depthmap_predicted.dtype) 103 | print("depthmap_predicted.shape", depthmap_predicted.shape) 104 | plot_depth_gray = cv2.convertScaleAbs(depthmap_predicted*255./np.max(depthmap_predicted)) 105 | depth_predicted_plot = cv2.cvtColor(plot_depth_gray, cv2.COLOR_GRAY2RGB) 106 | depth_predicted_plot[:,:,0] = 255 - cv2.convertScaleAbs((0-depthmap_predicted)*255.) 107 | depth_predicted_plot[:,:,1] = 255 - cv2.convertScaleAbs((1-depthmap_predicted)*255.) 108 | depth_predicted_plot[:,:,2] = 255 - cv2.convertScaleAbs((2-depthmap_predicted)*255.) 109 | cv2.imshow('Predicted Depthmap', depth_predicted_plot) 110 | cv2.waitKey(0) 111 | ## uncomment below to plot with matplotlib 112 | #plt.figure("fused_depthmap (scale in m)") 113 | #plt.imshow(depthmap_predicted, cmap='hot') 114 | #plt.colorbar() 115 | #plt.show() 116 | 117 | return PredictDepthmapResponse(depthmap_predicted_msg, residual_msg) 118 | 119 | def run(self): 120 | service = rospy.Service('predict_depthmap', PredictDepthmap, self.predict_depthmap_cb) 121 | print("Ready to predict depthmaps") 122 | try: 123 | rospy.spin() 124 | except KeyboardInterrupt: 125 | print("Shutting down single image depth predictor") 126 | 127 | if __name__ == "__main__": 128 | predictor = DepthmapPredictor() 129 | 130 | # params to change for network: 131 | #model_name = "NetV0_L1SigL1_tr1" 132 | model_name = "NetV0l_L1SigL1_tr2" 133 | #model_function = modelfn_NetV0_LossL1SigL1 134 | model_function = modelfn_NetV0l_LossL1SigL1 135 | rospy.loginfo("Loading single image depth predictor model: %s", model_name) 136 | 137 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 138 | model_dir = os.path.join(model_base_dir, model_name) 139 | 140 | config = tf.ConfigProto() 141 | config.gpu_options.per_process_gpu_memory_fraction = 0.1 142 | run_config = tf.estimator.RunConfig(session_config=config) 143 | 144 | predictor.configure( 145 | model_dir=model_dir, 146 | model_function = model_function, 147 | width=640, height=480, 148 | run_config=run_config) 149 | predictor.run() 150 | -------------------------------------------------------------------------------- /notebooks/check_training_data.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "name": "stderr", 10 | "output_type": "stream", 11 | "text": [ 12 | "/misc/lmbraid19/thomasa/virtual-env/tf_venv/lib/python3.5/site-packages/h5py/__init__.py:36: FutureWarning: Conversion of the second argument of issubdtype from `float` to `np.floating` is deprecated. In future, it will be treated as `np.float64 == np.dtype(float).type`.\n", 13 | " from ._conv import register_converters as _register_converters\n" 14 | ] 15 | }, 16 | { 17 | "name": "stdout", 18 | "output_type": "stream", 19 | "text": [ 20 | "Using /misc/lmbraid19/thomasa/deep-networks/lmbspecialops/build/lib/lmbspecialops.so\n" 21 | ] 22 | } 23 | ], 24 | "source": [ 25 | "from reinforced_visual_slam.srv import *\n", 26 | "import rospy\n", 27 | "\n", 28 | "import matplotlib.pyplot as plt\n", 29 | "from matplotlib import colors\n", 30 | "\n", 31 | "import os\n", 32 | "import sys\n", 33 | "sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages')\n", 34 | "\n", 35 | "import cv2\n", 36 | "import numpy as np\n", 37 | "from skimage.transform import resize\n", 38 | "\n", 39 | "import tensorflow as tf\n", 40 | "\n", 41 | "sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion')\n", 42 | "from net.my_models import *\n", 43 | "\n", 44 | "import random\n", 45 | "\n", 46 | "import lmbspecialops as sops" 47 | ] 48 | }, 49 | { 50 | "cell_type": "markdown", 51 | "metadata": {}, 52 | "source": [ 53 | "# check sparse invariant gradients #" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": 2, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "# define the sig params to be checked!\n", 63 | "sig_params_list = [{'deltas':[1,], 'weights':[1,], 'epsilon': 1e-9},\n", 64 | " {'deltas':[2,], 'weights':[1,], 'epsilon': 1e-9},\n", 65 | " {'deltas':[4,], 'weights':[1,], 'epsilon': 1e-9},\n", 66 | " ]\n", 67 | "\n", 68 | "# helper function to invert gt depth\n", 69 | "def invert_finite_depth(x):\n", 70 | " # mask is true if input is finite and greater than 0. If condition is false, make it invalid (nan)\n", 71 | " mask = tf.logical_and(tf.is_finite(x), tf.greater(x, 0.))\n", 72 | " ix_clean = tf.where(mask, tf.reciprocal(x), tf.fill(x.shape, np.nan))\n", 73 | " return ix_clean\n" 74 | ] 75 | }, 76 | { 77 | "cell_type": "code", 78 | "execution_count": null, 79 | "metadata": {}, 80 | "outputs": [ 81 | { 82 | "name": "stdout", 83 | "output_type": "stream", 84 | "text": [ 85 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 86 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n", 87 | "Press Enter to continue...\n", 88 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 89 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n", 90 | "Press Enter to continue...\n", 91 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 92 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n", 93 | "Press Enter to continue...\n", 94 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 95 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n", 96 | "Press Enter to continue...\n", 97 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 98 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n", 99 | "Press Enter to continue...\n", 100 | "sigs_nhwc_x.shape (3, 240, 320, 1)\n", 101 | "sigs_nhwc_y.shape (3, 240, 320, 1)\n" 102 | ] 103 | } 104 | ], 105 | "source": [ 106 | "# load data file\n", 107 | "test_file = '/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/test/rgbd_lsdDepth_test.txt'\n", 108 | "base_path = os.path.dirname(test_file)\n", 109 | "sess = tf.Session()\n", 110 | "with open(test_file) as file:\n", 111 | " # read lines of file and loop over it randomly\n", 112 | " lines = file.readlines()\n", 113 | " random.shuffle(lines)\n", 114 | " cnt = 0\n", 115 | " for line in lines:\n", 116 | " # load only the depth GT image, convert to tensor and convert scale to meters\n", 117 | " depth_gt_file = line.strip().split(',')[0]\n", 118 | " depth_gt = tf.read_file(os.path.join(base_path, depth_gt_file))\n", 119 | " depth_tensor = tf.image.decode_png(depth_gt, dtype=tf.uint16)\n", 120 | " depth_tensor = tf.reshape(depth_tensor, [1, 480, 640, 1])\n", 121 | " depth_tensor = tf.cast(depth_tensor, tf.float32)\n", 122 | " depth_tensor = tf.scalar_mul(0.0002, depth_tensor)\n", 123 | " # make inverse depth similar to training input and downsample it\n", 124 | " idepth_tensor = invert_finite_depth(depth_tensor)\n", 125 | " idepth_tensor = tf.image.resize_images(idepth_tensor, [240, 320], align_corners=True, \n", 126 | " method=tf.image.ResizeMethod.NEAREST_NEIGHBOR)\n", 127 | " idepth_tensor = tf.transpose(idepth_tensor,[0,3,1,2])\n", 128 | " # find different scale invariant gradients and plot it alongside the depth gt\n", 129 | " sig_list = []\n", 130 | " for sig_param in sig_params_list:\n", 131 | " gt_sig = sops.scale_invariant_gradient(idepth_tensor,**sig_param)\n", 132 | " sig_list.append(sops.replace_nonfinite(tf.transpose(gt_sig, [0,2,3,1]))) \n", 133 | " sigs_nhwc_x = tf.concat([sig_list[0][:,:,:,0:1], sig_list[1][:,:,:,0:1], \n", 134 | " sig_list[2][:,:,:,0:1]], 0)\n", 135 | " sigs_nhwc_y = tf.concat([sig_list[0][:,:,:,1:], sig_list[1][:,:,:,1:], \n", 136 | " sig_list[2][:,:,:,1:]], 0)\n", 137 | " sigs_nhwc_x = sops.replace_nonfinite(sigs_nhwc_x)\n", 138 | " sigs_nhwc_y = sops.replace_nonfinite(sigs_nhwc_y)\n", 139 | " print('sigs_nhwc_x.shape', sigs_nhwc_x.shape)\n", 140 | " print('sigs_nhwc_y.shape', sigs_nhwc_y.shape)\n", 141 | " tf.summary.image('sigs_x', sigs_nhwc_x, max_outputs=3)\n", 142 | " tf.summary.image('sigs_y', sigs_nhwc_y, max_outputs=3)\n", 143 | " merged = tf.summary.merge_all()\n", 144 | " writer = tf.summary.FileWriter('/tmp/akhil/')\n", 145 | " summary = sess.run(merged)\n", 146 | " writer.add_summary(summary, cnt)\n", 147 | " cnt += 1\n", 148 | " input(\"Press Enter to continue...\")" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [] 157 | } 158 | ], 159 | "metadata": { 160 | "kernelspec": { 161 | "display_name": "Python 3", 162 | "language": "python", 163 | "name": "python3" 164 | }, 165 | "language_info": { 166 | "codemirror_mode": { 167 | "name": "ipython", 168 | "version": 3 169 | }, 170 | "file_extension": ".py", 171 | "mimetype": "text/x-python", 172 | "name": "python", 173 | "nbconvert_exporter": "python", 174 | "pygments_lexer": "ipython3", 175 | "version": "3.5.2" 176 | } 177 | }, 178 | "nbformat": 4, 179 | "nbformat_minor": 2 180 | } 181 | -------------------------------------------------------------------------------- /networks/depth_fusion/net/my_losses.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import sys 3 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 4 | from net.helpers import * 5 | 6 | sig_params_list = [{'deltas':[1,], 'weights':[1,], 'epsilon': 1e-9}, 7 | {'deltas':[2,], 'weights':[1,], 'epsilon': 1e-9}, 8 | {'deltas':[4,], 'weights':[1,], 'epsilon': 1e-9}, 9 | ] 10 | 11 | def pointwise_l1_loss_sig_l1_loss_res_l1_loss(pred, gt, data_format='channels_first', weights=[0.4, 0.6, 0.5], 12 | res_converter=res_converter_exp_conf, sig_params_list=sig_params_list): 13 | """ Computes pointwise l1 loss and sig loss (gradient) of depth, and loss of residual 14 | The input tensors must use the format NCHW. 15 | This loss ignores nan values. 16 | Total loss is weighted sum of both normalized by the number of pixels. 17 | """ 18 | with tf.name_scope('pointwise_l1_loss_sig_l1_loss_res_l1_loss'): 19 | if data_format=='channels_first': 20 | depth = pred[:,0:1,:,:] 21 | res = pred[:,1:,:,:] 22 | else: 23 | depth = pred[:,:,:,0:1] 24 | res = pred[:,:,:,1:] 25 | loss_depth_l1 = mean_l1_loss_nonfinite(depth, gt) 26 | tf.summary.scalar('loss_depth_l1', loss_depth_l1) 27 | tf.losses.add_loss(weights[0] * loss_depth_l1) 28 | 29 | # finding scale invarient gradients of prediction and gt using different params 30 | print('depth.shape', depth.shape) 31 | inp_sig = get_multi_sig_list(depth, sig_params_list) 32 | gt_sig = get_multi_sig_list(gt, sig_params_list) 33 | print('inp_sig[0].shape', inp_sig[0].shape) 34 | 35 | 36 | for ind in range(len(inp_sig)): 37 | # adding the sig images tp tensorboard 38 | if data_format=='channels_last': 39 | inp_sig_nhwc = sops.replace_nonfinite(inp_sig[ind]) 40 | gt_sig_nhwc = sops.replace_nonfinite(gt_sig[ind]) 41 | else: 42 | inp_sig_nhwc = sops.replace_nonfinite(tf.transpose(inp_sig[ind], [0,2,3,1])) 43 | gt_sig_nhwc = sops.replace_nonfinite(tf.transpose(gt_sig[ind], [0,2,3,1])) 44 | sigs_nhwc = tf.concat([inp_sig_nhwc[0:1,:,:,0:1], gt_sig_nhwc[0:1,:,:,0:1], 45 | inp_sig_nhwc[0:1,:,:,1:], gt_sig_nhwc[0:1,:,:,1:]], 2) 46 | print('sigs_nhwc.shape', sigs_nhwc.shape) 47 | tf.summary.image('sigs'+str(ind), sigs_nhwc, max_outputs=1) 48 | loss_depth2_sig = mean_l1_loss_nonfinite(inp_sig[ind], gt_sig[ind]) 49 | tf.summary.scalar('loss_depth2_sig'+str(ind), loss_depth2_sig) 50 | tf.losses.add_loss(weights[1] * loss_depth2_sig) 51 | 52 | # finding loss for depth residual prediction 53 | res_gt = gt - depth 54 | res_gt = res_converter(res_gt) 55 | # writing residuals to tensorboard 56 | if data_format=='channels_last': 57 | res_gt_nhwc = sops.replace_nonfinite(res_gt) 58 | res_pr_nhwc = res 59 | else: 60 | res_gt_nhwc = sops.replace_nonfinite(tf.transpose(res_gt, [0,2,3,1])) 61 | res_pr_nhwc = tf.transpose(res, [0,2,3,1]) 62 | res_nhwc = tf.concat([res_pr_nhwc[0:1, :, :, :], res_gt_nhwc[0:1, :, :, :]], 2) 63 | tf.summary.image('confidence', res_nhwc, max_outputs=1) 64 | 65 | # calculating loss 66 | loss_res = mean_l1_loss_nonfinite(res, res_gt) 67 | tf.summary.scalar('loss_res', loss_res) 68 | tf.losses.add_loss(weights[2] * loss_res) 69 | 70 | return tf.losses.get_total_loss() 71 | 72 | def pointwise_l2_loss(inp, gt, epsilon=1e-6, data_format='channels_last'): 73 | """Computes the pointwise unsquared l2 loss. 74 | The input tensors must use the format NCHW. 75 | This loss ignores nan values. 76 | The loss is normalized by the number of pixels. 77 | 78 | inp: Tensor 79 | This is the prediction. 80 | 81 | gt: Tensor 82 | The ground truth with the same shape as 'inp' 83 | 84 | epsilon: float 85 | The epsilon value to avoid division by zero in the gradient computation 86 | """ 87 | with tf.name_scope('pointwise_l2_loss'): 88 | gt_ = tf.stop_gradient(gt) 89 | diff = replace_nonfinite(inp-gt_) 90 | if data_format == 'channels_first': 91 | return tf.reduce_mean(tf.sqrt(tf.reduce_sum(diff**2, axis=1)+epsilon)) 92 | else: # NHWC 93 | return tf.reduce_mean(tf.sqrt(tf.reduce_sum(diff**2, axis=3)+epsilon)) 94 | 95 | def mean_l1_loss_nonfinite(inp, gt, epsilon=1e-6): 96 | """L1 loss ignoring nonfinite values. 97 | 98 | Returns a scalar tensor with the loss. 99 | The loss is the mean. 100 | """ 101 | with tf.name_scope("l1_loss_nonfinite"): 102 | gt_ = tf.stop_gradient(gt) 103 | diff = sops.replace_nonfinite(inp-gt_) 104 | return tf.reduce_mean(tf.sqrt(diff**2 + epsilon)) 105 | 106 | def pointwise_l2_loss_clean(inp, gt, epsilon=1e-6, data_format='channels_last'): 107 | """Computes the pointwise unsquared l2 loss. 108 | The input tensors must use the format NCHW. 109 | This loss ignores nan values. 110 | The loss is normalized by the number of pixels. 111 | 112 | inp: Tensor 113 | This is the prediction. 114 | 115 | gt: Tensor 116 | The ground truth with the same shape as 'inp' 117 | 118 | epsilon: float 119 | The epsilon value to avoid division by zero in the gradient computation 120 | """ 121 | with tf.name_scope('pointwise_l2_loss_clean'): 122 | gt_ = tf.stop_gradient(gt) 123 | diff = replace_nonfinite_loss(inp-gt_, gt_) 124 | if data_format == 'channels_first': 125 | return tf.reduce_mean(tf.sqrt(tf.reduce_sum(diff**2, axis=1)+epsilon)) 126 | else: # NHWC 127 | return tf.reduce_mean(tf.sqrt(tf.reduce_sum(diff**2, axis=3)+epsilon)) 128 | 129 | def mean_l1_loss_robust(inp, gt, epsilon=1e-6): 130 | """ L1 loss ignoring nonfinite values. 131 | 132 | inp: Tensor 133 | This is the prediction. 134 | 135 | gt: Tensor 136 | The ground truth with the same shape as 'inp' 137 | 138 | epsilon: float 139 | The epsilon value to avoid division by zero in the gradient computation 140 | """ 141 | with tf.name_scope('pointwise_l1_loss_robust'): 142 | gt_ = tf.stop_gradient(gt) 143 | diff = replace_nonfinite_loss(inp-gt_, gt_) 144 | return tf.reduce_mean(tf.sqrt(diff**2 + epsilon)) 145 | 146 | 147 | def pointwise_l1_loss_sig_l1_loss(inp, gt, data_format='channels_first', weights=[0.4, 0.6], sig_params_list=sig_params_list): 148 | """ Computes pointwise l1 loss and sig loss (gradient). 149 | The input tensors must use the format NCHW. 150 | This loss ignores nan values. 151 | Total loss is weighted sum of both normalized by the number of pixels. 152 | """ 153 | with tf.name_scope('pointwise_l1_loss_sig_l1_loss'): 154 | loss_depth_l1 = mean_l1_loss_nonfinite(inp, gt) 155 | tf.summary.scalar('loss_depth_l1', loss_depth_l1) 156 | tf.losses.add_loss(weights[0] * loss_depth_l1) 157 | 158 | # finding scale invarient gradients of prediction and gt using different params 159 | inp_sig = get_multi_sig_list(inp, sig_params_list) 160 | gt_sig = get_multi_sig_list(gt, sig_params_list) 161 | 162 | for ind in range(len(inp_sig)): 163 | loss_depth2_sig = mean_l1_loss_nonfinite(inp_sig[ind], gt_sig[ind]) 164 | tf.summary.scalar('loss_depth2_sig'+str(ind), loss_depth2_sig) 165 | tf.losses.add_loss(weights[1] * loss_depth2_sig) 166 | return tf.losses.get_total_loss() 167 | 168 | 169 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(reinforced_visual_slam) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | cv_bridge 12 | geometry_msgs 13 | message_generation 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | find_package(OpenCV REQUIRED) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | keyframeMsg.msg 57 | keyframeGraphMsg.msg 58 | keyframeMsgSiasa.msg 59 | ) 60 | 61 | ## Generate services in the 'srv' folder 62 | add_service_files( 63 | FILES 64 | TrackImage.srv 65 | TrackerStatus.srv 66 | PredictDepthmap.srv 67 | DepthFusion.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | geometry_msgs 81 | sensor_msgs 82 | std_msgs 83 | ) 84 | 85 | ################################################ 86 | ## Declare ROS dynamic reconfigure parameters ## 87 | ################################################ 88 | 89 | ## To declare and build dynamic reconfigure parameters within this 90 | ## package, follow these steps: 91 | ## * In the file package.xml: 92 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 93 | ## * In this file (CMakeLists.txt): 94 | ## * add "dynamic_reconfigure" to 95 | ## find_package(catkin REQUIRED COMPONENTS ...) 96 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 97 | ## and list every .cfg file to be processed 98 | 99 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 100 | # generate_dynamic_reconfigure_options( 101 | # cfg/DynReconf1.cfg 102 | # cfg/DynReconf2.cfg 103 | # ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if your package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | # INCLUDE_DIRS include 116 | # LIBRARIES reinforced_v_slam 117 | # CATKIN_DEPENDS cv_bridge geometry_msgs message_generation rospy sensor_msgs std_msgs 118 | # DEPENDS system_lib 119 | CATKIN_DEPENDS message_runtime 120 | ) 121 | 122 | ########### 123 | ## Build ## 124 | ########### 125 | 126 | ## Specify additional locations of header files 127 | ## Your package locations should be listed before other locations 128 | include_directories( 129 | # include 130 | ${catkin_INCLUDE_DIRS} 131 | ${OPENCV_INCLUDE_DIRS} 132 | ) 133 | 134 | ## Declare a C++ library 135 | # add_library(${PROJECT_NAME} 136 | # src/${PROJECT_NAME}/reinforced_v_slam.cpp 137 | # ) 138 | 139 | ## Add cmake target dependencies of the library 140 | ## as an example, code may need to be generated before libraries 141 | ## either from message generation or dynamic reconfigure 142 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Declare a C++ executable 145 | ## With catkin_make all packages are built within a single CMake context 146 | ## The recommended prefix ensures that target names across packages don't collide 147 | # add_executable(${PROJECT_NAME}_node src/reinforced_v_slam_node.cpp) 148 | 149 | ## Rename C++ executable without prefix 150 | ## The above recommended prefix causes long target names, the following renames the 151 | ## target back to the shorter version for ease of user use 152 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 153 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 154 | 155 | ## Add cmake target dependencies of the executable 156 | ## same as for the library above 157 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 158 | 159 | ## Specify libraries to link a library or executable target against 160 | # target_link_libraries(${PROJECT_NAME}_node 161 | # ${catkin_LIBRARIES} 162 | # ) 163 | 164 | ############# 165 | ## Install ## 166 | ############# 167 | 168 | # all install targets should use catkin DESTINATION variables 169 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 170 | 171 | ## Mark executable scripts (Python etc.) for installation 172 | ## in contrast to setup.py, you can choose the destination 173 | # install(PROGRAMS 174 | # scripts/my_python_script 175 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark executables and/or libraries for installation 179 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 180 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark cpp header files for installation 186 | # install(DIRECTORY include/${PROJECT_NAME}/ 187 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 188 | # FILES_MATCHING PATTERN "*.h" 189 | # PATTERN ".svn" EXCLUDE 190 | # ) 191 | 192 | ## Mark other files for installation (e.g. launch and bag files, etc.) 193 | # install(FILES 194 | # # myfile1 195 | # # myfile2 196 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 197 | # ) 198 | 199 | ############# 200 | ## Testing ## 201 | ############# 202 | 203 | ## Add gtest based cpp test target and link libraries 204 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_reinforced_v_slam.cpp) 205 | # if(TARGET ${PROJECT_NAME}-test) 206 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 207 | # endif() 208 | 209 | ## Add folders to be run by python nosetests 210 | # catkin_add_nosetests(test) 211 | -------------------------------------------------------------------------------- /scripts/depthmap_fuser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import sys 5 | 6 | # for cv_bridge 7 | sys.path.insert(0, '/misc/lmbraid19/thomasa/catkin_ws/install/lib/python3/dist-packages') 8 | #for cv2 9 | sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages') 10 | 11 | import cv2 12 | import numpy as np 13 | 14 | #import matplotlib 15 | #matplotlib.use('agg') 16 | #import matplotlib.pyplot as plt 17 | 18 | from reinforced_visual_slam.srv import * 19 | import rospy 20 | from cv_bridge import CvBridge 21 | 22 | import tensorflow as tf 23 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 24 | from net.my_models import * 25 | 26 | class DepthmapFuser(object): 27 | 28 | def __init__(self, input_img_width=640, input_img_height=480): 29 | rospy.init_node('depthmap_fuser') 30 | self._cv_bridge_rgb = CvBridge() 31 | self._cv_bridge_depth = CvBridge() 32 | 33 | def configure(self, model_dir, model_function, width=320, height=240): 34 | self.width = width 35 | self.height = height 36 | self.lsd_depth_fuser = tf.estimator.Estimator(model_fn=model_function, model_dir=model_dir, 37 | params={'data_format':"channels_first", 'multi_gpu':False}) 38 | rospy.logwarn("Finished loading the depthmap fuser network") 39 | 40 | def convertOpenCvRGBToTfTensor(self, rgb, data_format="channels_first"): 41 | #rgb_new = cv2.normalize(rgb, None, alpha=-0.5, beta=0.5, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F) 42 | rgb_new = rgb.astype(np.float32)/255 43 | # Resize Keyframe RGB image to (320W,240H) and reshape it to (3, height, width) 44 | rgb_new = cv2.resize(rgb_new, (self.width, self.height)) 45 | if data_format=="channels_first": 46 | rgb_new = np.transpose(rgb_new, (2,0,1)) 47 | return rgb_new 48 | 49 | def predict_input_fn(self, rgb, sparse_idepth, sparse_idepth_var): 50 | feature_names = [ 51 | 'rgb', 52 | 'sparseInverseDepth', 53 | 'sparseInverseDepthVariance' 54 | ] 55 | input_tensors = [rgb, sparse_idepth[np.newaxis,:,:], sparse_idepth_var[np.newaxis,:,:]] 56 | inputs = dict(zip(feature_names, input_tensors)) 57 | #print(inputs['rgb'].shape) 58 | dataset = tf.data.Dataset.from_tensors(inputs) 59 | dataset = dataset.batch(1) 60 | iterator = dataset.make_one_shot_iterator() 61 | features = iterator.get_next() 62 | #print(features['rgb'].shape, features['sparseInverseDepth'].shape, 63 | #features['sparseInverseDepthVariance'].shape) 64 | return features 65 | 66 | def fuse_depthmap_cb(self, req, debug=False): 67 | rospy.loginfo("Received image and sparse idepths to fuse") 68 | # Requirement: RGB has to np array float32, normalized in range (-0.5,0.5), with shape (1,3,240,320) 69 | # Get the Keyframe RGB image from ROS message and convert it to openCV Image format 70 | try: 71 | rgb_image = self._cv_bridge_rgb.imgmsg_to_cv2(req.rgb_image, "passthrough") 72 | except CvBridgeError as e: 73 | print(e) 74 | return DepthFusionResponse(0) 75 | if debug: 76 | cv2.imshow('Input RGB', rgb_image) 77 | rgb_tensor = self.convertOpenCvRGBToTfTensor(rgb_image) 78 | 79 | # Get input sparse idepth and its variance. Convert it to gt scale (m) 80 | idepth_descaled = np.array(req.idepth, dtype=np.float32)/req.scale 81 | idepth_var_descaled = np.array(req.idepth_var, dtype=np.float32)/req.scale 82 | 83 | # Reshape and resize sparse idepths 84 | sparse_idepth = idepth_descaled.reshape((480, 640)) 85 | sparse_idepth = cv2.resize(sparse_idepth, (320, 240), cv2.INTER_NEAREST) 86 | 87 | sparse_idepth_var = idepth_var_descaled.reshape((480, 640)) 88 | sparse_idepth_var = cv2.resize(sparse_idepth_var, (320, 240), cv2.INTER_NEAREST) 89 | 90 | #***Get idepth prediction from net. Scale it back to input sparse idepth scale and invert it*** 91 | idepth_predictions = self.lsd_depth_fuser.predict(lambda : 92 | self.predict_input_fn(rgb_tensor, sparse_idepth, sparse_idepth_var)) 93 | #idepthmap_scaled = list(idepth_predictions)[0]['depth'][0] * req.scale 94 | prediction = list(idepth_predictions)[0]['depth'] 95 | depthmap_predicted = prediction[0] 96 | 97 | if prediction.shape[0] > 1: 98 | confidence = prediction[1] 99 | else: 100 | confidence = np.ones_like(depthmap_predicted) 101 | 102 | #** convert confidence to variance for LSD SLAM compatibality **# 103 | #** (approach 1): change k_res according to the confidence function used while training the network **# 104 | #k_res = 2.5 105 | #residual = -np.log(confidence)/k_res 106 | #** (approach 2): change minVar and maxVar as per need **# 107 | minVar = 0.0001 108 | maxVar = 0.125 109 | residual = maxVar - (maxVar - minVar)*confidence 110 | 111 | # Doing cv_bridge conversion 112 | depthmap_predicted_msg = self._cv_bridge_depth.cv2_to_imgmsg(depthmap_predicted, "passthrough") 113 | depthmap_predicted_msg.step = int (depthmap_predicted_msg.step) 114 | residual_msg = self._cv_bridge_depth.cv2_to_imgmsg(residual, "passthrough") 115 | residual_msg.step = int(residual_msg.step) 116 | if debug: 117 | print("depthmap_predicted max:", np.max(depthmap_predicted)) 118 | print("depthmap_predicted min:", np.min(depthmap_predicted)) 119 | print("depthmap_predicted.dtype", depthmap_predicted.dtype) 120 | print("depthmap_predicted.shape", depthmap_predicted.shape) 121 | plot_depth_gray = cv2.convertScaleAbs(depthmap_predicted*255./np.max(depthmap_predicted)) 122 | depth_predicted_plot = cv2.cvtColor(plot_depth_gray, cv2.COLOR_GRAY2RGB) 123 | depth_predicted_plot[:,:,0] = 255 - cv2.convertScaleAbs((0-depthmap_predicted*req.scale)*255.) 124 | depth_predicted_plot[:,:,1] = 255 - cv2.convertScaleAbs((1-depthmap_predicted*req.scale)*255.) 125 | depth_predicted_plot[:,:,2] = 255 - cv2.convertScaleAbs((2-depthmap_predicted*req.scale)*255.) 126 | cv2.imshow('Predicted Depthmap', depth_predicted_plot) 127 | cv2.waitKey(100) 128 | ## uncomment below to plot with matplotlib 129 | #plt.figure("fused_depthmap (scale in m)") 130 | #plt.imshow(depthmap_predicted, cmap='hot') 131 | #plt.colorbar() 132 | #plt.show() 133 | 134 | return DepthFusionResponse(depthmap_predicted_msg, residual_msg) 135 | 136 | def run(self): 137 | service = rospy.Service('fuse_depthmap', DepthFusion, self.fuse_depthmap_cb) 138 | print("Ready to predict depthmaps") 139 | try: 140 | rospy.spin() 141 | except KeyboardInterrupt: 142 | print("Shutting down depthmap fuser") 143 | 144 | if __name__ == "__main__": 145 | fuser = DepthmapFuser() 146 | 147 | # params to change for network: 148 | model_name = "NetV02_L1Sig4L1_down_tr1" 149 | #model_name = "NetV04Res_L1Sig4L1ExpResL1_down_tr2" 150 | #model_name = "NetV04Res_L1Sig4L1ExpResL1_down_tr1" 151 | #model_name = "NetV02_L1SigL1_down_aug_" 152 | #model_name = "NetV03_L1SigEqL1_down_aug" 153 | #model_function = model_fn_NetV04Res_LossL1SigL1ExpResL1 154 | model_function = model_fn_Netv2_LossL1SigL1_down 155 | #model_function = model_fn_Netv3_LossL1SigL1 156 | rospy.loginfo("Loading depth fusion model: %s", model_name) 157 | 158 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 159 | model_dir = os.path.join(model_base_dir, model_name) 160 | 161 | fuser.configure( 162 | model_dir=model_dir, 163 | model_function = model_function) 164 | fuser.run() 165 | -------------------------------------------------------------------------------- /networks/depth_fusion/trainer_singleImage.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from __future__ import absolute_import 3 | from __future__ import print_function 4 | import argparse 5 | import numpy as np 6 | import tensorflow as tf 7 | 8 | from tensorflow.python import debug as tf_debug 9 | 10 | import sys 11 | import os 12 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 13 | from net.my_networks import * 14 | from net.my_losses import * 15 | from net.my_models import * 16 | 17 | feature_names = [ 18 | 'rgb'] 19 | 20 | FLAGS = { 21 | 'multi_gpu': False, 22 | 'batch_size':12, 23 | 'prefetch_buffer_size': 12, 24 | 'num_parallel_calls': 12, 25 | 'num_epochs':50, 26 | 'data_format': "channels_first", 27 | 'epochs_bw_eval':1, 28 | 'downsample': False 29 | } 30 | 31 | augment_props = { 32 | 'max_contrast_factor': 1.1, 33 | 'max_brightness_factor': 0.05, 34 | } 35 | 36 | def parse_n_load(filename_line, basepath, data_format=FLAGS['data_format'], augment=True, width=320, height=240): 37 | filenames = tf.decode_csv(filename_line, [[''], [''], [''], ['']]) 38 | images = [] 39 | for cnt in range(2): 40 | image_string = tf.read_file(basepath +'/'+ filenames[cnt]) 41 | if cnt == 0: 42 | image_decoded = tf.image.decode_png(image_string, dtype=tf.uint16) 43 | image_decoded = tf.reshape(image_decoded, [480, 640, 1]) 44 | image_decoded = tf.cast(image_decoded, tf.float32) 45 | image_decoded = tf.scalar_mul(0.0002, image_decoded) 46 | # converting depth to idepth but keeping NaN's 47 | image_decoded = invert_finite_depth(image_decoded) 48 | else: 49 | image_decoded = tf.image.decode_png(image_string) 50 | image_decoded = tf.reshape(image_decoded, [480, 640, 3]) 51 | image_decoded = tf.cast(image_decoded, tf.float32) 52 | # converting rgb to [0,1] from [0,255] 53 | image_decoded = tf.divide(image_decoded, 255) 54 | if FLAGS['downsample']: 55 | image_decoded = tf.image.resize_images(image_decoded, [height, width], 56 | align_corners=True) 57 | if data_format == 'channels_first': 58 | image_decoded = tf.transpose(image_decoded,[2,0,1]) 59 | images.append(image_decoded) 60 | idepth_gt_clean = images[0] 61 | rgb = images[1] 62 | if augment: 63 | rgb, idepth_gt_clean = random_augmentations(rgb, idepth_gt_clean) 64 | d = dict(zip(feature_names, [rgb])), idepth_gt_clean 65 | return d 66 | 67 | def random_augmentations(rgb, idepth_gt): 68 | rand_nums = np.random.rand(2) 69 | rand_bools = rand_nums < 0.3 70 | # brightness and contrast transform only for rgb 71 | rgb = tf.image.random_brightness(rgb, max_delta=augment_props['max_brightness_factor']) 72 | rgb = tf.image.random_contrast(rgb, upper=augment_props['max_contrast_factor'], 73 | lower=1./augment_props['max_contrast_factor']) 74 | # input augmentations (for all inputs) 75 | if rand_bools[0]: 76 | rgb = tf.image.flip_left_right(rgb) 77 | idepth_gt = tf.image.flip_left_right(idepth_gt) 78 | if rand_bools[1]: 79 | rgb = tf.image.flip_up_down(rgb) 80 | idepth_gt = tf.image.flip_up_down(idepth_gt) 81 | return(rgb, idepth_gt) 82 | 83 | 84 | def dataset_shuffler(training_file): 85 | filename_records = tf.data.TextLineDataset(training_file) 86 | #dataset = filename_records.shuffle(buffer_size=50000) 87 | dataset = filename_records.apply(tf.contrib.data.shuffle_and_repeat(50000, FLAGS['num_epochs'])) 88 | return dataset 89 | 90 | def input_fn(dataset, basename, augment=True, batch_size=FLAGS['batch_size']): 91 | #dataset = dataset.apply(tf.contrib.data.parallel_interleave(parse_n_extract_images, 92 | # cycle_length=FLAGS['num_parallel_calls'], sloppy=True)) 93 | #dataset = dataset.map(map_func=parse_n_load, num_parallel_calls=FLAGS['num_parallel_calls']) 94 | #dataset = dataset.batch(batch_size=FLAGS['batch_size']) 95 | dataset = dataset.apply(tf.contrib.data.map_and_batch(map_func=lambda filename:parse_n_load(filename, basename, augment=True), 96 | num_parallel_batches=2, batch_size=batch_size)) 97 | 98 | #dataset = dataset.shuffle(buffer_size=10000) 99 | 100 | dataset = dataset.prefetch(buffer_size=FLAGS['prefetch_buffer_size']) 101 | #iterator = dataset.make_one_shot_iterator() 102 | #features, label = iterator.get_next() 103 | #return features, label 104 | return dataset 105 | 106 | def train_input_fn(): 107 | training_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/train/rgbd_lsdDepth_train.txt" 108 | dataset = dataset_shuffler(training_file) 109 | basename = os.path.dirname(training_file) 110 | dataset = input_fn(dataset, basename, augment=True) 111 | return dataset 112 | 113 | def test_input_fn(): 114 | test_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/test/rgbd_lsdDepth_test.txt" 115 | dataset = tf.data.TextLineDataset(test_file) 116 | basename = os.path.dirname(test_file) 117 | dataset = dataset.map(map_func=lambda filename:parse_n_load(filename, basename, augment=False), 118 | num_parallel_calls=FLAGS['num_parallel_calls']) 119 | dataset = dataset.apply(tf.contrib.data.batch_and_drop_remainder(4)) 120 | #dataset = input_fn(dataset, basename, augment=False, batch_size=4) 121 | return dataset 122 | 123 | def validate_batch_size_for_multi_gpu(batch_size): 124 | """For multi-gpu, batch-size must be a multiple of the number of GPUs. 125 | Note that this should eventually be handled by replicate_model_fn 126 | directly. Multi-GPU support is currently experimental, however, 127 | so doing the work here until that feature is in place. 128 | Args: 129 | batch_size: the number of examples processed in each training batch. 130 | Raises: 131 | ValueError: if no GPUs are found, or selected batch_size is invalid. 132 | """ 133 | from tensorflow.python.client import device_lib # pylint: disable=g-import-not-at-top 134 | 135 | local_device_protos = device_lib.list_local_devices() 136 | num_gpus = sum([1 for d in local_device_protos if d.device_type == 'GPU']) 137 | if not num_gpus: 138 | raise ValueError('Multi-GPU mode was specified, but no GPUs ' 139 | 'were found. To use CPU, run without --multi_gpu.') 140 | 141 | remainder = batch_size % num_gpus 142 | if remainder: 143 | err = ('When running with multiple GPUs, batch size ' 144 | 'must be a multiple of the number of available GPUs. ' 145 | 'Found {} GPUs with a batch size of {}; try --batch_size={} instead.' 146 | ).format(num_gpus, batch_size, batch_size - remainder) 147 | raise ValueError(err) 148 | 149 | 150 | 151 | def run_trainer(model_fn, model_dir_name, training_steps, config): 152 | """Run training and eval loop for lsd depth fusion network. 153 | """ 154 | model_function = model_fn 155 | hooks = [tf_debug.LocalCLIDebugHook()] 156 | 157 | if FLAGS['multi_gpu']: 158 | validate_batch_size_for_multi_gpu(FLAGS['batch_size']) 159 | 160 | # There are two steps required if using multi-GPU: (1) wrap the model_fn, 161 | # and (2) wrap the optimizer. The first happens here, and (2) happens 162 | # in the model_fn itself when the optimizer is defined. 163 | model_function = tf.contrib.estimator.replicate_model_fn( 164 | model_fn, loss_reduction=tf.losses.Reduction.MEAN) 165 | 166 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 167 | model_dir = os.path.join(model_base_dir, model_dir_name) 168 | 169 | run_config = tf.estimator.RunConfig(session_config=config) 170 | 171 | lsd_depth_fuser = tf.estimator.Estimator( 172 | model_fn=model_function, 173 | model_dir=model_dir, 174 | params={ 175 | 'data_format': FLAGS['data_format'], 176 | 'multi_gpu': FLAGS['multi_gpu'] 177 | }, 178 | config=run_config) 179 | 180 | # Train and evaluate model. 181 | for _ in range(FLAGS['num_epochs'] // FLAGS['epochs_bw_eval']): 182 | lsd_depth_fuser.train(input_fn=train_input_fn, steps=training_steps) 183 | eval_results = lsd_depth_fuser.evaluate(input_fn=test_input_fn) 184 | print('\nEvaluation results:\n\t%s\n' % eval_results) 185 | 186 | 187 | #if model_helpers.past_stop_threshold(flags_obj.stop_threshold, 188 | # eval_results['loss']): 189 | #break 190 | # Export the model for later use 191 | if FLAGS['downsample']: 192 | rgb = tf.placeholder(tf.float32, [1, 3, 240, 320]) 193 | else: 194 | rgb = tf.placeholder(tf.float32, [1, 3, 480, 640]) 195 | export_input_fn = tf.estimator.export.build_raw_serving_input_receiver_fn({ 196 | 'rgb': rgb, 197 | }) 198 | lsd_depth_fuser.export_savedmodel(os.path.join(model_dir, "exported_model"), 199 | export_input_fn) 200 | 201 | def dataset_len(dataset_loc): 202 | with open(dataset_loc) as f: 203 | for i, l in enumerate(f): 204 | pass 205 | return i + 1 206 | 207 | 208 | def main(): 209 | parser = argparse.ArgumentParser(description=( "Train LSDDepth fusion network using the custom estimator.")) 210 | parser.add_argument("--model_name", type=str, required=True, help="Specify name of model (will be used as ouput dir_name)") 211 | parser.add_argument("--num_epochs", type=int, required=True, help="Number of training epochs") 212 | parser.add_argument("--limit_gpu", action='store_true', help="run program with limited GPU (dacky)") 213 | args = parser.parse_args() 214 | 215 | training_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/train/rgbd_lsdDepth_train.txt" 216 | data_len = dataset_len(training_file) 217 | steps_per_epoch = data_len // FLAGS['batch_size'] 218 | #num_steps = steps_per_epoch * args.num_epochs 219 | num_steps = steps_per_epoch * FLAGS['epochs_bw_eval'] 220 | 221 | FLAGS['num_epochs'] = args.num_epochs 222 | 223 | print("Number of training steps (until eval):", num_steps) 224 | #### 225 | #model_fn = modelfn_NetV0l_LossL1SigL1 226 | model_fn = modelfn_NetV0lRes_LossL1SigL1ExpResL1 227 | #### 228 | if args.limit_gpu: 229 | config = tf.ConfigProto() 230 | config.gpu_options.per_process_gpu_memory_fraction = 0.25 231 | FLAGS['multi_gpu'] = False 232 | else: 233 | config = None 234 | 235 | run_trainer(model_fn, args.model_name, num_steps, config) 236 | 237 | if __name__ == '__main__': 238 | tf.logging.set_verbosity(tf.logging.INFO) 239 | main() 240 | sys.exit() 241 | -------------------------------------------------------------------------------- /notebooks/LSDDepth dataset reader.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Checking validity of dataset" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "def check_validity(dataset_folder, basename, matplotlib=True, debug=False):\n", 17 | " import cv2\n", 18 | " import numpy as np\n", 19 | " import matplotlib.pyplot as plt\n", 20 | " import matplotlib\n", 21 | " #Inside notebook uncomment the following\n", 22 | " matplotlib.rcsetup.interactive_bk\n", 23 | " plt.switch_backend('nbAgg')\n", 24 | " \n", 25 | " # Setting paths of the keyframe batch\n", 26 | " depth_gt_png = dataset_folder + basename + \"depthGT.png\"\n", 27 | " rgb_png = dataset_folder + basename + \"rgb.png\"\n", 28 | " sparse_depth_bin = dataset_folder + basename + \"sparse_depth.bin\"\n", 29 | " sparse_variance_bin = dataset_folder + basename + \"sparse_depthVar.bin\"\n", 30 | " \n", 31 | " # opening depth_gt and converting it to float\n", 32 | " print(\"GT depth---\")\n", 33 | " depth_gt = cv2.imread(depth_gt_png, -1).astype(np.float32)/5000\n", 34 | " rgb = cv2.imread(rgb_png, -1)\n", 35 | " print(\"shape: \", depth_gt.shape)\n", 36 | " print(\"max: \", np.nanmax(depth_gt))\n", 37 | " print(\"dtype: \", depth_gt.dtype)\n", 38 | " \n", 39 | " # loading sparse idepth from bin (as half-float) and converting it to float32\n", 40 | " print(\"Sparse idepth ---\")\n", 41 | " sparse_idepth = np.fromfile(sparse_depth_bin, dtype=np.float16)\n", 42 | " if(debug):\n", 43 | " print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth))\n", 44 | " print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth))\n", 45 | " sparse_idepth = sparse_idepth.astype(np.float32)\n", 46 | " sparse_idepth = sparse_idepth.reshape((480, 640))\n", 47 | " print(\"max: \", np.nanmax(sparse_idepth))\n", 48 | " print(\"shape: \",sparse_idepth.shape)\n", 49 | " print(\"dtype: \", sparse_idepth.dtype)\n", 50 | " \n", 51 | " # converting sparse idepth to depth\n", 52 | " print(\"Sparse depth ---\")\n", 53 | " sparse_depth = 1./sparse_idepth\n", 54 | " print(\"max: \", np.nanmax(sparse_depth))\n", 55 | " print(\"shape: \", sparse_depth.shape)\n", 56 | " print(\"dtype: \", sparse_depth.dtype)\n", 57 | " \n", 58 | " # loading sparse idepthVar from bin (as half-float) and converting it to float32\n", 59 | " print(\"Sparse idepthVar ---\")\n", 60 | " sparse_idepthVar = np.fromfile(sparse_variance_bin, dtype=np.float16)\n", 61 | " if(debug):\n", 62 | " print(\"min half-float sparse_idepthVar: \", np.nanmin(sparse_idepthVar))\n", 63 | " print(\"max half-float sparse_idepthVar: \", np.nanmax(sparse_idepthVar))\n", 64 | " sparse_idepthVar = sparse_idepthVar.astype(np.float32)\n", 65 | " sparse_idepthVar = sparse_idepthVar.reshape((480, 640))\n", 66 | " print(\"max: \", np.nanmax(sparse_idepthVar))\n", 67 | " print(\"shape: \", sparse_idepthVar.shape)\n", 68 | " print(\"dtype: \", sparse_idepthVar.dtype)\n", 69 | " \n", 70 | " # plotting images\n", 71 | " if matplotlib:\n", 72 | " # plot using matplotlib\n", 73 | " plt.figure(\"sparse_depth\")\n", 74 | " plt.imshow(sparse_depth, cmap='hot')\n", 75 | " plt.clim(0,4)\n", 76 | " plt.colorbar()\n", 77 | " plt.figure(\"gt_depth\")\n", 78 | " plt.imshow(depth_gt, cmap='hot')\n", 79 | " plt.clim(0,4)\n", 80 | " plt.colorbar()\n", 81 | " plt.figure(\"sparse_depth_variance\")\n", 82 | " plt.imshow(sparse_idepthVar, cmap='hot')\n", 83 | " plt.figure(\"rgb\")\n", 84 | " plt.imshow(rgb)\n", 85 | " plt.show()\n", 86 | " else:\n", 87 | " # plot using opencv\n", 88 | " sparse_plot = cv2.convertScaleAbs(sparse_depth*255./4.5)\n", 89 | " gt_plot = cv2.convertScaleAbs(depth_gt*255./4.5)\n", 90 | " cv2.imshow(\"sparse_depth\", sparse_plot)\n", 91 | " cv2.imshow(\"gt_depth\", gt_plot)\n", 92 | " cv2.imshow(\"rgb\", rgb_gt)\n", 93 | " cv2.waitKey(0)\n", 94 | "\n", 95 | " \n", 96 | " if(debug):\n", 97 | " print(\"min depth_gt: \", np.nanmin(depth_gt))\n", 98 | " print(\"min sparse_idepth: \", np.nanmin(sparse_idepth))\n", 99 | " print(\"min sparse_depth: \", np.nanmin(sparse_depth))\n", 100 | " print(\"min sparse_idepthVar: \", np.nanmin(sparse_idepthVar))" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "dataset_folder = \"/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/rgbd_dataset_freiburg1_teddy/\"\n", 110 | "basename = \"0_1305032222.008934_171_\"\n", 111 | "check_validity(dataset_folder, basename)" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "import cv2\n", 121 | "import numpy as np\n", 122 | "import matplotlib.pyplot as plt\n", 123 | "import matplotlib\n", 124 | "matplotlib.rcsetup.interactive_bk" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "plt.switch_backend('nbAgg')" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "dataset_folder = \"/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/rgbd_dataset_freiburg1_teddy/\"\n", 143 | "basename = \"0_1305032227.075600_186_\"\n", 144 | "depth_gt_png = dataset_folder + basename + \"depthGT.png\"\n", 145 | "rgb_png = dataset_folder + basename + \"rgb.png\"\n", 146 | "sparse_depth_bin = dataset_folder + basename + \"sparse_depth.bin\"\n", 147 | "sparse_variance_bin = dataset_folder + basename + \"sparse_depthVar.bin\"" 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "depth_gt = cv2.imread(depth_gt_png, -1).astype(np.float32)/5000\n", 157 | "#depth_gt = cv2.imread(depth_gt_png, -1)\n", 158 | "rgb_gt = cv2.imread(rgb_png, -1)\n", 159 | "idepth_gt = 1./depth_gt\n" 160 | ] 161 | }, 162 | { 163 | "cell_type": "code", 164 | "execution_count": null, 165 | "metadata": {}, 166 | "outputs": [], 167 | "source": [ 168 | "print(depth_gt.shape)\n", 169 | "print(np.nanmax(depth_gt))\n", 170 | "print(np.nanmin(depth_gt))\n", 171 | "print(depth_gt.dtype)" 172 | ] 173 | }, 174 | { 175 | "cell_type": "code", 176 | "execution_count": null, 177 | "metadata": {}, 178 | "outputs": [], 179 | "source": [ 180 | "print(idepth_gt.shape)\n", 181 | "print(np.nanmax(idepth_gt))\n", 182 | "print(np.nanmin(idepth_gt))\n", 183 | "print(idepth_gt.dtype)" 184 | ] 185 | }, 186 | { 187 | "cell_type": "code", 188 | "execution_count": null, 189 | "metadata": {}, 190 | "outputs": [], 191 | "source": [ 192 | "sparse_idepth = np.fromfile(sparse_depth_bin, dtype=np.float16)\n", 193 | "print(np.nanmax(sparse_idepth))\n", 194 | "print(np.nanmin(sparse_idepth))" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "metadata": {}, 201 | "outputs": [], 202 | "source": [ 203 | "sparse_idepth = sparse_idepth.astype(np.float32)\n", 204 | "sparse_idepth = sparse_idepth.reshape((480, 640))\n", 205 | "print(np.nanmax(sparse_idepth))\n", 206 | "print(np.nanmin(sparse_idepth))\n", 207 | "print(sparse_idepth.shape)\n", 208 | "print(sparse_idepth.dtype)" 209 | ] 210 | }, 211 | { 212 | "cell_type": "code", 213 | "execution_count": null, 214 | "metadata": {}, 215 | "outputs": [], 216 | "source": [ 217 | "sparse_depth = 1./sparse_idepth\n", 218 | "print(np.nanmax(sparse_depth))\n", 219 | "print(np.nanmin(sparse_depth))\n", 220 | "print(sparse_depth.shape)\n", 221 | "print(sparse_depth.dtype)" 222 | ] 223 | }, 224 | { 225 | "cell_type": "code", 226 | "execution_count": null, 227 | "metadata": {}, 228 | "outputs": [], 229 | "source": [ 230 | "sparse_idepthVar = np.fromfile(sparse_variance_bin, dtype=np.float16)\n", 231 | "print(np.nanmax(sparse_idepthVar))\n", 232 | "print(np.nanmin(sparse_idepthVar))\n", 233 | "sparse_idepthVar = sparse_idepthVar.astype(np.float32)\n", 234 | "sparse_idepthVar = sparse_idepthVar.reshape((480, 640))\n", 235 | "print(np.nanmax(sparse_idepthVar))\n", 236 | "print(np.nanmin(sparse_idepthVar))\n", 237 | "print(sparse_idepthVar.shape)\n", 238 | "print(sparse_idepthVar.dtype)" 239 | ] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": null, 244 | "metadata": {}, 245 | "outputs": [], 246 | "source": [ 247 | "sparse_plot = cv2.convertScaleAbs(sparse_depth*255./4.5)\n", 248 | "gt_plot = cv2.convertScaleAbs(depth_gt*255./4.5)\n", 249 | "# plot using matplotlib\n", 250 | "plt.figure()\n", 251 | "plt.imshow(sparse_depth, cmap='hot')\n", 252 | "plt.clim(0,4)\n", 253 | "plt.colorbar()\n", 254 | "plt.figure()\n", 255 | "plt.imshow(depth_gt, cmap='hot')\n", 256 | "plt.clim(0,4)\n", 257 | "plt.colorbar()\n", 258 | "plt.show()\n", 259 | "\"\"\"\n", 260 | "# plot using opencv\n", 261 | "cv2.imshow(\"sparse_depth\", sparse_plot)\n", 262 | "cv2.imshow(\"gt_depth\", gt_plot)\n", 263 | "cv2.imshow(\"rgb\", rgb_gt)\n", 264 | "cv2.waitKey(0)\n", 265 | "\"\"\"" 266 | ] 267 | }, 268 | { 269 | "cell_type": "code", 270 | "execution_count": null, 271 | "metadata": {}, 272 | "outputs": [], 273 | "source": [ 274 | "fig,ax = plt.subplots()\n", 275 | "cax = plt.imshow(sparse_idepthVar, cmap='hot')\n", 276 | "cbar = fig.colorbar(cax)\n", 277 | "plt.show()" 278 | ] 279 | }, 280 | { 281 | "cell_type": "code", 282 | "execution_count": null, 283 | "metadata": {}, 284 | "outputs": [], 285 | "source": [ 286 | "original_depth = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/depth/1305032182.042220.png\"\n", 287 | "depth_org = cv2.imread(original_depth, -1)\n", 288 | "print(depth_org.max())\n", 289 | "print(depth_org.min())\n", 290 | "depth_org.dtype" 291 | ] 292 | } 293 | ], 294 | "metadata": { 295 | "kernelspec": { 296 | "display_name": "Python 3", 297 | "language": "python", 298 | "name": "python3" 299 | }, 300 | "language_info": { 301 | "codemirror_mode": { 302 | "name": "ipython", 303 | "version": 3 304 | }, 305 | "file_extension": ".py", 306 | "mimetype": "text/x-python", 307 | "name": "python", 308 | "nbconvert_exporter": "python", 309 | "pygments_lexer": "ipython3", 310 | "version": "3.5.2" 311 | } 312 | }, 313 | "nbformat": 4, 314 | "nbformat_minor": 2 315 | } 316 | -------------------------------------------------------------------------------- /notebooks/.ipynb_checkpoints/LSDDepth dataset reader-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Checking validity of dataset" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": [ 16 | "def check_validity(dataset_folder, basename, matplotlib=True, debug=False):\n", 17 | " import cv2\n", 18 | " import numpy as np\n", 19 | " import matplotlib.pyplot as plt\n", 20 | " import matplotlib\n", 21 | " #Inside notebook uncomment the following\n", 22 | " matplotlib.rcsetup.interactive_bk\n", 23 | " plt.switch_backend('nbAgg')\n", 24 | " \n", 25 | " # Setting paths of the keyframe batch\n", 26 | " depth_gt_png = dataset_folder + basename + \"depthGT.png\"\n", 27 | " rgb_png = dataset_folder + basename + \"rgb.png\"\n", 28 | " sparse_depth_bin = dataset_folder + basename + \"sparse_depth.bin\"\n", 29 | " sparse_variance_bin = dataset_folder + basename + \"sparse_depthVar.bin\"\n", 30 | " \n", 31 | " # opening depth_gt and converting it to float\n", 32 | " print(\"GT depth---\")\n", 33 | " depth_gt = cv2.imread(depth_gt_png, -1).astype(np.float32)/5000\n", 34 | " rgb = cv2.imread(rgb_png, -1)\n", 35 | " print(\"shape: \", depth_gt.shape)\n", 36 | " print(\"max: \", np.nanmax(depth_gt))\n", 37 | " print(\"dtype: \", depth_gt.dtype)\n", 38 | " \n", 39 | " # loading sparse idepth from bin (as half-float) and converting it to float32\n", 40 | " print(\"Sparse idepth ---\")\n", 41 | " sparse_idepth = np.fromfile(sparse_depth_bin, dtype=np.float16)\n", 42 | " if(debug):\n", 43 | " print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth))\n", 44 | " print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth))\n", 45 | " sparse_idepth = sparse_idepth.astype(np.float32)\n", 46 | " sparse_idepth = sparse_idepth.reshape((480, 640))\n", 47 | " print(\"max: \", np.nanmax(sparse_idepth))\n", 48 | " print(\"shape: \",sparse_idepth.shape)\n", 49 | " print(\"dtype: \", sparse_idepth.dtype)\n", 50 | " \n", 51 | " # converting sparse idepth to depth\n", 52 | " print(\"Sparse depth ---\")\n", 53 | " sparse_depth = 1./sparse_idepth\n", 54 | " print(\"max: \", np.nanmax(sparse_depth))\n", 55 | " print(\"shape: \", sparse_depth.shape)\n", 56 | " print(\"dtype: \", sparse_depth.dtype)\n", 57 | " \n", 58 | " # loading sparse idepthVar from bin (as half-float) and converting it to float32\n", 59 | " print(\"Sparse idepthVar ---\")\n", 60 | " sparse_idepthVar = np.fromfile(sparse_variance_bin, dtype=np.float16)\n", 61 | " if(debug):\n", 62 | " print(\"min half-float sparse_idepthVar: \", np.nanmin(sparse_idepthVar))\n", 63 | " print(\"max half-float sparse_idepthVar: \", np.nanmax(sparse_idepthVar))\n", 64 | " sparse_idepthVar = sparse_idepthVar.astype(np.float32)\n", 65 | " sparse_idepthVar = sparse_idepthVar.reshape((480, 640))\n", 66 | " print(\"max: \", np.nanmax(sparse_idepthVar))\n", 67 | " print(\"shape: \", sparse_idepthVar.shape)\n", 68 | " print(\"dtype: \", sparse_idepthVar.dtype)\n", 69 | " \n", 70 | " # plotting images\n", 71 | " if matplotlib:\n", 72 | " # plot using matplotlib\n", 73 | " plt.figure(\"sparse_depth\")\n", 74 | " plt.imshow(sparse_depth, cmap='hot')\n", 75 | " plt.clim(0,4)\n", 76 | " plt.colorbar()\n", 77 | " plt.figure(\"gt_depth\")\n", 78 | " plt.imshow(depth_gt, cmap='hot')\n", 79 | " plt.clim(0,4)\n", 80 | " plt.colorbar()\n", 81 | " plt.figure(\"sparse_depth_variance\")\n", 82 | " plt.imshow(sparse_idepthVar, cmap='hot')\n", 83 | " plt.figure(\"rgb\")\n", 84 | " plt.imshow(rgb)\n", 85 | " plt.show()\n", 86 | " else:\n", 87 | " # plot using opencv\n", 88 | " sparse_plot = cv2.convertScaleAbs(sparse_depth*255./4.5)\n", 89 | " gt_plot = cv2.convertScaleAbs(depth_gt*255./4.5)\n", 90 | " cv2.imshow(\"sparse_depth\", sparse_plot)\n", 91 | " cv2.imshow(\"gt_depth\", gt_plot)\n", 92 | " cv2.imshow(\"rgb\", rgb_gt)\n", 93 | " cv2.waitKey(0)\n", 94 | "\n", 95 | " \n", 96 | " if(debug):\n", 97 | " print(\"min depth_gt: \", np.nanmin(depth_gt))\n", 98 | " print(\"min sparse_idepth: \", np.nanmin(sparse_idepth))\n", 99 | " print(\"min sparse_depth: \", np.nanmin(sparse_depth))\n", 100 | " print(\"min sparse_idepthVar: \", np.nanmin(sparse_idepthVar))" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "dataset_folder = \"/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/rgbd_dataset_freiburg1_teddy/\"\n", 110 | "basename = \"0_1305032222.008934_171_\"\n", 111 | "check_validity(dataset_folder, basename)" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "import cv2\n", 121 | "import numpy as np\n", 122 | "import matplotlib.pyplot as plt\n", 123 | "import matplotlib\n", 124 | "matplotlib.rcsetup.interactive_bk" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "plt.switch_backend('nbAgg')" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "dataset_folder = \"/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/rgbd_dataset_freiburg1_teddy/\"\n", 143 | "basename = \"0_1305032227.075600_186_\"\n", 144 | "depth_gt_png = dataset_folder + basename + \"depthGT.png\"\n", 145 | "rgb_png = dataset_folder + basename + \"rgb.png\"\n", 146 | "sparse_depth_bin = dataset_folder + basename + \"sparse_depth.bin\"\n", 147 | "sparse_variance_bin = dataset_folder + basename + \"sparse_depthVar.bin\"" 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "depth_gt = cv2.imread(depth_gt_png, -1).astype(np.float32)/5000\n", 157 | "#depth_gt = cv2.imread(depth_gt_png, -1)\n", 158 | "rgb_gt = cv2.imread(rgb_png, -1)\n", 159 | "idepth_gt = 1./depth_gt\n" 160 | ] 161 | }, 162 | { 163 | "cell_type": "code", 164 | "execution_count": null, 165 | "metadata": {}, 166 | "outputs": [], 167 | "source": [ 168 | "print(depth_gt.shape)\n", 169 | "print(np.nanmax(depth_gt))\n", 170 | "print(np.nanmin(depth_gt))\n", 171 | "print(depth_gt.dtype)" 172 | ] 173 | }, 174 | { 175 | "cell_type": "code", 176 | "execution_count": null, 177 | "metadata": {}, 178 | "outputs": [], 179 | "source": [ 180 | "print(idepth_gt.shape)\n", 181 | "print(np.nanmax(idepth_gt))\n", 182 | "print(np.nanmin(idepth_gt))\n", 183 | "print(idepth_gt.dtype)" 184 | ] 185 | }, 186 | { 187 | "cell_type": "code", 188 | "execution_count": null, 189 | "metadata": {}, 190 | "outputs": [], 191 | "source": [ 192 | "sparse_idepth = np.fromfile(sparse_depth_bin, dtype=np.float16)\n", 193 | "print(np.nanmax(sparse_idepth))\n", 194 | "print(np.nanmin(sparse_idepth))" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "metadata": {}, 201 | "outputs": [], 202 | "source": [ 203 | "sparse_idepth = sparse_idepth.astype(np.float32)\n", 204 | "sparse_idepth = sparse_idepth.reshape((480, 640))\n", 205 | "print(np.nanmax(sparse_idepth))\n", 206 | "print(np.nanmin(sparse_idepth))\n", 207 | "print(sparse_idepth.shape)\n", 208 | "print(sparse_idepth.dtype)" 209 | ] 210 | }, 211 | { 212 | "cell_type": "code", 213 | "execution_count": null, 214 | "metadata": {}, 215 | "outputs": [], 216 | "source": [ 217 | "sparse_depth = 1./sparse_idepth\n", 218 | "print(np.nanmax(sparse_depth))\n", 219 | "print(np.nanmin(sparse_depth))\n", 220 | "print(sparse_depth.shape)\n", 221 | "print(sparse_depth.dtype)" 222 | ] 223 | }, 224 | { 225 | "cell_type": "code", 226 | "execution_count": null, 227 | "metadata": {}, 228 | "outputs": [], 229 | "source": [ 230 | "sparse_idepthVar = np.fromfile(sparse_variance_bin, dtype=np.float16)\n", 231 | "print(np.nanmax(sparse_idepthVar))\n", 232 | "print(np.nanmin(sparse_idepthVar))\n", 233 | "sparse_idepthVar = sparse_idepthVar.astype(np.float32)\n", 234 | "sparse_idepthVar = sparse_idepthVar.reshape((480, 640))\n", 235 | "print(np.nanmax(sparse_idepthVar))\n", 236 | "print(np.nanmin(sparse_idepthVar))\n", 237 | "print(sparse_idepthVar.shape)\n", 238 | "print(sparse_idepthVar.dtype)" 239 | ] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": null, 244 | "metadata": {}, 245 | "outputs": [], 246 | "source": [ 247 | "sparse_plot = cv2.convertScaleAbs(sparse_depth*255./4.5)\n", 248 | "gt_plot = cv2.convertScaleAbs(depth_gt*255./4.5)\n", 249 | "# plot using matplotlib\n", 250 | "plt.figure()\n", 251 | "plt.imshow(sparse_depth, cmap='hot')\n", 252 | "plt.clim(0,4)\n", 253 | "plt.colorbar()\n", 254 | "plt.figure()\n", 255 | "plt.imshow(depth_gt, cmap='hot')\n", 256 | "plt.clim(0,4)\n", 257 | "plt.colorbar()\n", 258 | "plt.show()\n", 259 | "\"\"\"\n", 260 | "# plot using opencv\n", 261 | "cv2.imshow(\"sparse_depth\", sparse_plot)\n", 262 | "cv2.imshow(\"gt_depth\", gt_plot)\n", 263 | "cv2.imshow(\"rgb\", rgb_gt)\n", 264 | "cv2.waitKey(0)\n", 265 | "\"\"\"" 266 | ] 267 | }, 268 | { 269 | "cell_type": "code", 270 | "execution_count": null, 271 | "metadata": {}, 272 | "outputs": [], 273 | "source": [ 274 | "fig,ax = plt.subplots()\n", 275 | "cax = plt.imshow(sparse_idepthVar, cmap='hot')\n", 276 | "cbar = fig.colorbar(cax)\n", 277 | "plt.show()" 278 | ] 279 | }, 280 | { 281 | "cell_type": "code", 282 | "execution_count": null, 283 | "metadata": {}, 284 | "outputs": [], 285 | "source": [ 286 | "original_depth = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/depth/1305032182.042220.png\"\n", 287 | "depth_org = cv2.imread(original_depth, -1)\n", 288 | "print(depth_org.max())\n", 289 | "print(depth_org.min())\n", 290 | "depth_org.dtype" 291 | ] 292 | } 293 | ], 294 | "metadata": { 295 | "kernelspec": { 296 | "display_name": "Python 3", 297 | "language": "python", 298 | "name": "python3" 299 | }, 300 | "language_info": { 301 | "codemirror_mode": { 302 | "name": "ipython", 303 | "version": 3 304 | }, 305 | "file_extension": ".py", 306 | "mimetype": "text/x-python", 307 | "name": "python", 308 | "nbconvert_exporter": "python", 309 | "pygments_lexer": "ipython3", 310 | "version": "3.5.2" 311 | } 312 | }, 313 | "nbformat": 4, 314 | "nbformat_minor": 2 315 | } 316 | -------------------------------------------------------------------------------- /networks/depth_fusion/trainer_rgbd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from __future__ import absolute_import 3 | from __future__ import print_function 4 | import argparse 5 | import numpy as np 6 | import tensorflow as tf 7 | 8 | from tensorflow.python import debug as tf_debug 9 | 10 | import sys 11 | import os 12 | sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion') 13 | from net.my_networks import * 14 | from net.my_losses import * 15 | from net.my_models import * 16 | 17 | feature_names = [ 18 | 'rgb', 19 | 'sparseInverseDepth', 20 | 'sparseInverseDepthVariance' 21 | ] 22 | 23 | FLAGS = { 24 | 'multi_gpu': False, 25 | 'batch_size':12, 26 | 'prefetch_buffer_size': 12, 27 | 'num_parallel_calls': 12, 28 | 'num_epochs':50, 29 | 'learning_rate':0.0004, 30 | 'data_format': "channels_first", 31 | 'epochs_bw_eval':1 32 | } 33 | 34 | augment_props = { 35 | 'max_contrast_factor': 1.1, 36 | 'max_brightness_factor': 0.05, 37 | 'max_scale_factor': 1.2 38 | } 39 | 40 | def parse_n_load(filename_line, basepath, data_format=FLAGS['data_format'], augment=True, width=320, height=240): 41 | filenames = tf.decode_csv(filename_line, [[''], [''], [''], ['']]) 42 | images = [] 43 | for cnt in range(4): 44 | image_string = tf.read_file(basepath +'/'+ filenames[cnt]) 45 | if cnt < 2: 46 | if cnt == 0: 47 | image_decoded = tf.image.decode_png(image_string, dtype=tf.uint16) 48 | image_decoded = tf.reshape(image_decoded, [480, 640, 1]) 49 | image_decoded = tf.cast(image_decoded, tf.float32) 50 | image_decoded = tf.scalar_mul(0.0002, image_decoded) 51 | # converting depth to idepth but keeping NaN's 52 | image_decoded = invert_finite_depth(image_decoded) 53 | else: 54 | image_decoded = tf.image.decode_png(image_string) 55 | image_decoded = tf.reshape(image_decoded, [480, 640, 3]) 56 | image_decoded = tf.cast(image_decoded, tf.float32) 57 | # converting rgb to [0,1] from [0,255] 58 | image_decoded = tf.divide(image_decoded, 255) 59 | else: 60 | image_decoded = tf.decode_raw(image_string, tf.half) 61 | image_decoded = tf.reshape(image_decoded, [480, 640, 1]) 62 | image_decoded = tf.cast(image_decoded, tf.float32) 63 | image_decoded = replace_nonfinite(image_decoded) 64 | 65 | image_decoded = tf.image.resize_images(image_decoded, [height, width], 66 | align_corners=True, method=tf.image.ResizeMethod.NEAREST_NEIGHBOR) 67 | if data_format == 'channels_first': 68 | image_decoded = tf.transpose(image_decoded,[2,0,1]) 69 | images.append(image_decoded) 70 | idepth_gt_clean = images[0] 71 | del images[0] 72 | if augment: 73 | images, idepth_gt_clean = random_augmentations(images, idepth_gt_clean) 74 | d = dict(zip(feature_names, images)), idepth_gt_clean 75 | return d 76 | 77 | def random_augmentations(images, idepth_gt): 78 | rand_nums = np.random.rand(2) 79 | rand_bools = rand_nums < 0.3 80 | rand_scale = np.random.uniform(1./augment_props['max_scale_factor'], 81 | augment_props['max_scale_factor']) 82 | # rand_bools = np.random.choice([True, False], 2) 83 | 84 | # brightness and contrast transform only for rgb 85 | images[0] = tf.image.random_brightness(images[0], max_delta=augment_props['max_brightness_factor']) 86 | images[0] = tf.image.random_contrast(images[0], upper=augment_props['max_contrast_factor'], 87 | lower=1./augment_props['max_contrast_factor']) 88 | 89 | # random scale operation for sparse depth inputs 90 | for ii in [1,2]: 91 | images[ii] = tf.scalar_mul(rand_scale, images[ii]) 92 | 93 | # input augmentations (for all inputs) 94 | for ii in range(len(images)): 95 | # common augmentations 96 | if rand_bools[0]: 97 | images[ii] = tf.image.flip_left_right(images[ii]) 98 | if rand_bools[1]: 99 | images[ii] = tf.image.flip_up_down(images[ii]) 100 | 101 | # modifiying gt for mirroring 102 | if rand_bools[0]: 103 | idepth_gt = tf.image.flip_left_right(idepth_gt) 104 | if rand_bools[1]: 105 | idepth_gt = tf.image.flip_up_down(idepth_gt) 106 | return(images, idepth_gt) 107 | 108 | 109 | def dataset_shuffler(training_file): 110 | filename_records = tf.data.TextLineDataset(training_file) 111 | #dataset = filename_records.shuffle(buffer_size=50000) 112 | dataset = filename_records.apply(tf.contrib.data.shuffle_and_repeat(50000, FLAGS['num_epochs'])) 113 | return dataset 114 | 115 | def input_fn(dataset, basename, augment=True, batch_size=FLAGS['batch_size']): 116 | #dataset = dataset.apply(tf.contrib.data.parallel_interleave(parse_n_extract_images, 117 | # cycle_length=FLAGS['num_parallel_calls'], sloppy=True)) 118 | #dataset = dataset.map(map_func=parse_n_load, num_parallel_calls=FLAGS['num_parallel_calls']) 119 | #dataset = dataset.batch(batch_size=FLAGS['batch_size']) 120 | dataset = dataset.apply(tf.contrib.data.map_and_batch(map_func=lambda filename:parse_n_load(filename, basename, augment=True), 121 | num_parallel_batches=2, batch_size=batch_size)) 122 | 123 | #dataset = dataset.shuffle(buffer_size=10000) 124 | 125 | dataset = dataset.prefetch(buffer_size=FLAGS['prefetch_buffer_size']) 126 | #iterator = dataset.make_one_shot_iterator() 127 | #features, label = iterator.get_next() 128 | #return features, label 129 | return dataset 130 | 131 | def train_input_fn(): 132 | training_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/train/rgbd_lsdDepth_train.txt" 133 | dataset = dataset_shuffler(training_file) 134 | basename = os.path.dirname(training_file) 135 | dataset = input_fn(dataset, basename, augment=True) 136 | return dataset 137 | 138 | def test_input_fn(): 139 | test_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/test/rgbd_lsdDepth_test.txt" 140 | dataset = tf.data.TextLineDataset(test_file) 141 | basename = os.path.dirname(test_file) 142 | dataset = dataset.map(map_func=lambda filename:parse_n_load(filename, basename, augment=False), 143 | num_parallel_calls=FLAGS['num_parallel_calls']) 144 | dataset = dataset.apply(tf.contrib.data.batch_and_drop_remainder(4)) 145 | #dataset = input_fn(dataset, basename, augment=False, batch_size=4) 146 | return dataset 147 | 148 | def validate_batch_size_for_multi_gpu(batch_size): 149 | """For multi-gpu, batch-size must be a multiple of the number of GPUs. 150 | Note that this should eventually be handled by replicate_model_fn 151 | directly. Multi-GPU support is currently experimental, however, 152 | so doing the work here until that feature is in place. 153 | Args: 154 | batch_size: the number of examples processed in each training batch. 155 | Raises: 156 | ValueError: if no GPUs are found, or selected batch_size is invalid. 157 | """ 158 | from tensorflow.python.client import device_lib # pylint: disable=g-import-not-at-top 159 | 160 | local_device_protos = device_lib.list_local_devices() 161 | num_gpus = sum([1 for d in local_device_protos if d.device_type == 'GPU']) 162 | if not num_gpus: 163 | raise ValueError('Multi-GPU mode was specified, but no GPUs ' 164 | 'were found. To use CPU, run without --multi_gpu.') 165 | 166 | remainder = batch_size % num_gpus 167 | if remainder: 168 | err = ('When running with multiple GPUs, batch size ' 169 | 'must be a multiple of the number of available GPUs. ' 170 | 'Found {} GPUs with a batch size of {}; try --batch_size={} instead.' 171 | ).format(num_gpus, batch_size, batch_size - remainder) 172 | raise ValueError(err) 173 | 174 | 175 | 176 | def run_trainer(model_fn, model_dir_name, training_steps, config): 177 | """Run training and eval loop for lsd depth fusion network. 178 | """ 179 | model_function = model_fn 180 | hooks = [tf_debug.LocalCLIDebugHook()] 181 | 182 | if FLAGS['multi_gpu']: 183 | validate_batch_size_for_multi_gpu(FLAGS['batch_size']) 184 | 185 | # There are two steps required if using multi-GPU: (1) wrap the model_fn, 186 | # and (2) wrap the optimizer. The first happens here, and (2) happens 187 | # in the model_fn itself when the optimizer is defined. 188 | model_function = tf.contrib.estimator.replicate_model_fn( 189 | model_fn, loss_reduction=tf.losses.Reduction.MEAN) 190 | 191 | model_base_dir = "/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training" 192 | model_dir = os.path.join(model_base_dir, model_dir_name) 193 | 194 | run_config = tf.estimator.RunConfig(session_config=config) 195 | 196 | lsd_depth_fuser = tf.estimator.Estimator( 197 | model_fn=model_function, 198 | model_dir=model_dir, 199 | params={ 200 | 'data_format': FLAGS['data_format'], 201 | 'multi_gpu': FLAGS['multi_gpu'] 202 | }, 203 | config=run_config) 204 | 205 | # Train and evaluate model. 206 | for _ in range(FLAGS['num_epochs'] // FLAGS['epochs_bw_eval']): 207 | lsd_depth_fuser.train(input_fn=train_input_fn, steps=training_steps) 208 | eval_results = lsd_depth_fuser.evaluate(input_fn=test_input_fn) 209 | print('\nEvaluation results:\n\t%s\n' % eval_results) 210 | 211 | 212 | #if model_helpers.past_stop_threshold(flags_obj.stop_threshold, 213 | # eval_results['loss']): 214 | #break 215 | # Export the model for later use 216 | rgb = tf.placeholder(tf.float32, [1, 3, 240, 320]) 217 | idepth = tf.placeholder(tf.float32, [1, 1, 240, 320]) 218 | idepthVar = tf.placeholder(tf.float32, [1, 1, 240, 320]) 219 | export_input_fn = tf.estimator.export.build_raw_serving_input_receiver_fn({ 220 | 'rgb': rgb, 221 | 'sparseInverseDepth': idepth, 222 | 'sparseInverseDepthVariance': idepthVar 223 | }) 224 | lsd_depth_fuser.export_savedmodel(os.path.join(model_dir, "exported_model"), 225 | export_input_fn) 226 | 227 | def dataset_len(dataset_loc): 228 | with open(dataset_loc) as f: 229 | for i, l in enumerate(f): 230 | pass 231 | return i + 1 232 | 233 | 234 | def main(): 235 | parser = argparse.ArgumentParser(description=( "Train LSDDepth fusion network using the custom estimator.")) 236 | parser.add_argument("--model_name", type=str, required=True, help="Specify name of model (will be used as ouput dir_name)") 237 | parser.add_argument("--num_epochs", type=int, required=True, help="Number of training epochs") 238 | parser.add_argument("--limit_gpu", action='store_true', help="run program with limited GPU (dacky)") 239 | args = parser.parse_args() 240 | 241 | training_file = "/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/train/rgbd_lsdDepth_train.txt" 242 | data_len = dataset_len(training_file) 243 | steps_per_epoch = data_len // FLAGS['batch_size'] 244 | #num_steps = steps_per_epoch * args.num_epochs 245 | num_steps = steps_per_epoch * FLAGS['epochs_bw_eval'] 246 | 247 | FLAGS['num_epochs'] = args.num_epochs 248 | 249 | print("Number of training steps (until eval):", num_steps) 250 | #### 251 | model_fn = model_fn_NetV02Res_LossL1SigL1ExpResL1 252 | #model_fn = model_fn_NetV04_LossL1SigL1 253 | #model_fn = model_fn_NetV04Res_LossL1SigL1ExpResL1 254 | #model_fn = model_fn_NetV03Res_LossL1SigL1ExpResL1 255 | #model_fn = model_fn_Netv3_LossL1SigL1 256 | #model_fn = model_fn_Netv2_LossL1SigL1_down 257 | #### 258 | 259 | if args.limit_gpu: 260 | config = tf.ConfigProto() 261 | config.gpu_options.per_process_gpu_memory_fraction = 0.25 262 | FLAGS['multi_gpu'] = False 263 | else: 264 | config = None 265 | 266 | run_trainer(model_fn, args.model_name, num_steps, config) 267 | 268 | if __name__ == '__main__': 269 | tf.logging.set_verbosity(tf.logging.INFO) 270 | main() 271 | sys.exit() 272 | -------------------------------------------------------------------------------- /bin/slam_system_tester.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from enum import Enum 3 | from collections import namedtuple 4 | import os 5 | import sys 6 | import subprocess 7 | import time 8 | import argparse 9 | import re 10 | import rostopic 11 | import shutil 12 | 13 | FLAGS = { 14 | "first_run":True, 15 | "deepTAM_tracker_ON":False, 16 | "single_image_predictor_ON": False, 17 | "depth_completion_net_ON":False 18 | } 19 | 20 | run_config = namedtuple("run_config", ["doSlam", "gtBootstrap", "useGtDepth", "predictDepth", 21 | "depthCompletion", "readSparse"]) 22 | 23 | class mode(Enum): 24 | #LSDSLAM = run_config(True, True, False, False, False, False) 25 | DEEPTAM = run_config(False, True, True, False, False ,False) 26 | DEEPTAMSLAM = run_config(True, True, True, False, False ,False) 27 | CNNSLAM_GT = run_config(True, True, True, True, False ,False) 28 | CNNSLAM_GTINIT = run_config(True, True, False, True, False ,False) 29 | CNNSLAM = run_config(True, False, False, True, False ,False) 30 | SPARSEDEPTHCOMPSLAM = run_config(True, False, False, False, True ,True) 31 | DENSEDEPTHCOMPSLAM = run_config(True, False, False, False, True ,False) 32 | SPARSEDEPTHCOMPSLAM_GTINIT = run_config(True, True, False, False, True ,True) 33 | DENSEDEPTHCOMPSLAM_GTINIT = run_config(True, True, False, False, True ,False) 34 | 35 | class calib(Enum): 36 | Freiburg1 = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg" 37 | Freiburg2 = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/tum_rgbd_fr2_calib.cfg" 38 | Freiburg3 = "/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/tum_rgbd_fr3_calib.cfg" 39 | 40 | bool_str_maker = lambda x: "true" if x else "false" 41 | 42 | def get_calib_file(seq_name): 43 | found_calib = False 44 | for c in calib: 45 | if re.search(c.name, seq_name, re.IGNORECASE): 46 | found_calib = True 47 | print("Found calib file, returning!!") 48 | return c.value 49 | print("Cannot find calib file, returning default calib") 50 | return calib.Freiburg3.value 51 | 52 | 53 | def walkdir(folder, level): 54 | assert(os.path.isdir(folder)) 55 | num_sep = folder.count(os.path.sep) 56 | for root, dirs, files in os.walk(folder): 57 | yield root, dirs, files 58 | num_sep_this = root.count(os.path.sep) 59 | if num_sep + level <= num_sep_this: 60 | del dirs[:] 61 | return 62 | 63 | 64 | def run_SLAM(sequence, config, debug=False, use_siasa=False): 65 | if FLAGS["first_run"]: 66 | ## run the roscore if needed ## 67 | try: 68 | rostopic.get_topic_class('/rosout') 69 | roscore_running = True 70 | except rostopic.ROSTopicIOException as e: 71 | roscore_running = False 72 | if not roscore_running: 73 | command = "roscore" 74 | popen1 = subprocess.Popen(command, shell=True) 75 | 76 | if use_siasa: 77 | ## run siasa_viewer node 78 | command = "rosrun reinforced_visual_slam siasa_viewer.py" 79 | popen5 = subprocess.Popen(command, shell=True) 80 | 81 | FLAGS["first_run"] = False 82 | 83 | if not FLAGS["single_image_predictor_ON"]: 84 | if config.value.predictDepth and config.value.depthCompletion: 85 | ## run depthcompletion node 86 | command = "rosrun reinforced_visual_slam single_image_depth_predictor.py" 87 | popen3 = subprocess.Popen(command, shell=True) 88 | FLAGS["single_image_predictor_ON"] = True 89 | 90 | if not FLAGS["depth_completion_net_ON"]: 91 | if config.value.predictDepth and (not config.value.depthCompletion or not config.value.gtBootstrap): 92 | ## run singleimage depth prediction node 93 | command = "rosrun reinforced_visual_slam depthmap_fuser.py" 94 | popen4 = subprocess.Popen(command, shell=True) 95 | FLAGS["depth_completion_net_ON"] = True 96 | 97 | if not FLAGS["deepTAM_tracker_ON"]: 98 | ## run the required nodes depending on the configuration under test ## 99 | if config.name != "LSDSLAM": 100 | ## run DeepTAM tracker node 101 | command = "rosrun reinforced_visual_slam deepTAM_tracker.py" 102 | popen2 = subprocess.Popen(command, shell=True) 103 | FLAGS["deepTAM_tracker_ON"] = True 104 | #input("Wait until the DeepTAM is loaded and then Press Enter...") 105 | 106 | if config.name == "LSDSLAM": 107 | KFDistWeight = "5" 108 | KFUsageWeight = "3" 109 | else: 110 | ## These variables are used differently in LSD SLAM 111 | KFDistWeight = "0.15" 112 | KFUsageWeight = "5" 113 | 114 | ## set defaults for the SLAM system ## 115 | run_command = "rosrun lsd_slam_core deepTAM_dataset_slam" 116 | hz = "0" 117 | minUseGrad = "3" 118 | testMode = "true" 119 | writeTestDepths = "true" 120 | doKFReActivation = "true" 121 | showDebugWindow = "false" 122 | 123 | 124 | if debug: 125 | print("sequence: ", sequence) 126 | print("config: ", config.name) 127 | #input("Press enter to confirm ...") 128 | 129 | # Finding dataset base location 130 | base = "/misc/scratchSSD/ummenhof/rgbd_tum" 131 | seq_name = os.path.dirname(sequence).split('/')[-1] 132 | basename = os.path.join(base, seq_name) 133 | 134 | # Finding result folder 135 | result_base_dir = "/misc/lmbraid19/thomasa/results/tum_rgbd" 136 | seq_base_dir = os.path.join(result_base_dir, seq_name) 137 | output_folder = os.path.join(seq_base_dir, config.name) 138 | if os.path.isdir(output_folder): 139 | shutil.rmtree(output_folder) 140 | os.makedirs(output_folder, exist_ok=True) 141 | 142 | calib_file = get_calib_file(seq_name) 143 | 144 | # remapping nodename 145 | node_name = config.name + "_" + seq_name 146 | node_name_remap = " __name:=" + node_name 147 | 148 | command = (run_command + node_name_remap + " _calib:=" + calib_file +" _files:=" + sequence + " _basename:=" + basename + 149 | " _hz:=" + hz + " _minUseGrad:=" + minUseGrad + " _doKFReActivation:=" + doKFReActivation 150 | + " _testMode:=" + testMode + " _writeTestDepths:=" + writeTestDepths + " _showDebugWindow:=" + showDebugWindow 151 | + " _KFUsageWeight:=" + (KFUsageWeight) + " _KFDistWeight:=" + (KFDistWeight) 152 | + " _outputFolder:=" + output_folder 153 | + " _doSlam:=" + bool_str_maker(config.value.doSlam) 154 | + " _gtBootstrap:=" + bool_str_maker(config.value.gtBootstrap) 155 | + " _useGtDepth:=" + bool_str_maker(config.value.useGtDepth) 156 | + " _predictDepth:=" + bool_str_maker(config.value.predictDepth) 157 | + " _depthCompletion:=" + bool_str_maker(config.value.depthCompletion) 158 | + " _readSparse:=" + bool_str_maker(config.value.readSparse) 159 | ) 160 | 161 | if debug: 162 | print("command: ", command) 163 | print("seq_name: ", seq_name) 164 | print("calib_file: ", calib_file) 165 | 166 | subprocess.Popen(command, shell=True) 167 | 168 | def evaluate_result(sequence_name, config, result_folder): 169 | ## evaluate trajectory errors ## 170 | base = "/misc/scratchSSD/ummenhof/rgbd_tum" 171 | gt_file = os.path.join(base, sequence_name + "/groundtruth.txt") 172 | traj_file = os.path.join(result_folder, "final_trajectory.txt") 173 | 174 | while not os.path.isfile(traj_file): 175 | try: 176 | time.sleep(1) 177 | print("waiting for "+ config.name+ " on " + sequence_name + " to finish") 178 | except KeyboardInterrupt: 179 | skip = input('Enter s to skip this sequence:') 180 | if skip == 's': 181 | return 182 | 183 | eval_folder = os.path.join(result_folder, "eval") 184 | os.makedirs(eval_folder, exist_ok=True) 185 | ate_plot = os.path.join(eval_folder, "ate.png") 186 | rpe_plot = os.path.join(eval_folder, "rpe.png") 187 | summary_file_ate = os.path.join(eval_folder, "summary_ate.txt") 188 | result_file_ate = os.path.join(eval_folder, "result_ate.txt") 189 | summary_file_rpe = os.path.join(eval_folder, "summary_rpe.txt") 190 | result_file_rpe = os.path.join(eval_folder, "result_rpe.txt") 191 | 192 | eval_ate = ("evaluate_ate.py " + gt_file +" " + traj_file + " --plot "+ ate_plot +" --verbose" + 193 | " --save_associations " + result_file_ate + " --summary " + summary_file_ate) 194 | subprocess.Popen(eval_ate, shell=True) 195 | time.sleep(0.5) 196 | eval_rpe = ("evaluate_rpe.py " + gt_file +" " + traj_file + " --plot "+ rpe_plot + " --save " + 197 | result_file_rpe + " --summary " + summary_file_rpe +" --fixed_delta --verbose") 198 | subprocess.Popen(eval_rpe, shell=True) 199 | 200 | eval_depth = "evaluate_depth.py --result_folder " + result_folder 201 | if not config.value.predictDepth: 202 | eval_depth = eval_depth + " --no_dense_depths" 203 | subprocess.Popen(eval_depth, shell=True) 204 | 205 | def start_test(): 206 | parser = argparse.ArgumentParser(description=( "Test different SLAM system configurations on datasets.")) 207 | parser.add_argument("--sequence_name", type=str, help="Name of sequence in tum_rgbd to be tested. If not provided, test will be run on whole of tum_rgbd sequences") 208 | parser.add_argument("--config", type=str, help="Configuration of SLAM system to be tested. If not provided, all designed configurations will be tested.") 209 | parser.add_argument('--use_siasa', action='store_true', help="start siasa_viewer alongside") 210 | parser.add_argument('--only_eval', action='store_true', help="Run only evaluation, assuming that results are already made") 211 | parser.add_argument('--start_seq', type=str, help="Start eval from this sequence (make sure --sequence_name is not provided") 212 | 213 | root_seq_dir = "/misc/lmbraid19/thomasa/datasets/rgbd" 214 | sequences = [] 215 | 216 | args = parser.parse_args() 217 | assert(not(args.sequence_name and args.start_seq)) 218 | 219 | if args.sequence_name: 220 | sequence_name = args.sequence_name 221 | sequence = os.path.join(root_seq_dir, sequence_name, "rgb_depth_associated.txt") 222 | assert os.path.isfile(sequence) 223 | sequences.append(sequence) 224 | else: 225 | regex = re.compile('rgbd') 226 | for root, dirs, files in walkdir(root_seq_dir, 0): 227 | for dir in sorted(dirs): 228 | if regex.match(dir): 229 | sequences.append(os.path.join(root_seq_dir, dir, "rgb_depth_associated.txt")) 230 | assert(sequences) 231 | 232 | configs = [] 233 | found_config = False 234 | if args.config: 235 | for m in mode: 236 | if re.search(args.config, m.name, re.IGNORECASE): 237 | conf = m 238 | found_config = True 239 | print("Found configuration:", conf.value) 240 | configs.append(conf) 241 | break 242 | if not found_config: 243 | print("wrong configuration: available configurations are:") 244 | for m in mode: 245 | print(m.name) 246 | return 247 | else: 248 | for m in mode: 249 | configs.append(m) 250 | assert(configs) 251 | 252 | print("configs to test: ", configs) 253 | print("sequences to test: ", sequences) 254 | input("Press Enter to continue") 255 | 256 | start_eval = False 257 | if not args.only_eval: 258 | for sequence in sequences: 259 | for config in configs: 260 | run_SLAM(sequence=sequence, config=config, debug=True, use_siasa=args.use_siasa) 261 | seq_name = os.path.dirname(sequence).split('/')[-1] 262 | if args.start_seq and not start_eval: 263 | if args.start_seq != seq_name: 264 | break 265 | else: 266 | start_eval = True 267 | break 268 | # Finding result folder 269 | result_base_dir = "/misc/lmbraid19/thomasa/results/tum_rgbd" 270 | seq_base_dir = os.path.join(result_base_dir, seq_name) 271 | output_folder = os.path.join(seq_base_dir, config.name) 272 | evaluate_result(sequence_name=seq_name, config=config, result_folder=output_folder) 273 | time.sleep(10) 274 | 275 | return 276 | 277 | start_eval = False 278 | for sequence in sequences: 279 | for config in configs: 280 | seq_name = os.path.dirname(sequence).split('/')[-1] 281 | if args.start_seq and not start_eval: 282 | if args.start_seq != seq_name: 283 | break 284 | else: 285 | start_eval = True 286 | break 287 | # Finding result folder 288 | result_base_dir = "/misc/lmbraid19/thomasa/results/tum_rgbd" 289 | seq_base_dir = os.path.join(result_base_dir, seq_name) 290 | output_folder = os.path.join(seq_base_dir, config.name) 291 | evaluate_result(sequence_name=seq_name, config=config, result_folder=output_folder) 292 | 293 | if __name__ == "__main__": 294 | start_test() 295 | sys.exit() -------------------------------------------------------------------------------- /notebooks/LSDDepth dataset generator.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "\n", 8 | "# Run training data generator" 9 | ] 10 | }, 11 | { 12 | "cell_type": "code", 13 | "execution_count": null, 14 | "metadata": {}, 15 | "outputs": [], 16 | "source": [ 17 | "def dataset_len(dataset_loc):\n", 18 | " with open(dataset_loc) as f:\n", 19 | " for i, l in enumerate(f):\n", 20 | " pass\n", 21 | " return i + 1\n", 22 | "\n", 23 | "def gen_lsd_depth_training_data(dataset_loc, len_traj=200, keyframe_rate = 10, pause=2.5, test=True, debug=True):\n", 24 | " import time\n", 25 | " import subprocess\n", 26 | " import numpy as np\n", 27 | " \n", 28 | " # static params (not to be changed)\n", 29 | " bin_loc = \"rosrun lsd_slam_core depth_training_data_gen\"\n", 30 | " hz = \"0\"\n", 31 | " doSlam = \"true\"\n", 32 | " \n", 33 | " # params that may be changed for different datasets\n", 34 | " calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 35 | " minUseGrad = \"3\"\n", 36 | " \n", 37 | " # Finding number of iterations needed: (Assuming \"keyframe_rate\" frames avg between each KFs)\n", 38 | " len_sequence = dataset_len(dataset_loc)\n", 39 | " number_itrns = keyframe_rate * int(len_sequence/len_traj)\n", 40 | " \n", 41 | " # Finding start index for each iteration in the trajectory\n", 42 | " max_start = len_sequence - len_traj\n", 43 | " start_indx_array = np.random.randint(max_start, size=number_itrns)\n", 44 | " start_indx_array = start_indx_array.astype(np.unicode_)\n", 45 | " \n", 46 | " if test:\n", 47 | " itrn_max = 1\n", 48 | " else:\n", 49 | " itrn_max = number_itrns\n", 50 | " \n", 51 | " if debug:\n", 52 | " print(\"len_sequence: \", len_sequence)\n", 53 | " print(\"number_itrns: \", number_itrns)\n", 54 | " print(\"max_start: \", max_start)\n", 55 | " print(\"start_indx_array: \", start_indx_array)\n", 56 | " \n", 57 | " process_list= []\n", 58 | " args_prefix = (bin_loc + \" _calib:=\" + calib_file +\" _files:=\" + dataset_loc + \n", 59 | " \" _hz:=\" + hz + \" _doSlam:=\" + doSlam + \" _len_traj:=\" + str(len_traj) + \" _minUseGrad:=\" \n", 60 | " + minUseGrad + \" _KFUsageWeight:=20 _KFDistWeight:=20\" + \" _start_indx:=\")\n", 61 | " node_name_remap = \" /LSD_SLAM_TRAIN_DATA_GEN:=/LSDDepthtraining\"\n", 62 | " itr_num = \" _itr_num:=\"\n", 63 | "\n", 64 | " for ii in range(itrn_max):\n", 65 | " start_indx = start_indx_array[ii]\n", 66 | " command = args_prefix + start_indx + node_name_remap + str(ii) + itr_num + str(ii)\n", 67 | " process_list.append( subprocess.Popen(command, shell=True) )\n", 68 | " time.sleep(pause)" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": {}, 84 | "outputs": [], 85 | "source": [ 86 | "import subprocess\n", 87 | "import numpy as np" 88 | ] 89 | }, 90 | { 91 | "cell_type": "code", 92 | "execution_count": null, 93 | "metadata": {}, 94 | "outputs": [], 95 | "source": [ 96 | "#bin_loc = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/bin/depth_training_data_gen\"\n", 97 | "bin_loc = \"rosrun lsd_slam_core depth_training_data_gen\"\n", 98 | "calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 99 | "hz = \"0\"" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": {}, 106 | "outputs": [], 107 | "source": [ 108 | "dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n", 109 | "doSlam = \"true\"" 110 | ] 111 | }, 112 | { 113 | "cell_type": "code", 114 | "execution_count": null, 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "len_traj = \"200\"\n", 119 | "number_itrns = 70" 120 | ] 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": null, 125 | "metadata": {}, 126 | "outputs": [], 127 | "source": [ 128 | "def dataset_len(dataset_loc):\n", 129 | " with open(dataset_loc) as f:\n", 130 | " for i, l in enumerate(f):\n", 131 | " pass\n", 132 | " return i + 1" 133 | ] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "execution_count": null, 138 | "metadata": {}, 139 | "outputs": [], 140 | "source": [ 141 | "max_start = dataset_len(dataset_loc) - int(len_traj)\n", 142 | "max_start" 143 | ] 144 | }, 145 | { 146 | "cell_type": "code", 147 | "execution_count": null, 148 | "metadata": {}, 149 | "outputs": [], 150 | "source": [ 151 | "start_indx_array = np.random.randint(max_start, size=number_itrns)\n", 152 | "#start_indx_array_str = np.array(map(str, start_indx_array))\n", 153 | "#print(start_indx_array_str[0])\n", 154 | "start_indx_array = start_indx_array.astype(np.unicode_)\n", 155 | "start_indx_array" 156 | ] 157 | }, 158 | { 159 | "cell_type": "code", 160 | "execution_count": null, 161 | "metadata": {}, 162 | "outputs": [], 163 | "source": [ 164 | "subprocess.Popen('roscore', shell=True)" 165 | ] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "execution_count": null, 170 | "metadata": {}, 171 | "outputs": [], 172 | "source": [ 173 | "import time\n", 174 | "process_list= []\n", 175 | "args_prefix = (bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+ \n", 176 | " \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_len_traj:=\" + len_traj +\" \"+\n", 177 | " \"_minUseGrad:=3 _KFUsageWeight:=20 _KFDistWeight:=20\" + \"_start_indx:=\")\n", 178 | "node_name_remap = \" /LSD_SLAM_TRAIN_DATA_GEN:=/LSDDepthtraining\"\n", 179 | "itr_num = \" _itr_num:=\"\n", 180 | "\n", 181 | "#for ii in range(len(start_indx_array)):\n", 182 | "for ii in range(1):\n", 183 | " start_indx = start_indx_array[ii]\n", 184 | " process_list.append( subprocess.Popen(args_prefix + start_indx + \n", 185 | " node_name_remap + str(ii) + itr_num + str(ii), shell=True) )\n", 186 | " #print(args_prefix + start_indx + node_name_remap + str(ii))\n", 187 | " time.sleep(2.5)\n", 188 | " \n", 189 | " " 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": null, 195 | "metadata": {}, 196 | "outputs": [], 197 | "source": [ 198 | "#args_prefix = [bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc,\n", 199 | " # \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_len_traj:=\" + len_traj]\n", 200 | "args_prefix = (bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+ \n", 201 | " \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_len_traj:=\" + len_traj +\" \"+ \"_start_indx:=\")\n", 202 | "node_name_remap = \" LSD_SLAM_TRAIN_DATA_GEN:=LSDDepthtraining\"\n", 203 | "itr_num = \" _itr_num:=\"" 204 | ] 205 | }, 206 | { 207 | "cell_type": "code", 208 | "execution_count": null, 209 | "metadata": {}, 210 | "outputs": [], 211 | "source": [ 212 | "start_processes = lambda start_indx, node_num : subprocess.Popen(args_prefix + start_indx + \n", 213 | " node_name_remap + str(node_num), shell=True)" 214 | ] 215 | }, 216 | { 217 | "cell_type": "code", 218 | "execution_count": null, 219 | "metadata": {}, 220 | "outputs": [], 221 | "source": [ 222 | "node_nums = np.arange(len(start_indx_array))\n", 223 | "node_nums" 224 | ] 225 | }, 226 | { 227 | "cell_type": "code", 228 | "execution_count": null, 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "process_list = list(map(start_processes, start_indx_array[4:8], node_nums[4:8]))" 233 | ] 234 | }, 235 | { 236 | "cell_type": "code", 237 | "execution_count": null, 238 | "metadata": {}, 239 | "outputs": [], 240 | "source": [ 241 | "process_list" 242 | ] 243 | }, 244 | { 245 | "cell_type": "code", 246 | "execution_count": null, 247 | "metadata": {}, 248 | "outputs": [], 249 | "source": [ 250 | "start_processes(start_indx_array[10])" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "#for process in process_list:\n", 260 | " #print(process.poll())" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [ 269 | "cnt = 0\n", 270 | "process_list = []\n", 271 | "def start_processes(start_indx_array):\n", 272 | " \n", 273 | " " 274 | ] 275 | }, 276 | { 277 | "cell_type": "code", 278 | "execution_count": null, 279 | "metadata": {}, 280 | "outputs": [], 281 | "source": [ 282 | "def run_binary(start_indx):\n", 283 | " bin_loc = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/bin/depth_training_data_gen\"\n", 284 | " calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 285 | " hz = \"0\"\n", 286 | " dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n", 287 | " doSlam = \"true\"\n", 288 | " len_traj = \"300\"\n", 289 | " #args = (bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc, \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_start_indx:=\"+start_indx, \"_len_traj:=\" + len_traj)\n", 290 | " args = np.array([bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc,\n", 291 | " \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_start_indx:=\"+start_indx, \"_len_traj:=\" + len_traj])\n", 292 | " #args = np.array([bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+\n", 293 | " # \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_start_indx:=\"+start_indx +\" \"+ \"_len_traj:=\" + len_traj])\n", 294 | " args = np.vstack((args, args))\n", 295 | " result = pool.map( subprocess_call, args )\n", 296 | " #subprocess.call(args, shell=True)\n", 297 | " popen = subprocess.Popen(args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)\n", 298 | " #output, err = popen.communicate(b\"input data that is passed to subprocess' stdin\")\n", 299 | " #rc = popen.returncode\n" 300 | ] 301 | }, 302 | { 303 | "cell_type": "code", 304 | "execution_count": null, 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "run_binary(\"800\")\n" 309 | ] 310 | }, 311 | { 312 | "cell_type": "code", 313 | "execution_count": null, 314 | "metadata": {}, 315 | "outputs": [], 316 | "source": [ 317 | "popen = subprocess.Popen(args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)\n", 318 | "output, err = popen.communicate(b\"input data that is passed to subprocess' stdin\")\n", 319 | "rc = popen.returncode\n", 320 | "print(rc)\n", 321 | "print(output)\n", 322 | "print(err)" 323 | ] 324 | }, 325 | { 326 | "cell_type": "code", 327 | "execution_count": null, 328 | "metadata": {}, 329 | "outputs": [], 330 | "source": [] 331 | } 332 | ], 333 | "metadata": { 334 | "kernelspec": { 335 | "display_name": "Python 3", 336 | "language": "python", 337 | "name": "python3" 338 | }, 339 | "language_info": { 340 | "codemirror_mode": { 341 | "name": "ipython", 342 | "version": 3 343 | }, 344 | "file_extension": ".py", 345 | "mimetype": "text/x-python", 346 | "name": "python", 347 | "nbconvert_exporter": "python", 348 | "pygments_lexer": "ipython3", 349 | "version": "3.5.2" 350 | } 351 | }, 352 | "nbformat": 4, 353 | "nbformat_minor": 2 354 | } 355 | -------------------------------------------------------------------------------- /notebooks/.ipynb_checkpoints/LSDDepth dataset generator-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "\n", 8 | "# Run training data generator" 9 | ] 10 | }, 11 | { 12 | "cell_type": "code", 13 | "execution_count": null, 14 | "metadata": {}, 15 | "outputs": [], 16 | "source": [ 17 | "def dataset_len(dataset_loc):\n", 18 | " with open(dataset_loc) as f:\n", 19 | " for i, l in enumerate(f):\n", 20 | " pass\n", 21 | " return i + 1\n", 22 | "\n", 23 | "def gen_lsd_depth_training_data(dataset_loc, len_traj=200, keyframe_rate = 10, pause=2.5, test=True, debug=True):\n", 24 | " import time\n", 25 | " import subprocess\n", 26 | " import numpy as np\n", 27 | " \n", 28 | " # static params (not to be changed)\n", 29 | " bin_loc = \"rosrun lsd_slam_core depth_training_data_gen\"\n", 30 | " hz = \"0\"\n", 31 | " doSlam = \"true\"\n", 32 | " \n", 33 | " # params that may be changed for different datasets\n", 34 | " calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 35 | " minUseGrad = \"3\"\n", 36 | " \n", 37 | " # Finding number of iterations needed: (Assuming \"keyframe_rate\" frames avg between each KFs)\n", 38 | " len_sequence = dataset_len(dataset_loc)\n", 39 | " number_itrns = keyframe_rate * int(len_sequence/len_traj)\n", 40 | " \n", 41 | " # Finding start index for each iteration in the trajectory\n", 42 | " max_start = len_sequence - len_traj\n", 43 | " start_indx_array = np.random.randint(max_start, size=number_itrns)\n", 44 | " start_indx_array = start_indx_array.astype(np.unicode_)\n", 45 | " \n", 46 | " if test:\n", 47 | " itrn_max = 1\n", 48 | " else:\n", 49 | " itrn_max = number_itrns\n", 50 | " \n", 51 | " if debug:\n", 52 | " print(\"len_sequence: \", len_sequence)\n", 53 | " print(\"number_itrns: \", number_itrns)\n", 54 | " print(\"max_start: \", max_start)\n", 55 | " print(\"start_indx_array: \", start_indx_array)\n", 56 | " \n", 57 | " process_list= []\n", 58 | " args_prefix = (bin_loc + \" _calib:=\" + calib_file +\" _files:=\" + dataset_loc + \n", 59 | " \" _hz:=\" + hz + \" _doSlam:=\" + doSlam + \" _len_traj:=\" + str(len_traj) + \" _minUseGrad:=\" \n", 60 | " + minUseGrad + \" _KFUsageWeight:=20 _KFDistWeight:=20\" + \" _start_indx:=\")\n", 61 | " node_name_remap = \" /LSD_SLAM_TRAIN_DATA_GEN:=/LSDDepthtraining\"\n", 62 | " itr_num = \" _itr_num:=\"\n", 63 | "\n", 64 | " for ii in range(itrn_max):\n", 65 | " start_indx = start_indx_array[ii]\n", 66 | " command = args_prefix + start_indx + node_name_remap + str(ii) + itr_num + str(ii)\n", 67 | " process_list.append( subprocess.Popen(command, shell=True) )\n", 68 | " time.sleep(pause)" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": {}, 84 | "outputs": [], 85 | "source": [ 86 | "import subprocess\n", 87 | "import numpy as np" 88 | ] 89 | }, 90 | { 91 | "cell_type": "code", 92 | "execution_count": null, 93 | "metadata": {}, 94 | "outputs": [], 95 | "source": [ 96 | "#bin_loc = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/bin/depth_training_data_gen\"\n", 97 | "bin_loc = \"rosrun lsd_slam_core depth_training_data_gen\"\n", 98 | "calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 99 | "hz = \"0\"" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": {}, 106 | "outputs": [], 107 | "source": [ 108 | "dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n", 109 | "doSlam = \"true\"" 110 | ] 111 | }, 112 | { 113 | "cell_type": "code", 114 | "execution_count": null, 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "len_traj = \"200\"\n", 119 | "number_itrns = 70" 120 | ] 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": null, 125 | "metadata": {}, 126 | "outputs": [], 127 | "source": [ 128 | "def dataset_len(dataset_loc):\n", 129 | " with open(dataset_loc) as f:\n", 130 | " for i, l in enumerate(f):\n", 131 | " pass\n", 132 | " return i + 1" 133 | ] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "execution_count": null, 138 | "metadata": {}, 139 | "outputs": [], 140 | "source": [ 141 | "max_start = dataset_len(dataset_loc) - int(len_traj)\n", 142 | "max_start" 143 | ] 144 | }, 145 | { 146 | "cell_type": "code", 147 | "execution_count": null, 148 | "metadata": {}, 149 | "outputs": [], 150 | "source": [ 151 | "start_indx_array = np.random.randint(max_start, size=number_itrns)\n", 152 | "#start_indx_array_str = np.array(map(str, start_indx_array))\n", 153 | "#print(start_indx_array_str[0])\n", 154 | "start_indx_array = start_indx_array.astype(np.unicode_)\n", 155 | "start_indx_array" 156 | ] 157 | }, 158 | { 159 | "cell_type": "code", 160 | "execution_count": null, 161 | "metadata": {}, 162 | "outputs": [], 163 | "source": [ 164 | "subprocess.Popen('roscore', shell=True)" 165 | ] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "execution_count": null, 170 | "metadata": {}, 171 | "outputs": [], 172 | "source": [ 173 | "import time\n", 174 | "process_list= []\n", 175 | "args_prefix = (bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+ \n", 176 | " \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_len_traj:=\" + len_traj +\" \"+\n", 177 | " \"_minUseGrad:=3 _KFUsageWeight:=20 _KFDistWeight:=20\" + \"_start_indx:=\")\n", 178 | "node_name_remap = \" /LSD_SLAM_TRAIN_DATA_GEN:=/LSDDepthtraining\"\n", 179 | "itr_num = \" _itr_num:=\"\n", 180 | "\n", 181 | "#for ii in range(len(start_indx_array)):\n", 182 | "for ii in range(1):\n", 183 | " start_indx = start_indx_array[ii]\n", 184 | " process_list.append( subprocess.Popen(args_prefix + start_indx + \n", 185 | " node_name_remap + str(ii) + itr_num + str(ii), shell=True) )\n", 186 | " #print(args_prefix + start_indx + node_name_remap + str(ii))\n", 187 | " time.sleep(2.5)\n", 188 | " \n", 189 | " " 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": null, 195 | "metadata": {}, 196 | "outputs": [], 197 | "source": [ 198 | "#args_prefix = [bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc,\n", 199 | " # \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_len_traj:=\" + len_traj]\n", 200 | "args_prefix = (bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+ \n", 201 | " \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_len_traj:=\" + len_traj +\" \"+ \"_start_indx:=\")\n", 202 | "node_name_remap = \" LSD_SLAM_TRAIN_DATA_GEN:=LSDDepthtraining\"\n", 203 | "itr_num = \" _itr_num:=\"" 204 | ] 205 | }, 206 | { 207 | "cell_type": "code", 208 | "execution_count": null, 209 | "metadata": {}, 210 | "outputs": [], 211 | "source": [ 212 | "start_processes = lambda start_indx, node_num : subprocess.Popen(args_prefix + start_indx + \n", 213 | " node_name_remap + str(node_num), shell=True)" 214 | ] 215 | }, 216 | { 217 | "cell_type": "code", 218 | "execution_count": null, 219 | "metadata": {}, 220 | "outputs": [], 221 | "source": [ 222 | "node_nums = np.arange(len(start_indx_array))\n", 223 | "node_nums" 224 | ] 225 | }, 226 | { 227 | "cell_type": "code", 228 | "execution_count": null, 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "process_list = list(map(start_processes, start_indx_array[4:8], node_nums[4:8]))" 233 | ] 234 | }, 235 | { 236 | "cell_type": "code", 237 | "execution_count": null, 238 | "metadata": {}, 239 | "outputs": [], 240 | "source": [ 241 | "process_list" 242 | ] 243 | }, 244 | { 245 | "cell_type": "code", 246 | "execution_count": null, 247 | "metadata": {}, 248 | "outputs": [], 249 | "source": [ 250 | "start_processes(start_indx_array[10])" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "#for process in process_list:\n", 260 | " #print(process.poll())" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [ 269 | "cnt = 0\n", 270 | "process_list = []\n", 271 | "def start_processes(start_indx_array):\n", 272 | " \n", 273 | " " 274 | ] 275 | }, 276 | { 277 | "cell_type": "code", 278 | "execution_count": null, 279 | "metadata": {}, 280 | "outputs": [], 281 | "source": [ 282 | "def run_binary(start_indx):\n", 283 | " bin_loc = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/bin/depth_training_data_gen\"\n", 284 | " calib_file = \"/misc/lmbraid19/thomasa/rosbuild_ws/packages/lsd_slam/lsd_slam_core/calib/OpenCV_example_calib.cfg\"\n", 285 | " hz = \"0\"\n", 286 | " dataset_loc = \"/misc/lmbraid19/thomasa/datasets/rgbd/rgbd_dataset_freiburg1_teddy/rgb_depth_associated.txt\"\n", 287 | " doSlam = \"true\"\n", 288 | " len_traj = \"300\"\n", 289 | " #args = (bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc, \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_start_indx:=\"+start_indx, \"_len_traj:=\" + len_traj)\n", 290 | " args = np.array([bin_loc, \"_calib:=\" + calib_file, \"_files:=\" + dataset_loc,\n", 291 | " \"_hz:=\" + hz, \"_doSlam:=\" + doSlam, \"_start_indx:=\"+start_indx, \"_len_traj:=\" + len_traj])\n", 292 | " #args = np.array([bin_loc +\" \"+ \"_calib:=\" + calib_file +\" \" +\"_files:=\" + dataset_loc +\" \"+\n", 293 | " # \"_hz:=\" + hz +\" \"+ \"_doSlam:=\" + doSlam +\" \"+ \"_start_indx:=\"+start_indx +\" \"+ \"_len_traj:=\" + len_traj])\n", 294 | " args = np.vstack((args, args))\n", 295 | " result = pool.map( subprocess_call, args )\n", 296 | " #subprocess.call(args, shell=True)\n", 297 | " popen = subprocess.Popen(args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)\n", 298 | " #output, err = popen.communicate(b\"input data that is passed to subprocess' stdin\")\n", 299 | " #rc = popen.returncode\n" 300 | ] 301 | }, 302 | { 303 | "cell_type": "code", 304 | "execution_count": null, 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "run_binary(\"800\")\n" 309 | ] 310 | }, 311 | { 312 | "cell_type": "code", 313 | "execution_count": null, 314 | "metadata": {}, 315 | "outputs": [], 316 | "source": [ 317 | "popen = subprocess.Popen(args, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)\n", 318 | "output, err = popen.communicate(b\"input data that is passed to subprocess' stdin\")\n", 319 | "rc = popen.returncode\n", 320 | "print(rc)\n", 321 | "print(output)\n", 322 | "print(err)" 323 | ] 324 | }, 325 | { 326 | "cell_type": "code", 327 | "execution_count": null, 328 | "metadata": {}, 329 | "outputs": [], 330 | "source": [] 331 | } 332 | ], 333 | "metadata": { 334 | "kernelspec": { 335 | "display_name": "Python 3", 336 | "language": "python", 337 | "name": "python3" 338 | }, 339 | "language_info": { 340 | "codemirror_mode": { 341 | "name": "ipython", 342 | "version": 3 343 | }, 344 | "file_extension": ".py", 345 | "mimetype": "text/x-python", 346 | "name": "python", 347 | "nbconvert_exporter": "python", 348 | "pygments_lexer": "ipython3", 349 | "version": "3.5.2" 350 | } 351 | }, 352 | "nbformat": 4, 353 | "nbformat_minor": 2 354 | } 355 | -------------------------------------------------------------------------------- /notebooks/depth_fusion_network_test.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "from reinforced_visual_slam.srv import *\n", 10 | "import rospy\n", 11 | "\n", 12 | "import matplotlib.pyplot as plt\n", 13 | "from matplotlib import colors\n", 14 | "\n", 15 | "\n", 16 | "import os\n", 17 | "import sys\n", 18 | "sys.path.insert(0,'/misc/software/opencv/opencv-3.2.0_cuda8_with_contrib-x86_64-gcc5.4.0/lib/python3.5/dist-packages')\n", 19 | "\n", 20 | "import cv2\n", 21 | "import numpy as np\n", 22 | "from skimage.transform import resize\n", 23 | "\n", 24 | "import tensorflow as tf\n", 25 | "\n", 26 | "sys.path.insert(0,'/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion')\n", 27 | "from net.my_models import *" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "metadata": {}, 34 | "outputs": [], 35 | "source": [ 36 | "model_name = \"NetV02_L1cleanSigL1_down_aug_\"\n", 37 | "model_function = model_fn_Netv2_LossL1CleanSigL1_down\n", 38 | "model_base_dir = \"/misc/lmbraid19/thomasa/catkin_ws/src/reinforced_visual_slam/networks/depth_fusion/training\"\n", 39 | "model_dir = os.path.join(model_base_dir, model_name)\n", 40 | "print(model_dir)\n", 41 | "print(model_function)" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "lsd_depth_fuser = tf.estimator.Estimator(model_fn=model_function, model_dir=model_dir, \n", 51 | " params={'data_format':\"channels_first\", 'multi_gpu':False})" 52 | ] 53 | }, 54 | { 55 | "cell_type": "code", 56 | "execution_count": null, 57 | "metadata": {}, 58 | "outputs": [], 59 | "source": [ 60 | "test_file = '/misc/lmbraid19/thomasa/datasets/LSDDepthTraining/test/rgbd_lsdDepth_test.txt'\n", 61 | "base_path = os.path.dirname(test_file)\n" 62 | ] 63 | }, 64 | { 65 | "cell_type": "code", 66 | "execution_count": null, 67 | "metadata": {}, 68 | "outputs": [], 69 | "source": [ 70 | "def predict_input_fn(rgb, sparse_idepth, sparse_idepth_var):\n", 71 | " feature_names = [\n", 72 | " 'rgb',\n", 73 | " 'sparseInverseDepth',\n", 74 | " 'sparseInverseDepthVariance'\n", 75 | " ]\n", 76 | " input_tensors = [rgb, sparse_idepth[np.newaxis,:,:], sparse_idepth_var[np.newaxis,:,:]]\n", 77 | " inputs = dict(zip(feature_names, input_tensors))\n", 78 | " print(inputs['rgb'].shape)\n", 79 | " dataset = tf.data.Dataset.from_tensors(inputs)\n", 80 | " dataset = dataset.batch(1)\n", 81 | " iterator = dataset.make_one_shot_iterator()\n", 82 | " features = iterator.get_next()\n", 83 | " print(features['rgb'].shape, features['sparseInverseDepth'].shape, \n", 84 | " features['sparseInverseDepthVariance'].shape)\n", 85 | " return features" 86 | ] 87 | }, 88 | { 89 | "cell_type": "code", 90 | "execution_count": null, 91 | "metadata": {}, 92 | "outputs": [], 93 | "source": [ 94 | "with open(test_file) as file:\n", 95 | " for line in file.readlines():\n", 96 | " # prepare test input\n", 97 | " depth_gt_file, rgb_file, sparse_idepth_bin, sparse_idepth_var_bin = line.strip().split(',')[0:]\n", 98 | "\n", 99 | " print(\"Sparse idepth ---\")\n", 100 | " sparse_idepth = np.fromfile(os.path.join(base_path, sparse_idepth_bin), dtype=np.float16)\n", 101 | " print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth))\n", 102 | " print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth))\n", 103 | " sparse_idepth = sparse_idepth.astype(np.float32)\n", 104 | " sparse_idepth = sparse_idepth.reshape((480, 640))\n", 105 | " #sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0)\n", 106 | " sparse_idepth = cv2.resize(sparse_idepth, (320, 240), cv2.INTER_NEAREST)\n", 107 | " #sparse_idepth = sparse_idepth.resize()\n", 108 | " print(\"max: \", np.nanmax(sparse_idepth))\n", 109 | " print(\"shape: \",sparse_idepth.shape)\n", 110 | " print(\"dtype: \", sparse_idepth.dtype)\n", 111 | "\n", 112 | " print(\"Sparse idepth var ---\")\n", 113 | " sparse_idepth_var = np.fromfile(os.path.join(base_path, sparse_idepth_var_bin), dtype=np.float16)\n", 114 | " print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth_var))\n", 115 | " print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth_var))\n", 116 | " sparse_idepth_var = sparse_idepth_var.astype(np.float32)\n", 117 | " sparse_idepth_var = sparse_idepth_var.reshape((480, 640))\n", 118 | " #sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0)\n", 119 | " sparse_idepth_var = cv2.resize(sparse_idepth_var, (320, 240), cv2.INTER_NEAREST)\n", 120 | " #sparse_idepth = sparse_idepth.resize()\n", 121 | " print(\"max: \", np.nanmax(sparse_idepth_var))\n", 122 | " print(\"shape: \",sparse_idepth_var.shape)\n", 123 | " print(\"dtype: \", sparse_idepth_var.dtype)\n", 124 | "\n", 125 | " print(\"rgb---\")\n", 126 | " rgb = cv2.imread(os.path.join(base_path, rgb_file), -1).astype(np.float32)/255\n", 127 | " rgb = cv2.resize(rgb, (320, 240))\n", 128 | " rgb = np.transpose(rgb, (2,0,1))\n", 129 | " print(\"shape: \", rgb.shape)\n", 130 | " print(\"max: \", np.nanmax(rgb))\n", 131 | " print(\"dtype: \", rgb.dtype)\n", 132 | " \n", 133 | " depth_gt = cv2.imread(os.path.join(base_path, depth_gt_file), -1).astype(np.float32)/5000\n", 134 | " gt_max = np.nanmax(depth_gt)\n", 135 | "\n", 136 | " depthmap_predicted = lsd_depth_fuser.predict(lambda : predict_input_fn(rgb, sparse_idepth, sparse_idepth_var))\n", 137 | " depthmap_scaled = list(depthmap_predicted)[0]['depth'][0]\n", 138 | " \n", 139 | " #invrting depthmap\n", 140 | " depthmap_scaled = np.where(depthmap_scaled>0.2, 1./depthmap_scaled, np.zeros_like(depthmap_scaled))\n", 141 | " \n", 142 | " fig, axs = plt.subplots(1, 2)\n", 143 | " norm = colors.Normalize(vmin=0, vmax=gt_max)\n", 144 | " cmap = 'hot'\n", 145 | " images = []\n", 146 | " images.append(axs[0].imshow(depthmap_scaled, cmap=cmap))\n", 147 | " images.append(axs[1].imshow(depth_gt, cmap=cmap))\n", 148 | " for im in images:\n", 149 | " im.set_norm(norm)\n", 150 | " fig.colorbar(images[0], ax=axs, orientation='horizontal', fraction=.1)\n", 151 | " #plt.figure(\"sparse_depth\")\n", 152 | " #plt.imshow(sparse_depth, cmap='hot', norm=norm)\n", 153 | " #plt.clim(0,gt_max)\n", 154 | " #plt.colorbar()\n", 155 | " #plt.figure(\"gt_depth\")\n", 156 | " #plt.imshow(depth_gt, cmap='hot', norm=norm)\n", 157 | " #plt.clim(0,gt_max)\n", 158 | " #plt.colorbar()\n", 159 | " #plt.figure(\"sparse_depth_variance\")\n", 160 | " #plt.imshow(sparse_idepthVar, cmap='hot')\n", 161 | " #plt.figure(\"rgb\")\n", 162 | " #plt.imshow(rgb)\n", 163 | " plt.show()\n", 164 | " \n", 165 | " #plt.imshow(depthmap_scaled, cmap='hot')\n", 166 | " #plt.colorbar()\n", 167 | " #plt.show()\n", 168 | " inp = input(\"Press Enter to continue...\")\n", 169 | " print(inp)" 170 | ] 171 | }, 172 | { 173 | "cell_type": "code", 174 | "execution_count": null, 175 | "metadata": {}, 176 | "outputs": [], 177 | "source": [ 178 | "# prepare test input\n", 179 | "rgb_file, sparse_idepth_bin, sparse_idepth_var_bin = file.readline().strip().split(',')[1:]\n", 180 | "print(rgb_file)\n", 181 | "print(sparse_idepth_bin)\n", 182 | "print(sparse_idepth_var_bin)\n", 183 | "base_path = os.path.dirname(test_file)\n", 184 | "print(base_path)" 185 | ] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": null, 190 | "metadata": {}, 191 | "outputs": [], 192 | "source": [ 193 | "print(\"Sparse idepth ---\")\n", 194 | "sparse_idepth = np.fromfile(os.path.join(base_path, sparse_idepth_bin), dtype=np.float16)\n", 195 | "print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth))\n", 196 | "print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth))\n", 197 | "sparse_idepth = sparse_idepth.astype(np.float32)\n", 198 | "sparse_idepth = sparse_idepth.reshape((480, 640))\n", 199 | "#sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0)\n", 200 | "sparse_idepth = cv2.resize(sparse_idepth, (320, 240), cv2.INTER_NEAREST)\n", 201 | "#sparse_idepth = sparse_idepth.resize()\n", 202 | "print(\"max: \", np.nanmax(sparse_idepth))\n", 203 | "print(\"shape: \",sparse_idepth.shape)\n", 204 | "print(\"dtype: \", sparse_idepth.dtype)" 205 | ] 206 | }, 207 | { 208 | "cell_type": "code", 209 | "execution_count": null, 210 | "metadata": {}, 211 | "outputs": [], 212 | "source": [ 213 | "print(\"Sparse idepth var ---\")\n", 214 | "sparse_idepth_var = np.fromfile(os.path.join(base_path, sparse_idepth_var_bin), dtype=np.float16)\n", 215 | "print(\"min half-float sparse_idepth: \", np.nanmin(sparse_idepth_var))\n", 216 | "print(\"max half-float sparse_idepth: \", np.nanmax(sparse_idepth_var))\n", 217 | "sparse_idepth_var = sparse_idepth_var.astype(np.float32)\n", 218 | "sparse_idepth_var = sparse_idepth_var.reshape((480, 640))\n", 219 | "#sparse_idepth = resize(sparse_idepth, output_shape=(240,320), order=0)\n", 220 | "sparse_idepth_var = cv2.resize(sparse_idepth_var, (320, 240), cv2.INTER_NEAREST)\n", 221 | "#sparse_idepth = sparse_idepth.resize()\n", 222 | "print(\"max: \", np.nanmax(sparse_idepth_var))\n", 223 | "print(\"shape: \",sparse_idepth_var.shape)\n", 224 | "print(\"dtype: \", sparse_idepth_var.dtype)" 225 | ] 226 | }, 227 | { 228 | "cell_type": "code", 229 | "execution_count": null, 230 | "metadata": {}, 231 | "outputs": [], 232 | "source": [ 233 | "print(\"rgb---\")\n", 234 | "rgb = cv2.imread(os.path.join(base_path, rgb_file), -1).astype(np.float32)/255\n", 235 | "rgb = cv2.resize(rgb, (320, 240))\n", 236 | "rgb = np.transpose(rgb, (2,0,1))\n", 237 | "print(\"shape: \", rgb.shape)\n", 238 | "print(\"max: \", np.nanmax(rgb))\n", 239 | "print(\"dtype: \", rgb.dtype)" 240 | ] 241 | }, 242 | { 243 | "cell_type": "code", 244 | "execution_count": null, 245 | "metadata": {}, 246 | "outputs": [], 247 | "source": [ 248 | "depthmap_predicted = lsd_depth_fuser.predict(lambda : predict_input_fn(rgb, sparse_idepth, sparse_idepth_var))" 249 | ] 250 | }, 251 | { 252 | "cell_type": "code", 253 | "execution_count": null, 254 | "metadata": {}, 255 | "outputs": [], 256 | "source": [ 257 | "#print(list(depthmap_predicted)[0]['depth'][0].shape)" 258 | ] 259 | }, 260 | { 261 | "cell_type": "code", 262 | "execution_count": null, 263 | "metadata": {}, 264 | "outputs": [], 265 | "source": [ 266 | "depthmap_scaled = list(depthmap_predicted)[0]['depth'][0]" 267 | ] 268 | }, 269 | { 270 | "cell_type": "code", 271 | "execution_count": null, 272 | "metadata": {}, 273 | "outputs": [], 274 | "source": [] 275 | }, 276 | { 277 | "cell_type": "code", 278 | "execution_count": null, 279 | "metadata": {}, 280 | "outputs": [], 281 | "source": [ 282 | "plt.figure(\"depth_prediction\")" 283 | ] 284 | }, 285 | { 286 | "cell_type": "code", 287 | "execution_count": null, 288 | "metadata": {}, 289 | "outputs": [], 290 | "source": [ 291 | "plt.imshow(depthmap_scaled, cmap='hot')\n", 292 | "plt.colorbar()\n", 293 | "plt.show()" 294 | ] 295 | }, 296 | { 297 | "cell_type": "code", 298 | "execution_count": null, 299 | "metadata": {}, 300 | "outputs": [], 301 | "source": [] 302 | } 303 | ], 304 | "metadata": { 305 | "kernelspec": { 306 | "display_name": "Python 3", 307 | "language": "python", 308 | "name": "python3" 309 | }, 310 | "language_info": { 311 | "codemirror_mode": { 312 | "name": "ipython", 313 | "version": 3 314 | }, 315 | "file_extension": ".py", 316 | "mimetype": "text/x-python", 317 | "name": "python", 318 | "nbconvert_exporter": "python", 319 | "pygments_lexer": "ipython3", 320 | "version": "3.5.2" 321 | } 322 | }, 323 | "nbformat": 4, 324 | "nbformat_minor": 2 325 | } 326 | --------------------------------------------------------------------------------