def run(self, save_clouds, show_clouds): """ Args: save_clouds (bool): Default False show_clouds (bool): Default False Returns: void """ Logger._log("Pipeline") ### # Stage 0: Detect features in loaded images ## ''' NOTE: We should return modified things for scoping purposes, unless someone knows very well how to split classes in Python. I have looked at some ways but honestly it's not really "Pythonic". ''' images = [] images = self.load_images.load_images(self.folder_path, images) ### # Stage 1: Detect features in loaded images ## cam_Frames = [] descriptors_vec = [] images, cam_Frames, descriptors_vec = self.feature_extraction.extract_features( images, cam_Frames, descriptors_vec, self.low_thresh, self.high_thresh) ### # Stage 2: Calculate descriptors and find image pairs through matching ## image_pairs = [] image_pairs, cam_Frames = self.find_matching_pairs.find_matching_pairs( images, cam_Frames, descriptors_vec, image_pairs) ### # Stage 3: Compute pairwise R and t ## image_pairs, cam_Frames = self.registration.register_camera( image_pairs, cam_Frames, self.cameraMatrix, self.distCoeffs) ### # Stage 4: Construct associativity matrix and spannign tree ## assocMat = associativity(len(cam_Frames)) # print(dir(assocMat)) for p in range(len(image_pairs)): pair = image_pairs[p] i = pair.pair_index[0] j = pair.pair_index[1] if pair.R.size != 0: continue assocMat.assignPair(i, j, pair) assocMat.assignPair(j, i, pair) tree = associativity() camera_num = self.spanning_tree.build_spanning_tree( image_pairs, assocMat, tree) tree = assocMat ### # Stage 5: Compute global Rs and ts ## gCameraPoses = [] gCameraPoses = self.global_cam_poses.glo_cam_poses( images, gCameraPoses, image_pairs, tree) ### # Stage 6: Find and cluster depth points from local camera frame to global camera frame ## pointClusters = [] pointMap = {} pointClusters, pointMap = self.find_clust.find_clusters( assocMat, gCameraPoses, cam_Frames, pointClusters, pointMap, self.cameraMatrix, image_pairs) ### # Stage 7: get center of mass from clusters ## pointCloud = [] pointCloud = self.findCoM.find_CoM(pointClusters, pointCloud) ## Save cloud before BA view = viewer("Before BA", self.low_thresh, self.high_thresh) cloud = view.createPointCloud(images, gCameraPoses, self.cameraMatrix) # n = len(clouds.points) # folder = os.current_dir() fname = "./" + self.filename + ".pcd" # if save_clouds: view.saveCloud(cloud, fname)
def load_images(folder_path, images): Logger._log("\nStep 0 (preprocess)") ## Get list of image files from dataset path if not os.path.exists(folder_path): raise ValueError("Invalid data path: {}".format(folder_path)) ## Reset images list images = [] # Regex to parse image names fname_regex = "^.*/frame_([\\dT\\.]+)_(rgb|depth)\\.png$" ## Create intermediate map structure _tstamps = {} for filename in os.listdir(folder_path): path = os.path.abspath(filename) match = re.match(fname_regex, path) if match != None: """ * 0: ../data/frame_20150312T172452.627702_depth.png * 1: 20150312T172452.627702 * 2: depth """ time_str = match.group(1) _tstamps[time_str] = True ## Convert to vector of timestamps n = len(_tstamps) tstamps = [] for key in _tstamps.keys(): tstamps.append(key) new_n = 0 for i in range(n): ## Get time string time_str = tstamps[i] ## Get file paths rgb_path = "{}/frame_{}_rgb.jpg".format(folder_path, time_str) dep_path = "{}/frame_{}_depth.png".format(folder_path, time_str) rgb_img = cv.imread(rgb_path) # _depth_img = cv.imread(dep_path, cv.IMREAD_ANYDEPTH) # load 16bit img; 2 refers to CV_LOAD_IMAGE_ANYDEPTH _depth_img = cv.imread(dep_path, 2) ''' cv.imshow("rgb_img", rgb_img) cv.waitKey() cv.destroyAllWindows() ''' ## White-balance RGB image ''' wb = xphoto.createGrayworldWB() wb.setSaturationThreshold(0.9) # This threshold is set manually rgb_undist_img = wb.balanceWhite(rgb_img) ''' rgb_undist_img = np.zeros_like(rgb_img) cv.xphoto.balanceWhite(rgb_img, rgb_undist_img, 0) ## Get grayscale image gray_undist_img = cv.cvtColor(rgb_undist_img, cv.COLOR_BGR2GRAY) ## Bm1 gray_undist_img = cv.cvtColor(rgb_img, cv.COLOR_BGR2GRAY) ''' cv.imshow("gray_undist_img", gray_undist_img) cv.waitKey() cv.destroyAllWindows() ''' ## Convert depth map to floats # depth_img = (_depth_img.astype(float)) / 65535 # depth_img = img_as_float(_depth_img) depth_img = np.float32(_depth_img) ''' cv.imshow("depth_img", depth_img) cv.waitKey() cv.destroyAllWindows() ''' ## Smooth depth map depth_smooth_img = cv.bilateralFilter(depth_img, 5, 10, 10) image = structures.Image(i, time_str, rgb_undist_img, gray_undist_img, depth_smooth_img, rgb_path, dep_path) images.append(image) new_n += 1 print("Loaded {} RGB_D images.".format(new_n)) return images