├── HSM ├── Camera.py ├── FeatureTracker.py ├── HSM.py ├── config │ └── config.yaml ├── cuda │ └── source.cu ├── cuda_source.py ├── dataloader.py ├── filter.py ├── main.py ├── metric.py └── utils.py ├── LICENSE ├── README.md └── snapshot ├── depth.png ├── event.png └── frame.png /HSM/Camera.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | from cv2 import CV_16SC2, CV_32FC1 9 | import numpy as np 10 | import cv2 11 | 12 | class Camera: 13 | K_rect = np.eye(3) 14 | K_rect_inv = np.eye(3) 15 | camera_num = 0 16 | width = 0 17 | height = 0 18 | resolution = (height, width) 19 | baseline = 0.0 20 | def set_baseline(baseline, baseline_gt=-1): 21 | Camera.baseline = baseline 22 | if baseline_gt == -1: 23 | Camera.baseline_gt = baseline 24 | else: 25 | Camera.baseline_gt = baseline_gt 26 | 27 | class CameraDSEC(Camera): 28 | def __init__(self, params, rect_mat=np.eye(3)): 29 | # dsec dataset 30 | # cam0: event left 31 | # cam1: frame left 32 | # cam2: frame right 33 | # cam3: event right 34 | 35 | # resolution 36 | self.imsize = np.array(params['resolution']) 37 | self.resolution = np.array([self.imsize[1], self.imsize[0]]) 38 | self.height = self.imsize[1] 39 | self.width = self.imsize[0] 40 | 41 | # intrinsics 42 | self.fx = params['camera_matrix'][0] 43 | self.fy = params['camera_matrix'][1] 44 | self.cx = params['camera_matrix'][2] 45 | self.cy = params['camera_matrix'][3] 46 | self.K = np.array([[self.fx, 0.0, self.cx],[0.0, self.fy, self.cy],[0.0, 0.0, 1.0]]) 47 | # distortions 48 | try: 49 | self.dist_coeff = np.array(params['distortion_coeffs']) 50 | except: 51 | self.dist_coeff = np.zeros((4, 1)) 52 | self.cv_dist_coeff = np.array([self.dist_coeff[0], self.dist_coeff[1], 0.0, 0.0, self.dist_coeff[2]], dtype=object) 53 | # rectification 54 | self.rect_mat = rect_mat 55 | 56 | CameraDSEC.camera_num += 1 57 | 58 | # generate rect map 59 | self.generate_undist_map() 60 | 61 | @staticmethod 62 | def set_cam_rect(params): 63 | fx = params['camera_matrix'][0] 64 | fy = params['camera_matrix'][1] 65 | cx = params['camera_matrix'][2] 66 | cy = params['camera_matrix'][3] 67 | Camera.width = params['resolution'][0] 68 | Camera.height = params['resolution'][1] 69 | Camera.K_rect = np.array([[fx, 0.0, cx],[0.0, fy, cy],[0.0, 0.0, 1.0]]) 70 | Camera.K_rect_inv = np.linalg.inv(Camera.K_rect) 71 | Camera.resolution = (Camera.height, Camera.width) 72 | 73 | def generate_undist_map(self): 74 | self.undist_map1, self.undist_map2 = cv2.initUndistortRectifyMap(self.K, self.dist_coeff, None, self.K, self.imsize, CV_16SC2) 75 | self.rect_map1, self.rect_map2 = cv2.initUndistortRectifyMap(self.K, self.dist_coeff, self.rect_mat, Camera.K_rect, self.imsize, CV_16SC2) 76 | 77 | def rectify(self, image, interp = cv2.INTER_LINEAR): 78 | image_rect = cv2.remap(image.astype(np.float32), self.rect_map1, self.rect_map2, interpolation=interp)[:Camera.height, :Camera.width] 79 | return image_rect 80 | 81 | def undist(self, image): 82 | image_undist = cv2.remap(image, self.undist_map1, self.undist_map2, interpolation=cv2.INTER_LINEAR) 83 | return image_undist 84 | 85 | def rectify_events_default(self, events): 86 | event_reshaped = np.reshape(events, (-1,1,4)) 87 | uv_rect = cv2.undistortPoints(event_reshaped[:,:,0:2], self.K, self.dist_coeff, self.rect_mat, Camera.K_rect) 88 | events_rect = np.concatenate((np.reshape(uv_rect, (-1, 2)), events[:,2:4]), axis = 1) 89 | return events_rect 90 | 91 | def rectify_events(self, events): 92 | event_reshaped = np.reshape(events, (-1,1,4)) 93 | uv_rect = cv2.undistortPointsIter(event_reshaped[:,:,0:2], self.K, self.dist_coeff, self.rect_mat, Camera.K_rect, (cv2.TERM_CRITERIA_COUNT | cv2.TERM_CRITERIA_EPS, 40, 0.01)) 94 | events_rect = np.concatenate((np.reshape(uv_rect, (-1, 2)), events[:,2:4]), axis = 1) 95 | return events_rect 96 | 97 | def project_events(self, events): 98 | image = np.zeros((self.height, self.width)) 99 | for index, value in enumerate(events): 100 | u = int(value[0]) 101 | v = int(value[1]) 102 | if ((u >= 0) & (v >= 0) & (u < self.width) & (v < self.height)): 103 | image[v, u] += 2*value[3] - 1 104 | 105 | return image 106 | -------------------------------------------------------------------------------- /HSM/FeatureTracker.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | import numpy as np 9 | import numpy.linalg as la 10 | import copy 11 | import cv2 12 | 13 | from utils import * 14 | from Camera import Camera, CameraDSEC 15 | 16 | class Map: 17 | def __init__(self, id): 18 | self.id = id 19 | self.pts = np.zeros((1,3)) 20 | self.confidence = 0 21 | self.is_reconstructed = False 22 | 23 | def set_pts(self, pts): 24 | self.pts = pts 25 | self.is_reconstructed = True 26 | 27 | class Feature: 28 | id = 0 29 | def __init__(self): 30 | self.clear() 31 | 32 | def set_uv(self, uv, id): 33 | self.uv = uv 34 | self.id = id 35 | self.is_extracted = True 36 | self.is_tracked = True 37 | 38 | def new_uv(self, uv): 39 | self.clear() 40 | self.uv = uv 41 | self.id = Feature.id 42 | Feature.id += 1 43 | self.is_extracted = True 44 | self.is_tracked = False 45 | 46 | def clear(self): 47 | self.id = -1 48 | self.uv = np.zeros((1, 2), dtype=np.float32) 49 | 50 | self.is_tracked = False 51 | self.is_reconstructed = False 52 | self.is_extracted = False 53 | 54 | class FeatureTracker: 55 | def __init__(self, params): 56 | self.cam0 = params['cam0'] 57 | self.cam1 = params['cam1'] 58 | self.feature_num = params['feature num'] 59 | self.track_win_size = params['track_win_size'] 60 | self.extract_win_size = params['extract_win_size'] 61 | self.track_err_thres = params['track_err_thres'] 62 | self.track_params = dict(winSize=self.track_win_size, 63 | criteria=(cv2.TERM_CRITERIA_EPS | 64 | cv2.TERM_CRITERIA_COUNT, 30, 0.03)) 65 | 66 | self.features = [Feature() for i in range(self.feature_num)] 67 | self.feature_idx = [] 68 | self.tracked_feature_idx = [] 69 | self.empty_feature_idx = [i for i in range(self.feature_num)] 70 | 71 | self.map_pts = {} 72 | self.image = np.zeros(Camera.resolution) 73 | 74 | self.iter = 0 75 | 76 | self.is_first_frame = True 77 | self.is_recon_init = False 78 | self.is_init = False 79 | 80 | def detect_feature(self, image): 81 | self.iter += 1 82 | self.previmage = self.image 83 | self.prevfeatures = copy.deepcopy(self.features) 84 | self.prevfeature_idx = self.feature_idx 85 | 86 | image = image.astype(np.uint8) 87 | self.image = image 88 | feature_mask = np.ones(Camera.resolution) # TODO change to rect area 89 | if self.is_first_frame: 90 | self.is_first_frame = False 91 | else: 92 | tracked_points = self.track_feature(image) 93 | 94 | # extract new features 95 | for _, t_idx in enumerate(self.tracked_feature_idx): 96 | point = self.features[t_idx].uv.astype(np.uint8) 97 | cv2.circle(feature_mask, point, 5, 0, -1) 98 | if len(self.empty_feature_idx) > 1: 99 | new_points = cv2.goodFeaturesToTrack(image, len(self.empty_feature_idx), 0.01, 5, mask=feature_mask.astype(np.uint8), blockSize=self.extract_win_size, k=0.03).squeeze() 100 | 101 | for pts_idx, new_idx in enumerate(self.empty_feature_idx): 102 | if(pts_idx >= new_points.shape[0]): 103 | self.features[new_idx].clear() 104 | else: 105 | self.features[new_idx].new_uv(new_points[pts_idx, :]) 106 | id = self.features[new_idx].id 107 | self.map_pts[id] = Map(id) 108 | 109 | self.feature_idx = [i for i in range(self.feature_num) 110 | if self.features[i].is_extracted] 111 | # update parameters 112 | 113 | def track_feature(self, image): 114 | points = np.array([self.prevfeatures[idx].uv for i, idx in enumerate(self.prevfeature_idx)]) 115 | p1, st, err = cv2.calcOpticalFlowPyrLK(self.previmage, 116 | image, points.astype(np.float32), 117 | None, **self.track_params) 118 | 119 | for i, idx in enumerate(self.prevfeature_idx): 120 | if (st[i] & (err[i] < self.track_err_thres) 121 | & (p1[i, 0] < Camera.resolution[1]) & (p1[i, 0] >= 0) 122 | & (p1[i, 1] < Camera.resolution[0]) & (p1[i, 1] >= 0)): 123 | self.features[idx].set_uv(p1[i, :], self.prevfeatures[idx].id) 124 | if p1[i, 1] == 0: 125 | print(1) 126 | else: 127 | self.features[idx].clear() 128 | 129 | self.tracked_feature_idx = [i for i in range(self.feature_num) 130 | if self.features[i].is_tracked] 131 | self.empty_feature_idx = [i for i in range(self.feature_num) 132 | if not self.features[i].is_tracked] 133 | tracked_points = [self.features[i].uv for i in range(self.feature_num) 134 | if self.features[i].is_tracked] 135 | return tracked_points 136 | 137 | def drawFeatureTracks(self, image): 138 | draw_image = cv2.cvtColor(((self.previmage/2 + image/2)).astype(np.uint8), cv2.COLOR_GRAY2RGB) 139 | 140 | for _, t_idx in enumerate(self.tracked_feature_idx): 141 | a,b = self.features[t_idx].uv.astype(int).ravel() 142 | c,d = self.prevfeatures[t_idx].uv.astype(int).ravel() 143 | cv2.line(draw_image, (a,b),(c,d), (0,255,0), 1) 144 | cv2.circle(draw_image,(a,b),1, (0,0,255),-1) 145 | cv2.circle(draw_image,(c,d),1, (255,0,0),-1) 146 | 147 | return draw_image 148 | 149 | def drawMapPointReproj(self, T): 150 | map_pts, image_pts = self.get_map_points() 151 | map_pts_c = np.matmul(np.concatenate([map_pts, np.ones((len(map_pts), 1))], 1), T.T) 152 | map_pts_proj_c = np.matmul(np.divide(map_pts_c[:, :3], map_pts_c[:, 2].reshape(-1,1)), self.cam0.K_rect.T) 153 | 154 | draw_image = cv2.cvtColor(self.image.astype(np.uint8), cv2.COLOR_GRAY2RGB) 155 | 156 | for i in range(len(map_pts)): 157 | a = map_pts_proj_c[i, 0].astype(np.int32) 158 | b = map_pts_proj_c[i, 1].astype(np.int32) 159 | c = image_pts[i, 0].astype(np.int32) 160 | d = image_pts[i, 1].astype(np.int32) 161 | cv2.line(draw_image, (a,b),(c,d), (0,255,0), 1) 162 | cv2.circle(draw_image,(a,b),1, (0,255,255),-1) 163 | cv2.circle(draw_image,(c,d),1, (255,0,0),-1) 164 | 165 | def drawMapPoint(self, T): 166 | T = np.eye(4) 167 | map_pts, image_pts = self.get_map_points() 168 | map_pts_c = np.matmul(np.concatenate([map_pts, np.ones((len(map_pts), 1))], 1), T.T) 169 | map_pts_proj_c = np.matmul(np.divide(map_pts_c[:, :3], map_pts_c[:, 2].reshape(-1,1)), self.cam0.K_rect.T) 170 | 171 | draw_image = cv2.cvtColor(np.zeros_like(self.image, dtype=np.uint8), cv2.COLOR_GRAY2RGB) 172 | 173 | for i in range(len(map_pts)): 174 | a = map_pts_proj_c[i, 0].astype(np.int32) 175 | b = map_pts_proj_c[i, 1].astype(np.int32) 176 | cv2.circle(draw_image,(a,b),1, (0,0,255),-1) 177 | cv2.imshow("map points", draw_image) 178 | cv2.waitKey(1) 179 | 180 | def map_init(self, depth): 181 | for _, t_idx in enumerate(self.feature_idx): 182 | u, v = self.features[t_idx].uv.astype(int).ravel() 183 | id = self.features[t_idx].id 184 | if (depth[v,u] > 1e-1) & (depth[v,u] < 1e2): 185 | pts = np.matmul(self.cam0.K_rect_inv, np.array([u, v, 1]))*depth[v,u] 186 | self.map_pts[id].set_pts(pts) 187 | self.features[t_idx].is_reconstructed = True 188 | else: 189 | self.features[t_idx].is_reconstructed = False 190 | self.is_recon_init = True 191 | 192 | def map_update(self, depth, T): 193 | self.map_init(depth) 194 | 195 | def compute_camera_pose(self): 196 | map_pts, image_pts = self.get_map_points() 197 | success, r, t, inlier = cv2.solvePnPRansac(map_pts, image_pts, self.cam0.K_rect, np.zeros((5,1))) 198 | r = r.squeeze() 199 | t = t.squeeze() 200 | T = np.eye(4) 201 | T[:3, :3] = SO3(r) 202 | T[:3, 3] = t 203 | self.drawMapPointReproj(T) 204 | return T 205 | 206 | def get_map_points(self): 207 | # get map points 208 | map_id = [self.prevfeatures[idx].id for i, idx in enumerate(self.prevfeature_idx) 209 | if self.prevfeatures[idx].is_reconstructed & self.features[idx].is_tracked] 210 | map_pts = np.zeros((len(map_id), 3)) 211 | for i, id in enumerate(map_id): 212 | map_pts[i, :] = self.map_pts[id].pts 213 | # get image points 214 | image_idx = [idx for i, idx in enumerate(self.prevfeature_idx) 215 | if self.prevfeatures[idx].is_reconstructed & self.features[idx].is_tracked] 216 | image_pts = np.zeros((len(image_idx), 2)) 217 | for i, idx in enumerate(image_idx): 218 | image_pts[i, :] = self.features[idx].uv 219 | 220 | return map_pts, image_pts 221 | -------------------------------------------------------------------------------- /HSM/HSM.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | from signal import SIG_DFL 9 | from termios import VMIN 10 | import numpy as np 11 | import cv2 12 | 13 | import pycuda.autoinit 14 | import pycuda.driver as cuda 15 | from pycuda.compiler import SourceModule 16 | import time 17 | 18 | import copy 19 | 20 | from utils import * 21 | from metric import * 22 | from filter import * 23 | 24 | from Camera import Camera, CameraDSEC 25 | from FeatureTracker import FeatureTrackerKeyframeBased, FeatureTracker 26 | 27 | import cuda_source as cuda_source 28 | 29 | import math 30 | import scipy.io 31 | 32 | class HSM: 33 | def __init__(self, params): 34 | self.cam0 = params['cam0'] 35 | self.cam1 = params['cam1'] 36 | self.loader = params['loader'] 37 | self.generate_kernel() 38 | 39 | self.feature_tracker = FeatureTracker(params) 40 | 41 | # DISPARITY PARAMETERS 42 | self.min_disparity = -params['disparity_range'] 43 | self.max_disparity = 0 44 | self.ncc_gaussian_std = params['ncc_gaussian_std'] 45 | self.ncc_AEI_gaussian_std = 3 46 | self.focal = (self.cam0.K_rect[0][0] + self.cam0.K_rect[1][1])/2 47 | self.f_b = self.cam0.baseline * self.focal 48 | self.disp_range = np.abs(np.linspace(self.min_disparity, self.max_disparity, self.max_disparity-self.min_disparity+1)) 49 | self.depth_range = (self.f_b / (self.disp_range + 1e-9)).astype(np.float32) 50 | self.pi_v_gap = params['msd_gap'] 51 | self.kernel_radius = params['kernel_radius'] 52 | self.filter_radius = np.min([self.ncc_AEI_gaussian_std, self.kernel_radius]) 53 | # self.filter_radius = 1 54 | 55 | self.disparity = np.zeros(Camera.resolution, dtype=np.float32) 56 | self.disparity_init = np.zeros(Camera.resolution, dtype=np.float32) 57 | self.disparity_AEI = np.zeros(Camera.resolution, dtype=np.float32) 58 | 59 | ### CUDA PARAMETERS 60 | kernel_radius = self.kernel_radius 61 | self.cuda_EBS = (1024, 1, 1) # gpu event block size 62 | self.cuda_EGS = (8096, 1)# gpu event grid size | max = 65535 63 | self.cuda_SBS = (32, 16, 1) # gpu event block size_x 64 | self.cuda_SGS = (int(np.ceil(Camera.width/self.cuda_SBS[0])), 65 | int(np.ceil(Camera.height/self.cuda_SBS[1]))) 66 | 67 | cuda_code = cuda_source.template.substitute(WIDTH = Camera.width, 68 | HEIGHT = Camera.height, 69 | BLOCKDIM_X = self.cuda_SBS[0], 70 | BLOCKDIM_Y = self.cuda_SBS[1], 71 | MIN_DISP = self.min_disparity, 72 | MAX_DISP = self.max_disparity, 73 | RAD = self.kernel_radius, 74 | FILTER_RAD = self.filter_radius, 75 | FX = Camera.K_rect[0,0], 76 | FY = Camera.K_rect[1,1], 77 | CX = Camera.K_rect[0,2], 78 | CY = Camera.K_rect[1,2], 79 | FxB = self.f_b) 80 | self.cuda_module = SourceModule(cuda_code) 81 | self.event_projection = self.cuda_module.get_function('event_projection') 82 | self.clear_event_image = self.cuda_module.get_function('clear_event_image') 83 | self.clear_AEI = self.cuda_module.get_function('clear_AEI') 84 | self.compute_AEI = self.cuda_module.get_function('compute_AEI') 85 | self.stereo_ncc = self.cuda_module.get_function('stereo_ncc') 86 | self.stereo_ncc_AEI = self.cuda_module.get_function('stereo_ncc_AEI') 87 | self.stereo_postproc = self.cuda_module.get_function('stereo_postproc') 88 | self.stereo_postproc_AEI = self.cuda_module.get_function('stereo_postproc_AEI') 89 | self.densify_sparse_disp = self.cuda_module.get_function('densify_sparse_disp') 90 | 91 | 92 | gaussian1d = np.zeros((2*kernel_radius+1, 1)) 93 | for i in range(2 * kernel_radius + 1): 94 | x = i - kernel_radius 95 | gaussian1d[i] = np.exp(-(x * x) / (4*kernel_radius)) 96 | self.gaussian2d = gaussuian_filter(kernel_radius, self.ncc_gaussian_std) 97 | self.gaussian2d_AEI = gaussuian_filter(kernel_radius, self.ncc_AEI_gaussian_std) 98 | self.gaussian2d_gpu = cuda.In(self.gaussian2d.astype(np.float32)/np.max(self.gaussian2d.astype(np.float32))) 99 | self.gaussian2d_AEI_gpu = cuda.In(self.gaussian2d_AEI.astype(np.float32)) 100 | 101 | self.tex2D_left = self.cuda_module.get_texref("tex2D_left") 102 | self.tex2D_right = self.cuda_module.get_texref("tex2D_right") 103 | self.tex2D_left_edge = self.cuda_module.get_texref("tex2D_left_edge") 104 | 105 | self.zero_int32 = np.zeros(Camera.resolution, dtype=np.int32) 106 | self.zero_float32 = np.zeros(Camera.resolution, dtype=np.float32) 107 | self.zero_volume_float32 = np.zeros((self.max_disparity - self.min_disparity + 1,) + Camera.resolution, dtype=np.float32) 108 | 109 | self.disparity_AEI_gpu = cuda.mem_alloc(self.zero_float32.nbytes) 110 | self.disparity_temp_gpu = cuda.mem_alloc(self.zero_float32.nbytes) 111 | self.disparity_gpu = cuda.mem_alloc(self.zero_float32.nbytes) 112 | self.cost_gpu = cuda.mem_alloc(self.zero_volume_float32.nbytes) 113 | self.cost_AEI_gpu = cuda.mem_alloc(self.zero_volume_float32.nbytes) 114 | self.AEI_gpu = cuda.mem_alloc(self.zero_volume_float32.nbytes) 115 | 116 | # ITERATOR 117 | self.iter = -1 118 | # CAMERA POSE 119 | self.T_hist = {} 120 | self.image_diag_len = np.linalg.norm(Camera.resolution) 121 | # EVAL DATA 122 | self.eval_data = dict() 123 | self.eval_data['gt'] = dict() 124 | self.log_data = dict() 125 | 126 | # FLAGS 127 | self.ts_first = 0 128 | self.ts_event_first = 0 129 | self.is_first_process = True 130 | self.is_dIdt_init = False 131 | self.is_init = False 132 | self.is_time_estimation_mode = params['time_estimation'] 133 | self.show_disparity_inlier = params['show_disparity_inlier'] 134 | self.show_disparity = params['show_disparity'] 135 | self.semi_dense = params['semi_dense'] 136 | 137 | 138 | def __del__(self): 139 | self.disparity_AEI_gpu.free() 140 | self.disparity_temp_gpu.free() 141 | self.disparity_gpu.free() 142 | self.cost_gpu.free() 143 | self.cost_AEI_gpu.free() 144 | self.AEI_gpu.free() 145 | 146 | def process(self, image, image_ts, event): 147 | self.iter += 1 148 | image_ts = image_ts - self.ts_first + self.ts_event_first 149 | self.preprocess(image, image_ts, event) 150 | if self.is_dIdt_init: 151 | if not self.is_init: 152 | self.compute_disparity_ncc_init() 153 | self.T_hist[self.iter] = np.eye(4) 154 | self.is_init = True 155 | else: 156 | try: 157 | self.find_pose_from_pts3d() 158 | except: 159 | self.is_init = False 160 | return 161 | 162 | # self.compute_disparity_ncc_init() 163 | if self.is_time_estimation_mode: 164 | self.compute_disparity_ncc_time() 165 | else: 166 | self.compute_disparity_ncc() 167 | 168 | self.visualize() 169 | 170 | def preprocess(self, image, image_ts, event): 171 | if self.is_first_process: 172 | self.dt = 0 173 | self.ts_first = image_ts 174 | self.ts_event_first = event[-1, 2] 175 | self.image_prev = np.zeros(Camera.resolution) 176 | self.is_first_process = False 177 | self.ts = np.max(event[:, 2]) 178 | image_ts = np.max(event[:, 2]) 179 | else: 180 | self.dt = image_ts - self.ts 181 | self.image_prev = self.image 182 | self.is_dIdt_init = True 183 | self.ts = image_ts 184 | 185 | self.rectify(image, image_ts, event) 186 | self.compute_gradient() 187 | self.extract_features() 188 | self.compute_event_image() 189 | 190 | def rectify(self, image, image_ts, event): 191 | event[:, 2] = image_ts - event[:, 2] 192 | 193 | event = event[::1,:] 194 | self.image = self.cam0.rectify(image) 195 | self.event_raw = event.astype(np.float32) 196 | self.event_raw_cuda = cuda.In(self.event_raw) 197 | self.event = self.cam1.rectify_events(self.event_raw) 198 | self.event_cuda = cuda.In(self.event) 199 | 200 | def compute_gradient(self): 201 | self.dIdt = self.image.astype(np.float32) - self.image_prev.astype(np.float32) 202 | self.image = np.array(self.image).astype(np.float32) 203 | dIdu = cv2.filter2D(self.image, -1, self.kernel_du) 204 | dIdv = cv2.filter2D(self.image, -1, self.kernel_dv) 205 | self.dIdx = np.sqrt(np.array(dIdu**2 + dIdv**2)) 206 | 207 | self.dIdt_sigmoid = (1.0/(1 + np.exp(-cv2.GaussianBlur(self.dIdt.astype(np.float32)/255, (5, 5), 1)))-0.5) 208 | self.dIdx_sigmoid = (1.0/(1 + np.exp(-cv2.GaussianBlur(self.dIdx.astype(np.float32)/255, (5, 5), 1)))-0.5) 209 | cuda.matrix_to_texref(self.dIdx_sigmoid, self.tex2D_left_edge, order="C") 210 | 211 | def extract_features(self): 212 | self.feature_tracker.detect_feature(self.image) 213 | 214 | def compute_event_image(self): 215 | result_gpu = cuda.mem_alloc(self.zero_int32.nbytes) 216 | self.clear_event_image(result_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 217 | self.event_projection(result_gpu, self.event_raw_cuda, np.uint32(self.event.shape[0]), block=self.cuda_EBS, grid=self.cuda_EGS) 218 | image = cuda.from_device_like(result_gpu, self.zero_int32) 219 | 220 | self.event_image = self.cam1.rectify(image) 221 | self.event_sigmoid = 1./(1 + np.exp(-self.event_image.astype(np.float32)*0.5))-0.5 222 | result_gpu.free() 223 | return 224 | 225 | def compute_disparity_ncc_init(self): 226 | cuda.matrix_to_texref(self.dIdt_sigmoid, self.tex2D_left, order="C") 227 | cuda.matrix_to_texref(self.event_sigmoid, self.tex2D_right, order="C") 228 | self.stereo_ncc(self.disparity_temp_gpu, self.cost_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 229 | self.stereo_postproc(self.disparity_gpu, self.cost_gpu, self.gaussian2d_gpu, block=(self.cuda_SBS), grid=self.cuda_SGS) 230 | cuda.memcpy_dtoh(self.disparity, self.disparity_gpu) 231 | 232 | self.depth = self.f_b / (self.disparity + 1e-9) 233 | self.feature_tracker.map_init(self.depth) 234 | self.disparity_init = copy.deepcopy(self.disparity) 235 | 236 | def compute_disparity_ncc(self): 237 | xi = InvSE3(self.T_hist[self.iter]).astype(np.float32)/self.dt 238 | depth_msd, AEI_idx = self.compute_msd(xi) 239 | depth_msd_gpu = cuda.In(depth_msd.astype(np.float32)) 240 | self.AEI_idx_gpu = cuda.In(AEI_idx.astype(np.int32)) 241 | 242 | # compute aligned event images (AEI) 243 | # set memory 244 | cuda.matrix_to_texref(self.dIdx_sigmoid, self.tex2D_left_edge, order="C") 245 | cuda.matrix_to_texref(self.dIdt_sigmoid, self.tex2D_left, order="C") 246 | cuda.matrix_to_texref(self.event_sigmoid, self.tex2D_right, order="C") 247 | 248 | # CUDA functions 249 | self.clear_AEI(self.AEI_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 250 | self.compute_AEI(self.AEI_gpu, self.event_cuda, depth_msd_gpu, 251 | np.int32(self.event.shape[0]), np.int32(depth_msd.shape[0]), 252 | cuda.In(xi), block=self.cuda_EBS, grid=self.cuda_EGS) 253 | self.stereo_ncc_AEI(self.disparity_AEI_gpu, self.cost_AEI_gpu, self.AEI_gpu, self.AEI_idx_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 254 | self.stereo_ncc(self.disparity_temp_gpu, self.cost_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 255 | self.stereo_postproc_AEI(self.disparity_gpu, self.cost_gpu, self.cost_AEI_gpu, 256 | self.gaussian2d_AEI_gpu, block=(self.cuda_SBS), grid=self.cuda_SGS) 257 | # copy cuda memory 258 | cuda.memcpy_dtoh(self.disparity, self.disparity_gpu) 259 | cuda.memcpy_dtoh(self.disparity_init, self.disparity_temp_gpu) 260 | cuda.memcpy_dtoh(self.disparity_AEI, self.disparity_AEI_gpu) 261 | self.depth = self.f_b / (self.disparity + 1e-9) 262 | self.feature_tracker.map_update(self.depth, self.T_hist[self.iter]) 263 | return 264 | 265 | def find_pose_from_pts3d(self): 266 | T = self.feature_tracker.compute_camera_pose() 267 | self.T_hist[self.iter] = T 268 | return 269 | 270 | def compute_msd(self, xi): 271 | v_dt = xi[:3]*self.dt + 0.5*np.matmul(hat(xi[3:]), xi[:3])*self.dt**2 272 | v_xy = np.linalg.norm(v_dt[:2]) 273 | v_z = np.abs(v_dt[2]) 274 | pi_v = (self.focal * v_xy + self.image_diag_len * v_z) / self.depth_range 275 | round_pi_v = np.ceil((pi_v)/self.pi_v_gap)*self.pi_v_gap 276 | _, pi_v_idx, AEI_idx = np.unique(round_pi_v,return_index=True,return_inverse=True) 277 | depth_msd = self.depth_range[pi_v_idx] 278 | return depth_msd, AEI_idx 279 | 280 | def visualize(self): 281 | if self.show_disparity: 282 | if self.semi_dense: 283 | disp_color = cv2.applyColorMap((self.dIdx_sigmoid**2 > 1e-4)*(self.disparity/(self.max_disparity - self.min_disparity)*255).astype(np.uint8), cv2.COLORMAP_JET) 284 | cv2.imshow("disparity", disp_color) 285 | else: 286 | disp_color = cv2.applyColorMap((self.disparity/(self.max_disparity - self.min_disparity)*255).astype(np.uint8), cv2.COLORMAP_JET) 287 | cv2.imshow("disparity", disp_color) 288 | cv2.waitKey(1) 289 | 290 | def generate_kernel(self): 291 | self.kernel_du = np.array([[0.0, 0, 0], [-1, 0, 1], [0, 0, 0]]) 292 | self.kernel_dv = np.transpose(self.kernel_du) 293 | return 294 | 295 | def evaluate(self, idx): 296 | success, _, image_disparity, _ = self.loader.get_debug_data(idx) 297 | 298 | if not success: 299 | return 300 | 301 | gt_disparity = image_disparity 302 | result_gpu = cuda.mem_alloc(self.zero_float32.nbytes) 303 | sparse_gpu = cuda.mem_alloc(self.zero_float32.nbytes) 304 | cuda.memcpy_htod(result_gpu, self.zero_int32) 305 | cuda.memcpy_htod(sparse_gpu, gt_disparity) 306 | self.densify_sparse_disp(result_gpu, sparse_gpu, self.gaussian2d_AEI_gpu, block=(self.cuda_SBS), grid=self.cuda_SGS) 307 | gt_disparity_dense = cuda.from_device_like(result_gpu, self.zero_float32) 308 | disparity_gt_edge = (self.dIdx_sigmoid**2 > 4e-4) * (gt_disparity>0) * gt_disparity_dense 309 | gt_disparity = (self.dIdx_sigmoid**2 > 1e-4) * gt_disparity_dense 310 | self.disparity_gt = gt_disparity_dense 311 | 312 | if self.show_disparity_inlier: 313 | err_thres = 10 314 | disp_inlier = (self.disparity > 0) * (np.abs(gt_disparity_dense - self.disparity)< err_thres) * (gt_disparity>0) 315 | disp_color = cv2.applyColorMap(disp_inlier * (self.disparity/(self.max_disparity - self.min_disparity)*255).astype(np.uint8), cv2.COLORMAP_JET) 316 | cv2.imshow("disparity_inlier", disp_color) 317 | cv2.waitKey(1) 318 | 319 | self.metric('init', self.disparity_init, disparity_gt_edge, 3) 320 | self.metric('proposed', self.disparity, disparity_gt_edge, 3) 321 | return 322 | 323 | def compute_disparity_ncc_time(self): 324 | repeat = 10 325 | self.repeat = repeat 326 | st_all = time.time() 327 | for i in range(0, repeat): 328 | st = time.time() 329 | self.clear_AEI(self.AEI_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 330 | self.logger('clear_AEI', time.time() - st) 331 | 332 | for i in range(0, repeat): 333 | st = time.time() 334 | xi = InvSE3(self.T_hist[self.iter]).astype(np.float32)/self.dt 335 | depth_msd, AEI_idx = self.compute_msd(xi) 336 | depth_msd_gpu = cuda.In(depth_msd.astype(np.float32)) 337 | AEI_idx_gpu = cuda.In(AEI_idx.astype(np.int32)) 338 | self.logger('compute MSD', time.time() - st) 339 | 340 | for i in range(0, repeat): 341 | st = time.time() 342 | cuda.matrix_to_texref(self.dIdx_sigmoid, self.tex2D_left_edge, order="C") 343 | cuda.matrix_to_texref(self.dIdt_sigmoid, self.tex2D_left, order="C") 344 | cuda.matrix_to_texref(self.event_sigmoid, self.tex2D_right, order="C") 345 | self.logger('copy texture', time.time() - st) 346 | 347 | for i in range(0, repeat): 348 | st = time.time() 349 | self.compute_AEI(self.AEI_gpu, self.event_cuda, depth_msd_gpu, np.int32(self.event.shape[0]), 350 | np.int32(depth_msd.shape[0]), cuda.In(xi), block=self.cuda_EBS, grid=self.cuda_EGS) 351 | self.logger('compute AEI', time.time() - st) 352 | 353 | for i in range(0, repeat): 354 | st = time.time() 355 | self.stereo_ncc(self.disparity_temp_gpu, self.cost_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 356 | cuda.memcpy_dtoh(self.disparity_init, self.disparity_temp_gpu) 357 | self.logger('stereo NCC', time.time() - st) 358 | 359 | for i in range(0, repeat): 360 | st = time.time() 361 | self.stereo_ncc_AEI(self.disparity_AEI_gpu, self.cost_AEI_gpu, self.AEI_gpu, AEI_idx_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 362 | cuda.memcpy_dtoh(self.disparity_AEI, self.disparity_AEI_gpu) 363 | self.logger('stereo NCC AEI', time.time() - st) 364 | 365 | for i in range(0, repeat): 366 | st = time.time() 367 | self.stereo_postproc_AEI(self.disparity_gpu, self.cost_gpu, self.cost_AEI_gpu, self.gaussian2d_AEI_gpu, block=self.cuda_SBS, grid=self.cuda_SGS) 368 | cuda.memcpy_dtoh(self.disparity, self.disparity_gpu) 369 | self.logger('stereo postprocessing', time.time() - st) 370 | 371 | for i in range(0, repeat): 372 | st = time.time() 373 | cuda.memcpy_dtoh(self.disparity, self.disparity_gpu) 374 | self.logger('copy device to host', time.time() - st) 375 | 376 | self.logger('total time', time.time() - st_all) 377 | self.logger('the number of event', self.event.shape[0]) 378 | 379 | self.depth = self.f_b / (self.disparity + 1e-9) 380 | self.feature_tracker.map_update(self.depth, self.T_hist[self.iter]) 381 | 382 | def metric(self, method, disparity, disparity_gt, err_thres): 383 | depth = self.f_b/(disparity + 1e-9) 384 | depth_gt = self.f_b/(disparity_gt + 1e-9) 385 | inlier_idx = np.where((np.abs(disparity_gt - disparity) < err_thres) * (disparity_gt > 0)) 386 | all_idx = np.where((disparity_gt > 0) * (disparity_gt > 0)) 387 | gt_idx = np.where(disparity_gt > 0) 388 | 389 | if not method in self.eval_data.keys(): 390 | self.eval_data[method] = dict() 391 | 392 | if len(self.eval_data[method].keys()) == 0: 393 | self.eval_data[method]['disparity'] = disparity[inlier_idx] 394 | self.eval_data[method]['disparity_all'] = disparity[all_idx] 395 | self.eval_data[method]['depth'] = depth[inlier_idx] 396 | self.eval_data[method]['gt'] = dict() 397 | else: 398 | self.eval_data[method]['disparity'] = np.append(self.eval_data[method]['disparity'], disparity[inlier_idx]) 399 | self.eval_data[method]['disparity_all'] = np.append(self.eval_data[method]['disparity_all'], disparity[all_idx]) 400 | self.eval_data[method]['depth'] = np.append(self.eval_data[method]['depth'], depth[inlier_idx]) 401 | 402 | if len(self.eval_data[method]['gt'].keys()) == 0: 403 | self.eval_data[method]['gt']['disparity'] = disparity_gt[inlier_idx] 404 | self.eval_data[method]['gt']['disparity_all'] = disparity_gt[all_idx] 405 | self.eval_data[method]['gt']['depth'] = depth_gt[inlier_idx] 406 | self.eval_data[method]['gt']['depth_valid_num'] = depth_gt[gt_idx].shape[0] 407 | 408 | else: 409 | self.eval_data[method]['gt']['disparity'] = np.append(self.eval_data[method]['gt']['disparity'], disparity_gt[inlier_idx]) 410 | self.eval_data[method]['gt']['disparity_all'] = np.append(self.eval_data[method]['gt']['disparity_all'], disparity_gt[all_idx]) 411 | self.eval_data[method]['gt']['depth'] = np.append(self.eval_data[method]['gt']['depth'], depth_gt[inlier_idx]) 412 | self.eval_data[method]['gt']['depth_valid_num'] += depth_gt[gt_idx].shape[0] 413 | 414 | def metric_print(self, method): 415 | try: 416 | d_data = self.eval_data[method]['disparity'] 417 | d_data_all = self.eval_data[method]['disparity_all'] 418 | z_data = self.eval_data[method]['depth'] 419 | d_gt = self.eval_data[method]['gt']['disparity'] 420 | d_gt_all = self.eval_data[method]['gt']['disparity_all'] 421 | z_gt = self.eval_data[method]['gt']['depth'] 422 | valid_gt = self.eval_data[method]['gt']['depth_valid_num'] 423 | 424 | error = dict() 425 | error['RMSE'] = RMSE(d_data, d_gt) 426 | error['MAE'] = MAE(d_data, d_gt) 427 | error['DTdelta'] = DTdelta(d_data, d_gt, valid_gt) 428 | error['RTdelta'] = RTdelta(d_data_all, d_gt_all) 429 | error['depth_RMSE'] = RMSE(z_data, z_gt) 430 | error['depth_ARD'] = ARD(z_data, z_gt) 431 | error['depth_ATdelta'] = ATdelta(z_data, z_gt, valid_gt) 432 | 433 | print('Method: {0}\nEval data num: {2}\nErros: {1}\n'.format(method, error, len(self.eval_data[method]['gt']['depth']))) 434 | except: 435 | print('Method {0} print error occured'.format(method)) 436 | 437 | def logger(self, method, time): 438 | if not method in self.log_data.keys(): 439 | self.log_data[method] = np.zeros((10000, 1)) 440 | self.log_data[method][self.iter] += time 441 | 442 | def time_print(self): 443 | if self.is_time_estimation_mode: 444 | avg = dict() 445 | for key in self.log_data.keys(): 446 | log = self.log_data[key][np.where(self.log_data[key]!=0)[0]] 447 | avg[key] = np.mean(log) 448 | if not (key == 'the number of event'): 449 | avg[key] *= (1e3 / self.repeat) 450 | 451 | print(avg) 452 | -------------------------------------------------------------------------------- /HSM/config/config.yaml: -------------------------------------------------------------------------------- 1 | # Feature tracker 2 | feature num: 1000 3 | track_err_thres: 10 4 | track_win_size: 5 | - 21 6 | - 21 7 | extract_win_size: 12 8 | 9 | # Disparity estimator 10 | disparity_range: 100 11 | kernel_radius: 12 12 | ncc_gaussian_std: 2 13 | msd_gap: 10 14 | 15 | # Time estimation mode 16 | time_estimation: True 17 | 18 | # Visualize 19 | # show disparity inlier requires ground truth disparity 20 | show_disparity_inlier: True 21 | show_disparity: True 22 | semi_dense: True 23 | 24 | -------------------------------------------------------------------------------- /HSM/cuda/source.cu: -------------------------------------------------------------------------------- 1 | // edit: Haram Kim 2 | // email: rlgkfka614@gmail.com 3 | // github: https://github.com/haram-kim 4 | // homepage: https://haram-kim.github.io/ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #define WIDTH $WIDTH 11 | #define HEIGHT $HEIGHT 12 | #define blockDim_x $BLOCKDIM_X 13 | #define blockDim_y $BLOCKDIM_Y 14 | #define MIN_DISP $MIN_DISP 15 | #define MAX_DISP $MAX_DISP 16 | #define RAD $RAD 17 | #define FILTER_RAD $FILTER_RAD 18 | 19 | #define fx $FX 20 | #define fy $FY 21 | #define cx $CX 22 | #define cy $CY 23 | #define FxB $FxB 24 | 25 | #define EDGE_FLAG false 26 | 27 | #define STEPS 3 28 | 29 | #define MAX(a, b) (((a) < (b)) ? (b) : (a)) 30 | #define MIN(a, b) (((a) > (b)) ? (b) : (a)) 31 | 32 | texture tex2D_left; 33 | texture tex2D_right; 34 | texture tex2D_left_edge; 35 | struct Point3d{ 36 | float x; 37 | float y; 38 | float z; 39 | 40 | __device__ inline Point3d operator+(const Point3d &a){ 41 | return {a.x + x, a.y + y, a.z + z}; 42 | } 43 | __device__ inline Point3d operator*(const float &a){ 44 | return {a * x, a * y, a * z}; 45 | } 46 | __device__ inline Point3d operator/(const float &a){ 47 | return {x / a, y /a, z / a}; 48 | } 49 | }; 50 | 51 | struct Point2d{ 52 | float u; 53 | float v; 54 | }; 55 | 56 | __device__ inline int get_cost_idx(const int &x, const int &y, const int &d){ 57 | return x + y*WIDTH + (d-MIN_DISP)*WIDTH*HEIGHT; 58 | } 59 | 60 | __device__ inline int get_idx(const int &x, const int &y, const int &d){ 61 | return x + y*WIDTH + d*WIDTH*HEIGHT; 62 | } 63 | 64 | __device__ Point3d cross(const Point3d &a, const Point3d &b){ 65 | Point3d pts; 66 | pts.x = -a.z*b.y + a.y*b.z; 67 | pts.y = a.z*b.x - a.x*b.z; 68 | pts.z = -a.y*b.x + a.x+b.y; 69 | return pts; 70 | } 71 | 72 | __device__ Point3d inverse_projection(const float &u, const float &v){ 73 | float x = (u - cx) / fx; 74 | float y = (v - cy) / fy; 75 | Point3d pts; 76 | pts.x = x; 77 | pts.y = y; 78 | pts.z = 1; 79 | return pts; 80 | } 81 | 82 | __device__ Point2d projection(const Point3d &pts){ 83 | float x = pts.x / pts.z * fx + cx; 84 | float y = pts.y / pts.z * fy + cy; 85 | Point2d uv; 86 | uv.u = x; 87 | uv.v = y; 88 | return uv; 89 | } 90 | 91 | __device__ inline Point3d rotation(Point3d &x, float &dt, Point3d &w){ 92 | return x + cross(w, x)*dt + cross(w, cross(w, x))*0.5*dt*dt; 93 | } 94 | 95 | __device__ inline Point3d translation(Point3d &x, float &dt, float &depth, 96 | Point3d &w, Point3d &v){ 97 | if(depth > 1e5){ 98 | return x; 99 | } 100 | else{ 101 | Point3d v_dt = v*dt + cross(w, v)*0.5*dt*dt; 102 | return x + v_dt * (x.z / (depth - v_dt.z)); 103 | } 104 | } 105 | 106 | __device__ Point3d warping(float &u, float &v, 107 | float &dt, float &depth, 108 | Point3d &v_, Point3d &w_){ 109 | Point3d x = inverse_projection(u, v); 110 | Point3d x_rot_warped = rotation(x, dt, w_); 111 | Point3d x_warped = translation(x_rot_warped, dt, depth, w_, v_); 112 | Point2d x_proj = projection(x_warped); 113 | int u_proj = int(x_proj.u + 0.5); 114 | int v_proj = int(x_proj.v + 0.5); 115 | Point3d result; 116 | result.x = u_proj; 117 | result.y = v_proj; 118 | result.z = MAX(MIN(- FxB / x_warped.z, MAX_DISP), MIN_DISP); 119 | return result; 120 | } 121 | 122 | __device__ void timer(char *s, const int tidx, const int tidy, int d, clock_t start){ 123 | if(tidx == 100 && tidy == 100 && d == 0){ 124 | printf("%f: %s \n", double( clock () - start ) / CLOCKS_PER_SEC, s); 125 | } 126 | } 127 | 128 | __device__ inline int trun_idx(const int &idx, const int &max){ 129 | return MAX(MIN(idx, max - 1), 0); 130 | } 131 | 132 | __global__ void clear_event_image(int *event_image){ 133 | const unsigned int tidx = blockDim.x * blockIdx.x + threadIdx.x; 134 | const unsigned int tidy = blockDim.y * blockIdx.y + threadIdx.y; 135 | const unsigned int tid = tidx + tidy * WIDTH; 136 | if(tid < WIDTH*HEIGHT){ 137 | event_image[tid] = 0; 138 | } 139 | } 140 | 141 | __global__ void clear_AEI(float *AEI){ 142 | const unsigned int tidx = blockDim.x * blockIdx.x + threadIdx.x; 143 | const unsigned int tidy = blockDim.y * blockIdx.y + threadIdx.y; 144 | for (int d = MIN_DISP; d <= MAX_DISP + 1; d++){ 145 | int tid = get_cost_idx(tidx, tidy, d); 146 | if(tid < WIDTH*HEIGHT*(MAX_DISP-MIN_DISP + 1)){ 147 | AEI[tid] = 0; 148 | } 149 | } 150 | } 151 | 152 | __global__ void event_projection(int *event_image, float *event, unsigned int size) 153 | { 154 | // 1D grid of 1D blocks 155 | const unsigned int stride = blockDim.x * gridDim.x; 156 | unsigned int i = blockDim.x * blockIdx.x + threadIdx.x; 157 | 158 | while(i < size){ 159 | int u = int(event[0 + i*4] + 0.5); 160 | int v = int(event[1 + i*4] + 0.5); 161 | float t = event[2 + i*4]; 162 | float p = event[3 + i*4]; 163 | if(u >= 0 && u < WIDTH && v >= 0 && v < HEIGHT){ 164 | atomicAdd(&event_image[u + v * WIDTH], 2*p-1); 165 | } 166 | i += stride; 167 | } 168 | } 169 | 170 | // compute aligned event image 171 | __global__ void compute_AEI(float *AEI, float *event, float *depth_msd, int size, int msd_size, float *xi) 172 | { 173 | // 1D grid of 1D blocks 174 | const unsigned int stride = blockDim.x * gridDim.x; 175 | unsigned int i = blockDim.x * blockIdx.x + threadIdx.x; 176 | Point3d v_; 177 | v_.x = xi[0]; 178 | v_.y = xi[1]; 179 | v_.z = xi[2]; 180 | Point3d w_; 181 | w_.x = xi[3]; 182 | w_.y = xi[4]; 183 | w_.z = xi[5]; 184 | 185 | while(i < size){ 186 | float u = event[0 + i*4]; 187 | float v = event[1 + i*4]; 188 | float dt = event[2 + i*4]; 189 | // float p = event[3 + i*4]; 190 | 191 | Point3d x = inverse_projection(u, v); 192 | Point3d x_rot_warped = rotation(x, dt, w_); 193 | #pragma unroll 194 | for(int msd_idx = 0; msd_idx < msd_size; msd_idx++){ 195 | float depth = depth_msd[msd_idx]; 196 | Point3d x_warped = translation(x_rot_warped, dt, depth, w_, v_); 197 | Point2d x_proj = projection(x_warped); 198 | const int u_proj = int(x_proj.u + 0.5); 199 | const int v_proj = int(x_proj.v + 0.5); 200 | if(u_proj >= 0 && u_proj < WIDTH && v_proj >= 0 && v_proj < HEIGHT){ 201 | atomicAdd(&AEI[get_idx(u_proj, v_proj, msd_idx)], 1); 202 | } 203 | } 204 | i += stride; 205 | } 206 | } 207 | 208 | __global__ void stereo_ncc(float *disparity_gpu, float *cost_volume) 209 | { 210 | // set thread id 211 | const int tidx = blockDim.x * blockIdx.x + threadIdx.x; 212 | const int tidy = blockDim.y * blockIdx.y + threadIdx.y; 213 | const int sidx = threadIdx.x + RAD; 214 | const int sidy = threadIdx.y + RAD; 215 | 216 | // initialization 217 | const float kernel_area = (2 * RAD + 1)*(2 * RAD + 1); 218 | float ncc_cost[MAX_DISP - MIN_DISP + 1]; 219 | 220 | float imLeft; 221 | float imRight; 222 | float I1I2_sum, I1_sq_sum, I2_sq_sum, I1_sum, I2_sum; 223 | float I1I2_mean, I1_sq_mean, I2_sq_mean, I1_mean, I2_mean, ncc, ncc_nom; 224 | float best_ncc = -1.0; 225 | float bestDisparity = 0; 226 | __shared__ float I1I2[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 227 | __shared__ float I1_sq[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 228 | __shared__ float I2_sq[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 229 | __shared__ float I1[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 230 | __shared__ float I2[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 231 | 232 | float imLeftA[STEPS]; 233 | float imLeftB[STEPS]; 234 | 235 | if (tidy >= HEIGHT || tidy < 0 || tidx >= WIDTH || tidx < 0){ 236 | return; 237 | } 238 | else{ 239 | disparity_gpu[tidy * WIDTH + tidx] = 0; 240 | } 241 | // compute for left images 242 | // copy texture data considering image padding 243 | for (int i = 0; i < STEPS; i++) 244 | { 245 | int offset = -RAD + i * RAD; 246 | imLeftA[i] = tex2D(tex2D_left, tidx - RAD, tidy + offset); 247 | imLeftB[i] = tex2D(tex2D_left, MIN(tidx - RAD + blockDim.x, WIDTH - 1), tidy + offset); 248 | } 249 | // copy texture to shard memory 250 | #pragma unroll 251 | for (int i = 0; i < STEPS; i++) 252 | { 253 | int offset = -RAD + i * RAD; 254 | int sidy_offset = sidy + offset; 255 | int sidx_offset = sidx - RAD; 256 | imLeft = imLeftA[i]; 257 | I1_sq[sidy_offset][sidx_offset] = imLeft * imLeft; 258 | I1[sidy_offset][sidx_offset] = imLeft; 259 | } 260 | 261 | // copy texture to shard memory for image padding 262 | #pragma unroll 263 | for (int i = 0; i < STEPS; i++) 264 | { 265 | int offset = -RAD + i * RAD; 266 | int sidy_offset = sidy + offset; 267 | int sidx_offset = sidx - RAD + blockDim.x; 268 | if (threadIdx.x < 2 * RAD) 269 | { 270 | imLeft = imLeftB[i]; 271 | I1_sq[sidy_offset][sidx_offset] = imLeft * imLeft; 272 | I1[sidy_offset][sidx_offset] = imLeft; 273 | } 274 | } 275 | __syncthreads(); 276 | // preprocessing for NCC 277 | I1_sq_sum = 0; 278 | I1_sum = 0; 279 | #pragma unroll 280 | for (int i = -RAD; i <= RAD; i++){ 281 | I1_sq_sum += I1_sq[sidy][sidx + i]; 282 | I1_sum += I1[sidy][sidx + i]; 283 | } 284 | __syncthreads(); 285 | I1_sq[sidy][sidx] = I1_sq_sum; 286 | I1[sidy][sidx] = I1_sum; 287 | __syncthreads(); 288 | if (threadIdx.y < RAD){ 289 | int sidy_offset = sidy - RAD ; 290 | I1_sq_sum = 0; 291 | I1_sum = 0; 292 | for (int i = -RAD; i <= RAD; i++){ 293 | I1_sq_sum += I1_sq[sidy_offset][sidx + i]; 294 | I1_sum += I1[sidy_offset][sidx + i]; 295 | } 296 | I1_sq[sidy_offset][sidx] = I1_sq_sum; 297 | I1[sidy_offset][sidx] = I1_sum; 298 | 299 | } 300 | if (sidy >= blockDim_y){ 301 | int sidy_offset = sidy + RAD; 302 | I1_sq_sum = 0; 303 | I1_sum = 0; 304 | for (int i = -RAD; i <= RAD; i++){ 305 | I1_sq_sum += I1_sq[sidy_offset][sidx + i]; 306 | I1_sum += I1[sidy_offset][sidx + i]; 307 | } 308 | I1_sq[sidy_offset][sidx] = I1_sq_sum; 309 | I1[sidy_offset][sidx] = I1_sum; 310 | } 311 | __syncthreads(); 312 | I1_sq_sum = 0; 313 | I1_sum = 0; 314 | #pragma unroll 315 | for (int i = -RAD; i <= RAD; i++) 316 | { 317 | I1_sq_sum += I1_sq[sidy + i][sidx]; 318 | I1_sum += I1[sidy + i][sidx]; 319 | } 320 | I1_sq_mean = I1_sq_sum / kernel_area; 321 | I1_mean = I1_sum / kernel_area; 322 | 323 | // compute for right images 324 | #pragma unroll 325 | for (int d = MIN_DISP; d <= MAX_DISP; d++) 326 | { 327 | // copy texture 328 | #pragma unroll 329 | for (int i = 0; i < STEPS; i++) 330 | { 331 | int offset = -RAD + i * RAD; 332 | int sidy_offset = sidy + offset; 333 | int sidx_offset = sidx - RAD; 334 | imLeft = imLeftA[i]; 335 | imRight = tex2D(tex2D_right, MIN(tidx - RAD + d, WIDTH - 1), tidy + offset); 336 | 337 | I1I2[sidy_offset][sidx_offset] = imLeft * imRight; 338 | I2_sq[sidy_offset][sidx_offset] = imRight * imRight; 339 | I2[sidy_offset][sidx_offset] = imRight; 340 | } 341 | #pragma unroll 342 | for (int i = 0; i < STEPS; i++) 343 | { 344 | int offset = -RAD + i * RAD; 345 | int sidy_offset = sidy + offset; 346 | int sidx_offset = sidx - RAD + blockDim.x; 347 | if (threadIdx.x < 2 * RAD) 348 | { 349 | imLeft = imLeftB[i]; 350 | imRight = tex2D(tex2D_right, MIN(tidx - RAD + blockDim.x + d, WIDTH - 1), tidy + offset); 351 | 352 | I1I2[sidy_offset][sidx_offset] = imLeft * imRight; 353 | I2_sq[sidy_offset][sidx_offset] = imRight * imRight; 354 | I2[sidy_offset][sidx_offset] = imRight; 355 | } 356 | } 357 | __syncthreads(); 358 | // compute for NCC 359 | I1I2_sum = 0; 360 | I2_sq_sum = 0; 361 | I2_sum = 0; 362 | #pragma unroll 363 | for (int i = -RAD; i <= RAD; i++){ 364 | I1I2_sum += I1I2[sidy][sidx + i]; 365 | I2_sq_sum += I2_sq[sidy][sidx + i]; 366 | I2_sum += I2[sidy][sidx + i]; 367 | } 368 | __syncthreads(); 369 | I1I2[sidy][sidx] = I1I2_sum; 370 | I2_sq[sidy][sidx] = I2_sq_sum; 371 | I2[sidy][sidx] = I2_sum; 372 | __syncthreads(); 373 | if (threadIdx.y < RAD){ 374 | int sidy_offset = sidy - RAD ; 375 | I1I2_sum = 0; 376 | I2_sq_sum = 0; 377 | I2_sum = 0; 378 | for (int i = -RAD; i <= RAD; i++){ 379 | I1I2_sum += I1I2[sidy_offset][sidx + i]; 380 | I2_sq_sum += I2_sq[sidy_offset][sidx + i]; 381 | I2_sum += I2[sidy_offset][sidx + i]; 382 | } 383 | I1I2[sidy_offset][sidx] = I1I2_sum; 384 | I2_sq[sidy_offset][sidx] = I2_sq_sum; 385 | I2[sidy_offset][sidx] = I2_sum; 386 | 387 | } 388 | if (sidy >= blockDim_y){ 389 | int sidy_offset = sidy + RAD; 390 | I1I2_sum = 0; 391 | I2_sq_sum = 0; 392 | I2_sum = 0; 393 | for (int i = -RAD; i <= RAD; i++){ 394 | I1I2_sum += I1I2[sidy_offset][sidx + i]; 395 | I2_sq_sum += I2_sq[sidy_offset][sidx + i]; 396 | I2_sum += I2[sidy_offset][sidx + i]; 397 | } 398 | I1I2[sidy_offset][sidx] = I1I2_sum; 399 | I2_sq[sidy_offset][sidx] = I2_sq_sum; 400 | I2[sidy_offset][sidx] = I2_sum; 401 | 402 | } 403 | __syncthreads(); 404 | 405 | // sum cost vertically 406 | I1I2_sum = 0; 407 | I2_sq_sum = 0; 408 | I2_sum = 0; 409 | #pragma unroll 410 | for (int i = -RAD; i <= RAD; i++) 411 | { 412 | I1I2_sum += I1I2[sidy + i][sidx]; 413 | I2_sq_sum += I2_sq[sidy + i][sidx]; 414 | I2_sum += I2[sidy + i][sidx]; 415 | } 416 | I1I2_mean = I1I2_sum / kernel_area; 417 | I2_sq_mean = I2_sq_sum / kernel_area; 418 | I2_mean = I2_sum / kernel_area; 419 | ncc_nom = (I1I2_mean - I1_mean*I2_mean); 420 | __syncthreads(); 421 | ncc = ncc_nom/(std::sqrt((I1_sq_mean-I1_mean*I1_mean)*(I2_sq_mean-I2_mean*I2_mean))+1e-9); 422 | 423 | cost_volume[get_cost_idx(tidx,tidy,d)] = ncc; 424 | 425 | if (ncc >= best_ncc) 426 | { 427 | best_ncc = ncc; 428 | bestDisparity = d; 429 | } 430 | } 431 | 432 | if (best_ncc > 0.0) 433 | { 434 | disparity_gpu[tidy * WIDTH + tidx] = -bestDisparity; 435 | } 436 | else 437 | { 438 | disparity_gpu[tidy * WIDTH + tidx] = 0; 439 | } 440 | } 441 | 442 | __global__ void stereo_ncc_AEI(float *disparity_gpu, float *cost_volume, float *AEI, int *AEI_idx) 443 | { 444 | const int tidx = blockDim.x * blockIdx.x + threadIdx.x; 445 | const int tidy = blockDim.y * blockIdx.y + threadIdx.y; 446 | const int sidx = threadIdx.x + RAD; 447 | const int sidy = threadIdx.y + RAD; 448 | const float kernel_area = (2 * RAD + 1)*(2 * RAD + 1); 449 | float ncc_cost[MAX_DISP - MIN_DISP + 1]; 450 | 451 | float imLeft; 452 | float imRight; 453 | float I1I2_sum, I1_sq_sum, I2_sq_sum, I1_sum, I2_sum; 454 | float I1I2_mean, I1_sq_mean, I2_sq_mean, I1_mean, I2_mean, ncc, ncc_nom; 455 | float best_ncc = -1.0; 456 | float bestDisparity = 0; 457 | __shared__ float I1I2[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 458 | __shared__ float I1_sq[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 459 | __shared__ float I2_sq[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 460 | __shared__ float I1[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 461 | __shared__ float I2[blockDim_y + 2 * RAD][blockDim_x + 2 * RAD]; 462 | 463 | float imLeftA[STEPS]; 464 | float imLeftB[STEPS]; 465 | 466 | if (tidy >= HEIGHT || tidy < 0 || tidx >= WIDTH || tidx < 0){ 467 | return; 468 | } 469 | else{ 470 | disparity_gpu[tidy * WIDTH + tidx] = 0; 471 | } 472 | for (int i = 0; i < STEPS; i++) 473 | { 474 | int offset = -RAD + i * RAD; 475 | imLeftA[i] = tex2D(tex2D_left_edge, tidx - RAD, tidy + offset); 476 | imLeftB[i] = tex2D(tex2D_left_edge, MIN(tidx - RAD + blockDim.x, WIDTH - 1), tidy + offset); 477 | } 478 | #pragma unroll 479 | for (int i = 0; i < STEPS; i++) 480 | { 481 | int offset = -RAD + i * RAD; 482 | int sidy_offset = sidy + offset; 483 | int sidx_offset = sidx - RAD; 484 | imLeft = imLeftA[i]; 485 | I1_sq[sidy_offset][sidx_offset] = imLeft * imLeft; 486 | I1[sidy_offset][sidx_offset] = imLeft; 487 | } 488 | #pragma unroll 489 | for (int i = 0; i < STEPS; i++) 490 | { 491 | int offset = -RAD + i * RAD; 492 | int sidy_offset = sidy + offset; 493 | int sidx_offset = sidx - RAD + blockDim.x; 494 | if (threadIdx.x < 2 * RAD) 495 | { 496 | imLeft = imLeftB[i]; 497 | I1_sq[sidy_offset][sidx_offset] = imLeft * imLeft; 498 | I1[sidy_offset][sidx_offset] = imLeft; 499 | } 500 | } 501 | __syncthreads(); 502 | 503 | I1_sq_sum = 0; 504 | I1_sum = 0; 505 | #pragma unroll 506 | for (int i = -RAD; i <= RAD; i++){ 507 | I1_sq_sum += I1_sq[sidy][sidx + i]; 508 | I1_sum += I1[sidy][sidx + i]; 509 | } 510 | __syncthreads(); 511 | I1_sq[sidy][sidx] = I1_sq_sum; 512 | I1[sidy][sidx] = I1_sum; 513 | __syncthreads(); 514 | if (threadIdx.y < RAD){ 515 | int sidy_offset = sidy - RAD ; 516 | I1_sq_sum = 0; 517 | I1_sum = 0; 518 | for (int i = -RAD; i <= RAD; i++){ 519 | I1_sq_sum += I1_sq[sidy_offset][sidx + i]; 520 | I1_sum += I1[sidy_offset][sidx + i]; 521 | } 522 | I1_sq[sidy_offset][sidx] = I1_sq_sum; 523 | I1[sidy_offset][sidx] = I1_sum; 524 | 525 | } 526 | if (sidy >= blockDim_y){ 527 | int sidy_offset = sidy + RAD; 528 | I1_sq_sum = 0; 529 | I1_sum = 0; 530 | for (int i = -RAD; i <= RAD; i++){ 531 | I1_sq_sum += I1_sq[sidy_offset][sidx + i]; 532 | I1_sum += I1[sidy_offset][sidx + i]; 533 | } 534 | I1_sq[sidy_offset][sidx] = I1_sq_sum; 535 | I1[sidy_offset][sidx] = I1_sum; 536 | } 537 | __syncthreads(); 538 | I1_sq_sum = 0; 539 | I1_sum = 0; 540 | #pragma unroll 541 | for (int i = -RAD; i <= RAD; i++) 542 | { 543 | I1_sq_sum += I1_sq[sidy + i][sidx]; 544 | I1_sum += I1[sidy + i][sidx]; 545 | } 546 | I1_sq_mean = I1_sq_sum / kernel_area; 547 | I1_mean = I1_sum / kernel_area; 548 | 549 | 550 | #pragma unroll 551 | for (int d = MIN_DISP; d <= MAX_DISP; d++) 552 | { 553 | #pragma unroll 554 | for (int i = 0; i < STEPS; i++) 555 | { 556 | int offset = -RAD + i * RAD; 557 | int sidy_offset = sidy + offset; 558 | int sidx_offset = sidx - RAD; 559 | imLeft = imLeftA[i]; 560 | imRight = AEI[get_idx(trun_idx(tidx - RAD + d, WIDTH), trun_idx(tidy + offset, HEIGHT), AEI_idx[d - MIN_DISP])]; 561 | 562 | I1I2[sidy_offset][sidx_offset] = imLeft * imRight; 563 | I2_sq[sidy_offset][sidx_offset] = imRight * imRight; 564 | I2[sidy_offset][sidx_offset] = imRight; 565 | } 566 | #pragma unroll 567 | for (int i = 0; i < STEPS; i++) 568 | { 569 | int offset = -RAD + i * RAD; 570 | int sidy_offset = sidy + offset; 571 | int sidx_offset = sidx - RAD + blockDim.x; 572 | if (threadIdx.x < 2 * RAD) 573 | { 574 | imLeft = imLeftB[i]; 575 | imRight = AEI[get_idx(trun_idx(tidx - RAD + blockDim.x + d, WIDTH), trun_idx(tidy + offset, HEIGHT), AEI_idx[d - MIN_DISP])]; 576 | 577 | I1I2[sidy_offset][sidx_offset] = imLeft * imRight; 578 | I2_sq[sidy_offset][sidx_offset] = imRight * imRight; 579 | I2[sidy_offset][sidx_offset] = imRight; 580 | } 581 | } 582 | __syncthreads(); 583 | I1I2_sum = 0; 584 | I2_sq_sum = 0; 585 | I2_sum = 0; 586 | #pragma unroll 587 | for (int i = -RAD; i <= RAD; i++){ 588 | I1I2_sum += I1I2[sidy][sidx + i]; 589 | I2_sq_sum += I2_sq[sidy][sidx + i]; 590 | I2_sum += I2[sidy][sidx + i]; 591 | } 592 | __syncthreads(); 593 | I1I2[sidy][sidx] = I1I2_sum; 594 | I2_sq[sidy][sidx] = I2_sq_sum; 595 | I2[sidy][sidx] = I2_sum; 596 | __syncthreads(); 597 | if (threadIdx.y < RAD){ 598 | int sidy_offset = sidy - RAD ; 599 | I1I2_sum = 0; 600 | I2_sq_sum = 0; 601 | I2_sum = 0; 602 | for (int i = -RAD; i <= RAD; i++){ 603 | I1I2_sum += I1I2[sidy_offset][sidx + i]; 604 | I2_sq_sum += I2_sq[sidy_offset][sidx + i]; 605 | I2_sum += I2[sidy_offset][sidx + i]; 606 | } 607 | I1I2[sidy_offset][sidx] = I1I2_sum; 608 | I2_sq[sidy_offset][sidx] = I2_sq_sum; 609 | I2[sidy_offset][sidx] = I2_sum; 610 | 611 | } 612 | if (sidy >= blockDim_y){ 613 | int sidy_offset = sidy + RAD; 614 | I1I2_sum = 0; 615 | I2_sq_sum = 0; 616 | I2_sum = 0; 617 | for (int i = -RAD; i <= RAD; i++){ 618 | I1I2_sum += I1I2[sidy_offset][sidx + i]; 619 | I2_sq_sum += I2_sq[sidy_offset][sidx + i]; 620 | I2_sum += I2[sidy_offset][sidx + i]; 621 | } 622 | I1I2[sidy_offset][sidx] = I1I2_sum; 623 | I2_sq[sidy_offset][sidx] = I2_sq_sum; 624 | I2[sidy_offset][sidx] = I2_sum; 625 | 626 | } 627 | __syncthreads(); 628 | I1I2_sum = 0; 629 | I2_sq_sum = 0; 630 | I2_sum = 0; 631 | #pragma unroll 632 | for (int i = -RAD; i <= RAD; i++) 633 | { 634 | I1I2_sum += I1I2[sidy + i][sidx]; 635 | I2_sq_sum += I2_sq[sidy + i][sidx]; 636 | I2_sum += I2[sidy + i][sidx]; 637 | } 638 | I1I2_mean = I1I2_sum / kernel_area; 639 | I2_sq_mean = I2_sq_sum / kernel_area; 640 | I2_mean = I2_sum / kernel_area; 641 | ncc_nom = (I1I2_mean - I1_mean*I2_mean); 642 | __syncthreads(); 643 | ncc = ncc_nom/(std::sqrt((I1_sq_mean-I1_mean*I1_mean)*(I2_sq_mean-I2_mean*I2_mean))+1e-9); 644 | 645 | cost_volume[get_cost_idx(tidx,tidy,d)] = ncc; 646 | if (ncc >= best_ncc) 647 | { 648 | best_ncc = ncc; 649 | bestDisparity = d; 650 | } 651 | } 652 | 653 | if (best_ncc > 0.0) 654 | { 655 | disparity_gpu[tidy * WIDTH + tidx] = -bestDisparity; 656 | } 657 | else 658 | { 659 | disparity_gpu[tidy * WIDTH + tidx] = 0; 660 | } 661 | } 662 | 663 | __global__ void stereo_postproc(float *disparity_gpu, float *cost_volume, float *gaussian){ 664 | const int tidx = blockDim.x * blockIdx.x + threadIdx.x; 665 | const int tidy = blockDim.y * blockIdx.y + threadIdx.y; 666 | const int kernel_size = 2 * RAD + 1; 667 | float best_ncc = -1.0; 668 | float bestDisparity = 0; 669 | float cost_ncc[MAX_DISP - MIN_DISP]; 670 | 671 | 672 | for (int d = MIN_DISP; d <= MAX_DISP; d++){ 673 | float ncc_gaussian = 0; 674 | float ncc_gaussian_denom = 1e-6; 675 | if(tidx >= FILTER_RAD && tidx < (WIDTH - FILTER_RAD) && tidy >= FILTER_RAD && tidy < (HEIGHT - FILTER_RAD)){ 676 | for (int i = -FILTER_RAD; i <= FILTER_RAD; i++){ 677 | for (int j = -FILTER_RAD; j <= FILTER_RAD; j++){ 678 | float weight = gaussian[(j + RAD)* kernel_size + i + RAD]; 679 | if(cost_volume[get_cost_idx(tidx + i, tidy + j, d)] > 0.0 && weight > 0){ 680 | ncc_gaussian += cost_volume[get_cost_idx(tidx + i, tidy + j, d)] * weight; 681 | ncc_gaussian_denom += weight; 682 | } 683 | 684 | } 685 | } 686 | ncc_gaussian /= ncc_gaussian_denom; 687 | } 688 | else{ 689 | ncc_gaussian = cost_volume[get_cost_idx(tidx, tidy, d)]; 690 | } 691 | 692 | cost_ncc[d - MIN_DISP] = ncc_gaussian; 693 | 694 | if (ncc_gaussian > best_ncc) 695 | { 696 | best_ncc = ncc_gaussian; 697 | bestDisparity = d; 698 | } 699 | } 700 | 701 | if (best_ncc > 0.0 && !(EDGE_FLAG && tex2D(tex2D_left_edge, tidx, tidy)*tex2D(tex2D_left_edge, tidx, tidy) < 1e-4)){ 702 | { 703 | int d = (int)bestDisparity; 704 | 705 | float dp = cost_ncc[MIN(d + 1, MAX_DISP) - MIN_DISP]; 706 | float dm = best_ncc; 707 | float dn = cost_ncc[MAX(d - 1, MIN_DISP) - MIN_DISP]; 708 | 709 | disparity_gpu[tidy * WIDTH + tidx] = MAX(-(bestDisparity + (0.5f*(dp-dn)/(2.0f*dm-dp-dn))), 0.0); 710 | } 711 | } 712 | else 713 | { 714 | disparity_gpu[tidy * WIDTH + tidx] = 0; 715 | } 716 | 717 | } 718 | 719 | __global__ void stereo_postproc_AEI(float *disparity_gpu, float *cost_volume, float *cost_volume_AEI, float *gaussian){ 720 | const int tidx = blockDim.x * blockIdx.x + threadIdx.x; 721 | const int tidy = blockDim.y * blockIdx.y + threadIdx.y; 722 | const int kernel_size = 2 * RAD + 1; 723 | 724 | float best_ncc = -1.0; 725 | float bestDisparity = 0; 726 | float cost_ncc[MAX_DISP - MIN_DISP]; 727 | 728 | for (int d = MIN_DISP; d <= MAX_DISP; d++){ 729 | float ncc_gaussian = 0; 730 | float ncc_gaussian_denom = 1e-6; 731 | if(tidx >= FILTER_RAD && tidx < (WIDTH - FILTER_RAD) && tidy >= FILTER_RAD && tidy < (HEIGHT - FILTER_RAD)){ 732 | for (int i = -FILTER_RAD; i <= FILTER_RAD; i++){ 733 | for (int j = -FILTER_RAD; j <= FILTER_RAD; j++){ 734 | float weight = gaussian[(j + RAD)* kernel_size + i + RAD]; 735 | if(cost_volume[get_cost_idx(tidx + i, tidy + j, d)] > 0.0 736 | && cost_volume_AEI[get_cost_idx(tidx + i, tidy + j, d)] > 0.0 737 | && weight > 0){ 738 | ncc_gaussian += cost_volume[get_cost_idx(tidx + i, tidy + j, d)] * cost_volume_AEI[get_cost_idx(tidx + i, tidy + j, d)] * weight; 739 | ncc_gaussian_denom += weight; 740 | } 741 | } 742 | } 743 | ncc_gaussian /= ncc_gaussian_denom; 744 | } 745 | else{ 746 | ncc_gaussian = cost_volume[get_cost_idx(tidx, tidy, d)] * cost_volume_AEI[get_cost_idx(tidx, tidy, d)]; 747 | } 748 | cost_ncc[d - MIN_DISP] = ncc_gaussian; 749 | if (ncc_gaussian > best_ncc) 750 | { 751 | best_ncc = ncc_gaussian; 752 | bestDisparity = d; 753 | } 754 | } 755 | if (best_ncc > 0.0 && !(EDGE_FLAG && tex2D(tex2D_left_edge, tidx, tidy)*tex2D(tex2D_left_edge, tidx, tidy) < 1e-4)) 756 | { 757 | int d = (int)bestDisparity; 758 | 759 | float dp = cost_ncc[MIN(d + 1, MAX_DISP) - MIN_DISP]; 760 | float dm = best_ncc; 761 | float dn = cost_ncc[MAX(d - 1, MIN_DISP) - MIN_DISP]; 762 | 763 | disparity_gpu[tidy * WIDTH + tidx] = MAX(-(bestDisparity + (0.5f*(dp-dn)/(2.0f*dm-dp-dn))), 0.0); 764 | } 765 | else 766 | { 767 | disparity_gpu[tidy * WIDTH + tidx] = 0; 768 | } 769 | } 770 | 771 | __global__ void densify_sparse_disp(float *dense, float *sparse, float *gaussian){ 772 | const int tidx = blockDim.x * blockIdx.x + threadIdx.x; 773 | const int tidy = blockDim.y * blockIdx.y + threadIdx.y; 774 | int kernel_size = 2 * RAD + 1; 775 | 776 | float interp = 0; 777 | float interp_denom = 1e-6; 778 | int cnt = 0; 779 | if(tidx >= FILTER_RAD && tidx < (WIDTH - FILTER_RAD) && tidy >= FILTER_RAD && tidy < (HEIGHT - FILTER_RAD) && sparse[tidy * WIDTH + tidx] == 0 ){ 780 | for (int r = 0; r < FILTER_RAD; r++){ 781 | for (int i = -r; i <= r; i++){ 782 | for (int j = -r; j <= r; j++){ 783 | if(i!=r && j != r && i!=-r && j != -r){ 784 | continue; 785 | } 786 | float weight = gaussian[(j + RAD)* kernel_size + i + RAD]; 787 | if(sparse[(tidy + j) * WIDTH + (tidx + i)] > 0.0 && weight > 0){ 788 | interp += sparse[(tidy + j) * WIDTH + (tidx + i)] * weight; 789 | interp_denom += weight; 790 | cnt ++; 791 | } 792 | } 793 | } 794 | if(cnt > 3){ 795 | break; 796 | } 797 | } 798 | interp /= interp_denom; 799 | } 800 | else{ 801 | interp = sparse[tidy * WIDTH + tidx]; 802 | } 803 | dense[tidy * WIDTH + tidx] = interp; 804 | } 805 | -------------------------------------------------------------------------------- /HSM/cuda_source.py: -------------------------------------------------------------------------------- 1 | import pycuda.driver as cuda 2 | import pycuda.tools 3 | import pycuda.autoinit 4 | import pycuda.gpuarray as gpuarray 5 | from pycuda.compiler import SourceModule 6 | import string 7 | import numpy as np 8 | 9 | CUDA_SOURCE_FILE_PATH = "cuda/source.cu" 10 | 11 | cuda_source_file = open(CUDA_SOURCE_FILE_PATH, "r") 12 | template = string.Template(cuda_source_file.read()) 13 | cuda_source_file.close() 14 | -------------------------------------------------------------------------------- /HSM/dataloader.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | import cv2 8 | import glob 9 | import h5py 10 | import hdf5plugin 11 | import yaml 12 | import numpy as np 13 | from numpy import linalg as la 14 | from tqdm import tqdm 15 | from tqdm import trange 16 | 17 | from Camera import Camera, CameraDSEC 18 | 19 | class DataLoaderDSEC: 20 | is_initialized = False 21 | # events.hdf5: 'events', 'ms_to_idx', 't_offset' 22 | def __init__(self, dir_path, data_seq): 23 | 24 | self.calib_path = ''.join([dir_path, data_seq, '/', data_seq, '_calibration/cam_to_cam.yaml']) 25 | self.disparity_image_paths = glob.glob(''.join([dir_path, data_seq, '/', data_seq, '_disparity_image/*.png'])) 26 | self.disparity_event_paths = glob.glob(''.join([dir_path, data_seq, '/', data_seq, '_disparity_event/*.png'])) 27 | self.disparity_timestamp_path = ''.join([dir_path, data_seq, '/', data_seq, '_disparity_timestamps.txt']) 28 | self.events_left_path = ''.join([dir_path, data_seq, '/', data_seq, '_events_left/events.h5']) 29 | self.events_right_path = ''.join([dir_path, data_seq, '/', data_seq, '_events_right/events.h5']) 30 | self.images_left_paths = glob.glob(''.join([dir_path, data_seq, '/', data_seq, '_images_rectified_left/*.png'])) 31 | self.images_right_paths = glob.glob(''.join([dir_path, data_seq, '/', data_seq, '_images_rectified_right/*.png'])) 32 | self.image_timestamp_path = ''.join([dir_path, data_seq, '/', data_seq, '_image_timestamps.txt']) 33 | self.processed_data_path = ''.join([dir_path, data_seq, '_processed_data.hdf5']) 34 | self.E2VID_data_path = glob.glob(''.join([dir_path, data_seq, '/E2VID/*.png'])) 35 | self.data_seq = data_seq 36 | self.proc_data = {} 37 | 38 | try: 39 | self.proc_data = h5py.File(self.processed_data_path, "r") 40 | self.is_data_loaded = True 41 | print('Data loader initiated.') 42 | except: 43 | self.proc_data = h5py.File(self.processed_data_path, "w") 44 | self.is_data_loaded = False 45 | 46 | self.load_data() 47 | print("Completed loading [{0}] images and events from dataset [{1}].".format(self.image_num, self.data_seq)) 48 | 49 | 50 | def __del__(self): 51 | self.proc_data.close() 52 | 53 | def load_data(self): 54 | if not self.is_data_loaded: 55 | print("Start converting dataset into hdf file for faster loading later.") 56 | print("This process will only run for the first time.") 57 | self.load_images() 58 | self.load_events() 59 | print("Complete saving dataset as hdf file.") 60 | self.proc_data = h5py.File(self.processed_data_path, "r") 61 | self.is_data_loaded = True 62 | self.images = self.proc_data['images'] 63 | self.image_ts = self.proc_data['image_ts'] 64 | self.events = self.proc_data['events'] 65 | self.image_num = len(self.image_ts) 66 | 67 | def load_images(self): 68 | if not (len(self.images_left_paths) == len(self.images_right_paths)): 69 | raise Exception("Image file length is not equal") 70 | 71 | image_ts = np.loadtxt(self.image_timestamp_path, dtype = 'int')/1e6 72 | self.image_num = image_num = len(image_ts) 73 | 74 | cam0_images = np.zeros(((image_num,)+ cv2.imread(self.images_left_paths[0]).shape), dtype=np.uint8) 75 | print("Loading images") 76 | for i in trange(image_num): 77 | cam0_images[i, :, :, :] = cv2.imread(self.images_left_paths[i]).astype(np.uint8) 78 | self.image_ts = image_ts 79 | self.proc_data.create_dataset('images', data=cam0_images[:,:,:,:]) 80 | self.proc_data.create_dataset('image_ts', data=self.image_ts) 81 | 82 | def load_events(self): 83 | cam1_events = h5py.File(self.events_right_path)['events'] 84 | # # frame: left cam0 85 | # # event right cam1 86 | events = cam1_events 87 | image_ts = np.loadtxt(self.image_timestamp_path, dtype = 'int')/1e6 88 | event_chunk_st = [None] * image_ts.shape[0] 89 | 90 | t_idx = 0 91 | image_ts = image_ts - image_ts[0] 92 | ts = image_ts[t_idx] 93 | print("Indexing events") 94 | for e_idx in trange(0,len(events['t']),5000): 95 | if(events['t'][e_idx]/1e6 >= ts): 96 | event_chunk_st[t_idx] = e_idx 97 | t_idx += 1 98 | if (t_idx >= len(image_ts)): 99 | break 100 | ts = image_ts[t_idx] 101 | 102 | event_x = np.split(events['x'], event_chunk_st[:-1], axis = 0) 103 | event_y = np.split(events['y'], event_chunk_st[:-1], axis = 0) 104 | event_t = np.split(events['t'], event_chunk_st[:-1], axis = 0) 105 | event_p = np.split(events['p'], event_chunk_st[:-1], axis = 0) 106 | 107 | if not event_chunk_st[0]: 108 | event_x[0] = np.array([0]) 109 | event_y[0] = np.array([0]) 110 | event_t[0] = np.array([0]) 111 | event_p[0] = np.array([0]) 112 | 113 | event_group = self.proc_data.create_group('events') 114 | print("Loading events") 115 | for idx in trange(len(event_t)): 116 | events = np.stack((event_x[idx], event_y[idx], event_t[idx], event_p[idx]), axis=1).astype(np.float32) 117 | events[:,2] /= 1e6 118 | event_group.create_dataset('{0}'.format(idx), data=events) 119 | 120 | 121 | def get_data(self, idx): 122 | image = cv2.cvtColor(self.images[idx], cv2.COLOR_RGB2GRAY) 123 | image_ts = self.image_ts[idx] 124 | event = np.asarray(self.events['{0}'.format(idx)]) 125 | return image, image_ts, event 126 | 127 | def get_debug_data(self, idx): 128 | # load right image 129 | try: 130 | cam1_image = cv2.cvtColor(cv2.imread(self.images_right_paths[idx]).astype(np.uint8), cv2.COLOR_RGB2GRAY) 131 | cam1_image = self.cam0.rectify(cam1_image) 132 | except: 133 | cam1_image = np.zeros_like(self.cam0.resolution) 134 | # load E2VID image 135 | try: 136 | cam1_image_E2VID = cv2.cvtColor(cv2.imread(self.E2VID_data_path[idx]).astype(np.uint8), cv2.COLOR_RGB2GRAY) 137 | cam1_image_E2VID = self.cam1.rectify(cam1_image_E2VID) 138 | except: 139 | cam1_image_E2VID = np.zeros_like(self.cam0.resolution) 140 | # load disparity 141 | try: 142 | if idx%2 == 0: 143 | image_disparity = cv2.cvtColor(cv2.imread(self.disparity_image_paths[int(idx/2)]), 144 | cv2.COLOR_RGB2GRAY).astype(np.uint16) 145 | image_disparity = self.cam0.rectify(image_disparity, cv2.INTER_NEAREST)*( 146 | (self.cam0.K_rect[0][0]+self.cam0.K_rect[1][1])/(self.cam0.K[0][0]+self.cam0.K[1][1]) 147 | *self.cam0.baseline/self.cam0.baseline_gt) 148 | success = True 149 | else: 150 | image_disparity = np.zeros_like(self.cam0.resolution) 151 | success = False 152 | except: 153 | image_disparity = np.zeros_like(self.cam0.resolution) 154 | success = False 155 | 156 | return success, cam1_image, image_disparity, cam1_image_E2VID 157 | 158 | def load_calib_data(self): 159 | calib_data = [] 160 | with open(self.calib_path, "r") as stream: 161 | try: 162 | calib_data = yaml.safe_load(stream) 163 | except yaml.YAMLError as exc: 164 | print(exc) 165 | 166 | rect_mat = np.eye(3) 167 | T_32 = np.asarray(calib_data['extrinsics']['T_32']) 168 | T_21 = np.asarray(calib_data['extrinsics']['T_21']) 169 | T_rect1 = np.eye(4) 170 | T_rect1[:3,:3] = np.asarray(calib_data['extrinsics']['R_rect1']) 171 | T_rect31 = np.matmul(np.matmul(np.linalg.inv(T_32), np.linalg.inv(T_21)), T_rect1) 172 | rect_mat = T_rect31[:3, :3] 173 | baseline = la.norm(T_rect31[:3, 3]) 174 | 175 | rect_mat = np.eye(3) 176 | T_32 = np.asarray(calib_data['extrinsics']['T_32']) 177 | T_rect2 = np.eye(4) 178 | T_rect2[:3,:3] = np.asarray(calib_data['extrinsics']['R_rect2']) 179 | T_rect32 = np.matmul(np.linalg.inv(T_32), T_rect2) 180 | rect_mat = T_rect32[:3, :3] 181 | baseline = la.norm(T_rect31[:3, 3]) 182 | baseline_gt = la.norm(T_21[:3, 3]) 183 | 184 | # rect_mat = np.eye(3) 185 | 186 | CameraDSEC.set_baseline(baseline, baseline_gt) 187 | CameraDSEC.set_cam_rect(calib_data['intrinsics']['camRect3']) 188 | self.cam0 = CameraDSEC(calib_data['intrinsics']['camRect1']) 189 | self.cam1 = CameraDSEC(calib_data['intrinsics']['cam3'], rect_mat) 190 | 191 | return self.cam0, self.cam1 192 | 193 | def load_params(self): 194 | cam0, cam1 = self.load_calib_data() 195 | 196 | params = {} 197 | params["cam0"] = cam0 198 | params["cam1"] = cam1 199 | 200 | params['loader'] = self 201 | 202 | with open('config/config.yaml', "r") as stream: 203 | try: 204 | config_data = yaml.safe_load(stream) 205 | except yaml.YAMLError as exc: 206 | print(exc) 207 | 208 | params['feature num'] = config_data['feature num'] 209 | params['disparity_range'] = config_data['disparity_range'] 210 | 211 | params['kernel_radius'] = config_data['kernel_radius'] 212 | params['ncc_gaussian_std'] = config_data['ncc_gaussian_std'] 213 | 214 | params['track_win_size'] = config_data['track_win_size'] 215 | params['extract_win_size'] = config_data['extract_win_size'] 216 | params['track_err_thres'] = config_data['track_err_thres'] 217 | 218 | params['time_estimation'] = config_data['time_estimation'] 219 | params['show_disparity_inlier'] = config_data['show_disparity_inlier'] 220 | params['show_disparity'] = config_data['show_disparity'] 221 | params['semi_dense'] = config_data['semi_dense'] 222 | 223 | params['msd_gap'] = config_data['msd_gap'] 224 | 225 | return params 226 | -------------------------------------------------------------------------------- /HSM/filter.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | import numpy as np 9 | from scipy import ndimage 10 | from scipy.ndimage.filters import convolve 11 | 12 | def gaussuian_filter(kernel_radius, sigma=1): 13 | x, y = np.meshgrid(np.linspace(-kernel_radius, kernel_radius, 2*kernel_radius+1), 14 | np.linspace(-kernel_radius, kernel_radius, 2*kernel_radius+1)) 15 | dst = x**2+y**2 16 | normal = 1/np.sqrt(2.0 * np.pi * sigma**2) 17 | if(kernel_radius-2*sigma >= 0): 18 | mask = np.zeros_like(dst) 19 | mask[kernel_radius-2*sigma:kernel_radius+2*sigma+1, kernel_radius-2*sigma:kernel_radius+2*sigma+1] = 1 20 | else: 21 | mask = np.ones_like(dst) 22 | result = (np.exp(-( dst / (2.0 * sigma**2))) * mask).astype(np.float32) 23 | 24 | return result / np.sum(result) 25 | 26 | def non_max_suppression(img, D): 27 | M, N = img.shape 28 | Z = np.zeros((M,N), dtype=np.int32) 29 | angle = D * 180. / np.pi 30 | angle[angle < 0] += 180 31 | 32 | for i in range(1,M-1): 33 | for j in range(1,N-1): 34 | try: 35 | q = 255 36 | r = 255 37 | 38 | #angle 0 39 | if (0 <= angle[i,j] < 22.5) or (157.5 <= angle[i,j] <= 180): 40 | q = img[i, j+1] 41 | r = img[i, j-1] 42 | #angle 45 43 | elif (22.5 <= angle[i,j] < 67.5): 44 | q = img[i+1, j-1] 45 | r = img[i-1, j+1] 46 | #angle 90 47 | elif (67.5 <= angle[i,j] < 112.5): 48 | q = img[i+1, j] 49 | r = img[i-1, j] 50 | #angle 135 51 | elif (112.5 <= angle[i,j] < 157.5): 52 | q = img[i-1, j-1] 53 | r = img[i+1, j+1] 54 | 55 | if (img[i,j] >= q) and (img[i,j] >= r): 56 | Z[i,j] = img[i,j] 57 | else: 58 | Z[i,j] = 0 59 | 60 | except IndexError as e: 61 | pass 62 | return Z 63 | 64 | def sobel_filters(img): 65 | Kx = np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]], np.float32) 66 | Ky = np.array([[1, 2, 1], [0, 0, 0], [-1, -2, -1]], np.float32) 67 | 68 | Ix = ndimage.filters.convolve(img, Kx) 69 | Iy = ndimage.filters.convolve(img, Ky) 70 | 71 | G = np.hypot(Ix, Iy) 72 | G = G / G.max() * 255 73 | theta = np.arctan2(Iy, Ix) 74 | return (G, theta) -------------------------------------------------------------------------------- /HSM/main.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import yaml 3 | from HSM import HSM 4 | from dataloader import DataLoaderDSEC 5 | 6 | try: 7 | dir_path = sys.argv[1] 8 | data_seq = sys.argv[2] 9 | except: 10 | print("ERROR: Please enter data set path $directory path$ $data sequence name$") 11 | print("Example) python main.py /c/DSEC/ interlaken_00_c") 12 | exit() 13 | 14 | loader = DataLoaderDSEC(dir_path, data_seq) 15 | params = loader.load_params() 16 | 17 | if __name__ == '__main__': 18 | 19 | hsm = HSM(params) 20 | for idx in range(0, loader.image_num-1): 21 | image, image_ts, event = loader.get_data(idx) 22 | hsm.process(image, image_ts, event) 23 | hsm.evaluate(idx) 24 | print('processing: {0} / {1}'.format(idx+1, loader.image_num)) 25 | 26 | hsm.metric_print('init') 27 | hsm.metric_print('proposed') 28 | hsm.time_print() 29 | -------------------------------------------------------------------------------- /HSM/metric.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | import numpy as np 9 | import warnings 10 | 11 | warnings.filterwarnings("ignore") 12 | def logRMSE(data, gt): 13 | mask = (data > 0) & (gt > 0) & np.isfinite(data) & np.isfinite(gt) 14 | data = data[mask] 15 | gt = gt[mask] 16 | log_RMSE = np.sqrt(np.sum((np.log(data)-np.log(gt))**2)/np.log(10)**2/data.shape[0]) 17 | return log_RMSE 18 | 19 | def log10err(data, gt): 20 | mask = (data > 0) & (gt > 0) & np.isfinite(data) & np.isfinite(gt) 21 | data = data[mask] 22 | gt = gt[mask] 23 | log10err = np.sum(np.abs(np.log(data)-np.log(gt)))/np.log(10)/data.shape[0] 24 | return log10err 25 | 26 | def ARD(data, gt): 27 | mask = (data >= 0) & (gt >= 0) & np.isfinite(data) & np.isfinite(gt) 28 | data = data[mask] 29 | gt = gt[mask] 30 | ARD = np.sum(np.abs(data - gt)/gt)/data.shape[0] 31 | return ARD 32 | 33 | def ATdelta(data, gt, gt_valid_num): 34 | mask = (data > 0) & (gt > 0) & np.isfinite(data) & np.isfinite(gt) 35 | data = data[mask] 36 | gt = gt[mask] 37 | delta = np.array([1.05, 1.05**2, 1.05**3]) 38 | ATdelta = np.zeros_like(delta) 39 | if data.shape[0] == 0: 40 | return ATdelta 41 | else: 42 | for i, d in enumerate(delta): 43 | ATdelta[i] = np.sum(np.max([data/gt, gt/data], axis=0) < d) / gt_valid_num 44 | return ATdelta 45 | 46 | def DTdelta(data, gt, gt_valid_num): 47 | mask = (data > 0) & (gt > 0) & np.isfinite(data) & np.isfinite(gt) 48 | data = data[mask] 49 | gt = gt[mask] 50 | delta = np.array([1, 2, 3]) 51 | DTdelta = np.zeros_like(delta, dtype = np.float32) 52 | for i, d in enumerate(delta): 53 | DTdelta[i] = np.sum(np.abs([data - gt]) < d) / gt_valid_num 54 | return DTdelta 55 | 56 | def RTdelta(data, gt): 57 | mask = (data > 0) & (gt > 0) & np.isfinite(data) & np.isfinite(gt) 58 | data = data[mask] 59 | gt = gt[mask] 60 | delta = np.array([1, 2, 3]) 61 | RTdelta = np.zeros_like(delta, dtype = np.float32) 62 | for i, d in enumerate(delta): 63 | RTdelta[i] = np.sum(np.abs([data - gt]) < d) / data.shape[0] 64 | return RTdelta 65 | 66 | def MAE(data, gt): 67 | mask = (data >= 0) & (gt >= 0) & np.isfinite(data) & np.isfinite(gt) 68 | data = data[mask] 69 | gt = gt[mask] 70 | MAE = np.sum(np.abs(data - gt))/data.shape[0] 71 | return MAE 72 | 73 | def RMSE(data, gt): 74 | mask = (data >= 0) & (gt >= 0) & np.isfinite(data) & np.isfinite(gt) 75 | data = data[mask] 76 | gt = gt[mask] 77 | RMSE = np.sqrt(np.sum(np.abs(data - gt)**2)/data.shape[0]) 78 | return RMSE -------------------------------------------------------------------------------- /HSM/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | edit: Haram Kim 3 | email: rlgkfka614@gmail.com 4 | github: https://github.com/haram-kim 5 | homepage: https://haram-kim.github.io/ 6 | """ 7 | 8 | import numpy as np 9 | from scipy.linalg import expm, logm 10 | 11 | def SO3(w): 12 | return expm(hat(w)) 13 | 14 | def hat(x): 15 | x_hat = np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) 16 | return x_hat 17 | 18 | def SE3(r, t = None): 19 | if t is None: 20 | xi = r 21 | else: 22 | xi = np.concatenate([t, r],0) 23 | se3 = np.zeros((4,4)) 24 | se3[:3,:3] = hat(xi[3:]) 25 | se3[:3, 3] = xi[:3] 26 | return expm(se3) 27 | 28 | def InvSE3(SE3): 29 | xi = np.zeros(6) 30 | se3 = logm(SE3) 31 | 32 | xi[:3] = se3[:3, 3] 33 | xi[3] = se3[2,1] 34 | xi[4] = se3[0,2] 35 | xi[5] = se3[1,0] 36 | 37 | return xi 38 | 39 | 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Haram Kim 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Hetero Stereo Matching 2 | ======================= 3 | 4 | The source code is released under the **MIT License**. 5 | 6 | # Project Page 7 | 8 | frame event depth 9 | 10 | You can see the high-quality video at: 11 | [https://haram-kim.github.io/Hetero_Stereo_Matching/](https://haram-kim.github.io/Hetero_Stereo_Matching/) 12 | 13 | # Prerequisites 14 | ## Python 15 | We tested our code on [Python](https://www.python.org/) version 3.8.0 16 | [[Download]](https://www.python.org/downloads/release/python-380/) 17 | 18 | ## OpenCV 19 | We use [OpenCV](http://opencv.org) to visualize and manipulate images. 20 | 21 | ## CUDA 22 | We use [CUDA](https://developer.nvidia.com/cuda-toolkit) for parallel computation. 23 | [[Download]](https://developer.nvidia.com/cuda-downloads) [[Installation guide]](https://docs.nvidia.com/cuda/cuda-installation-guide-linux/) 24 | 25 | ### pyCUDA 26 | We use python wrapper [pyCUDA](https://documen.tician.de/pycuda/) for CUDA. 27 | 28 | ## Others 29 | You can install dependent libararies by running : 30 | ``` 31 | pip install opencv pycuda pyyaml scipy tqdm h5py hdf5plugin 32 | ``` 33 | 34 | # Installation & Running guide 35 | 36 | The code works best on Ubuntu 20.04 37 | 38 | 0. Download DSEC data set 39 | DSEC [[Download]](https://dsec.ifi.uzh.ch/dsec-datasets/download/) 40 | 41 | 0-1. (Optional) If you want to download only the preprocessed data, please download: 42 | ['interlaken_00_c_processed_data'](https://larr.snu.ac.kr/haramkim/DSEC/interlaken_00_c_processed_data.hdf5) (8.71GB) 43 | ['interlaken_00_d_processed_data'](https://larr.snu.ac.kr/haramkim/DSEC/interlaken_00_d_processed_data.hdf5) (34.4GB) 44 | ['interlaken_00_e_processed_data'](https://larr.snu.ac.kr/haramkim/DSEC/interlaken_00_e_processed_data.hdf5) (26.9GB) 45 | ['interlaken_00_f_processed_data'](https://larr.snu.ac.kr/haramkim/DSEC/interlaken_00_f_processed_data.hdf5) (14.6GB) 46 | ['interlaken_00_g_processed_data'](https://larr.snu.ac.kr/haramkim/DSEC/interlaken_00_g_processed_data.hdf5) (14.2GB) 47 | (Still, '[DATA_SEQUENCE]_calibration' must be download.) 48 | 49 | Directory structure : 50 | ``` 51 | /PATH_TO_DSEC 52 | ├────interlaken_00_c 53 | │ ├────interlaken_00_c_calibration 54 | │ ├────interlaken_00_c_disparity_event 55 | │ ├────interlaken_00_c_disparity_image 56 | │ ├────interlaken_00_c_events_left 57 | │ ├────interlaken_00_c_events_right 58 | │ ├────interlaken_00_c_images_rectified_left 59 | │ ├────interlaken_00_c_images_rectified_right 60 | │ ├────interlaken_00_c_disparity_timestamps.txt 61 | │ └────interlaken_00_c_image_timestamps.txt 62 | │ 63 | ├────interlaken_00_c_processed_data.hdf5 (Please locate the preprocessed data to this.) 64 | 65 | ... 66 | 67 | 68 | ├────interlaken_00_g 69 | │ ├────interlaken_00_g_calibration 70 | │ 71 | │ ... 72 | │ 73 | │ └────interlaken_00_g_image_timestamps.txt 74 | └────interlaken_00_g_processed_data.hdf5 75 | ``` 76 | 77 | 1. Clone this repository: 78 | ``` 79 | $ git clone https://github.com/Haram-kim/Hetero_Stereo_Matching.git 80 | ``` 81 | 82 | 2. Run the code 83 | ``` 84 | $ cd HSM 85 | $ python main.py [PATH_TO_DSEC] [DATA_SEQUENCE] 86 | Example) $ python main.py /c/DSEC/ interlaken_00_c 87 | ``` 88 | 89 | 90 | ## Configuration settings 91 | 92 | ### config/config.yaml 93 | 94 | #### Feature tracker 95 | feature num - the number of features 96 | track_err_thres - KLT traker error threshold 97 | track_win_size - KLT tracker window size 98 | extract_win_size - Feature extractor window size 99 | ``` 100 | feature num: 1000 101 | track_err_thres: 10 102 | track_win_size: 103 | - 21 104 | - 21 105 | extract_win_size: 12 106 | ``` 107 | 108 | #### Disparity estimator 109 | disparity_range - disparity range (0 - max_disp) 110 | kernel_radius - patch radius 111 | ncc_gaussian_std - standard deviation of Gaussian filter 112 | msd_gap - Maximum Shift Distance interval 113 | ``` 114 | disparity_range: 100 115 | kernel_radius: 12 116 | ncc_gaussian_std: 2 117 | msd_gap: 10 118 | ``` 119 | 120 | #### Time estimation mode 121 | If you want to estimate the computation time, you can run the time_estimation mode. 122 | ``` 123 | time_estimation: True 124 | ``` 125 | 126 | #### Visualize 127 | 'show disparity inlier' requires ground truth disparity. 128 | If you want to see all estimated disparity of the proposed method, please change the flag 'show_disparity' False to True 129 | If you want to see estimated disparity on edges of the proposed method, please change the flag 'semi_dense' False to True 130 | ``` 131 | show_disparity_inlier: True 132 | show_disparity: True 133 | semi_dense: True 134 | ``` 135 | 136 | 137 | -------------------------------------------------------------------------------- /snapshot/depth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Haram-kim/Hetero_Stereo_Matching/cc3389e0209eea31103c423dc5e9eaf7ea9491b4/snapshot/depth.png -------------------------------------------------------------------------------- /snapshot/event.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Haram-kim/Hetero_Stereo_Matching/cc3389e0209eea31103c423dc5e9eaf7ea9491b4/snapshot/event.png -------------------------------------------------------------------------------- /snapshot/frame.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Haram-kim/Hetero_Stereo_Matching/cc3389e0209eea31103c423dc5e9eaf7ea9491b4/snapshot/frame.png --------------------------------------------------------------------------------