def __init__(self, config=None): self.config = config self.init = False self.surf_last = None self.corner_last = None self.feature_extractor = FeatureExtract(config=config) self.rot_w_curr = np.eye(3) self.trans_w_curr = np.zeros((3,1)) self.transform = np.array([0., 0., 0., 0., 0., 0.]) # rx, ry, rz, tx, ty, tz self.frame_count = 0 if config is None: self.DIST_THRES = 5 self.RING_INDEX = 4 self.NEARBY_SCAN = 2.5 self.OPTIM_ITERATION = 25 self.VOXEL_SIZE = 0.2 else: self.DIST_THRES = config['odometry']['dist_threshold'] self.RING_INDEX = config['odometry']['ring_index'] self.NEARBY_SCAN = config['odometry']['nearby_scan'] self.OPTIM_ITERATION = config['odometry']['optim_iteration'] self.VOXEL_SIZE = config['odometry']['voxel_size'] # For test self.trans_list = []
def get_data(path): data = [] images = get_images(path) for obj in images: feat_extract = FeatureExtract(obj["image"]) kp, des = feat_extract.extract_features() data_obj = {"des": des} data_obj.update(obj) data.append(data_obj) return data
def __init__(self, config=None): self.config = config self.init = False self.surf_last = None self.corner_last = None self.feature_extractor = FeatureExtract() self.transform = np.array([0, 0, 0, 0, 0, 0]) # rx, ry, rz, tx, ty, tz # TODO: make below variables to config self.DIST_THRES = 25 self.RING_INDEX = 4 self.NEARBY_SCAN = 2.5 self.OPTIM_ITERATION = 2 self.DISTORTION = False self.USE_ROBUST_LOSS = True
def extractFeatures(color_dic): write_to_csv(os.path.join(new_dir, 'color.csv'), ['id', 'color'], color_dic) # cv2.waitKey(0) # cv2.destroyAllWindows() feature_extract = FeatureExtract(dir=new_dir) ref_segments = feature_extract.get_image_names() color_features = feature_extract.read_from_csv() print('Extracting features..') full_features = feature_extract.extract_features(ref_segments, color_features) # print(color_features) # print(full_features) print('Writing features..') if feature_extract.write_to_csv( ['id', 'color', 'area', 'center', 'has_child'], full_features): print('Segmentation complete!') return (True, feature_extract.ref_csv_path) else: return (False, '')
def df_extract(): dataer = FeatureExtract() features = dataer.DF(5000) write_file("./data/df.txt", features)
def mi_extract(): dataer = FeatureExtract() features = dataer.MI(5000) write_file("./data/mi.txt", features)
def ig_extract(): dataer = FeatureExtract() features = dataer.IG(5000) write_file("./data/ig.txt", features)
def chi_extract(): dataer = FeatureExtract() features = dataer.CHI(5000) write_file("./data/chi.txt", features)
class Odometry: def __init__(self, config=None): self.config = config self.init = False self.surf_last = None self.corner_last = None self.feature_extractor = FeatureExtract() self.transform = np.array([0, 0, 0, 0, 0, 0]) # rx, ry, rz, tx, ty, tz # TODO: make below variables to config self.DIST_THRES = 25 self.RING_INDEX = 4 self.NEARBY_SCAN = 2.5 self.OPTIM_ITERATION = 2 self.DISTORTION = False self.USE_ROBUST_LOSS = True def angle_norm(self, angle): if angle <= -math.pi: angle += math.pi elif angle > math.pi: angle -= math.pi return angle def grab_frame(self, cloud): corner_sharp, corner_less, surf_flat, surf_less = self.feature_extractor.feature_extract(cloud) if not self.init: self.init = True self.surf_last = surf_less self.corner_last = corner_less else: weight = 1.0 if self.USE_ROBUST_LOSS: loss = HuberLoss.Huber(1.0) else: loss = None for opt_iter in range(self.OPTIM_ITERATION): single_pose_graph = FactorGraph() # corner_points, corner_points_a, corner_points_b = self.get_corner_correspondences(corner_sharp) surf_points, surf_points_a, surf_points_b, surf_points_c = self.get_surf_correspondences(surf_flat) print("Surf points: ", len(surf_points)) for i in range(len(surf_points)): single_pose_graph.add(PlaneFactor(key('p', 0), surf_points[i], surf_points_a[i], surf_points_b[i], surf_points_c[i], weight, loss)) init_pose = Variables() init_pose.add(key('p', 0), self.transform) opt_param = LevenbergMarquardtOptimizerParams() opt_param.max_iterations = 3 opt_param.verbosity_level = NonlinearOptimizerVerbosityLevel.ITERATION opt = LevenbergMarquardtOptimizer(opt_param) opt_pose = Variables() status = opt.optimize(single_pose_graph, init_pose, opt_pose) if status is NonlinearOptimizationStatus.SUCCESS: print("Optimiazation error: ", status) print("Optimizied values: ", opt_pose.at(key('p', 0))) self.transform = opt_pose.at(key('p', 0)).copy() self.transform[0] = self.angle_norm(self.transform[0]) self.transform[1] = self.angle_norm(self.transform[1]) self.transform[2] = self.angle_norm(self.transform[2]) def get_corner_correspondences(self, corner_sharp): curr_points = [] points_a = [] points_b = [] corner_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.corner_last[:, :3])) for i in range(corner_sharp.shape[0]): point_sel = self.transform_to_start(corner_sharp[i, :3]) [_, ind, dist] = corner_last_tree.search_knn_vector_3d(point_sel, 1) closest_ind = -1 min_ind2 = -1 if dist[0] < self.DIST_THRES: closest_ind = ind[0] closest_scan_id = self.corner_last[ind[0], self.RING_INDEX] min_sq_dist2 = self.DIST_THRES for j in range(closest_ind+1, self.corner_last.shape[0]): if self.corner_last[j, self.RING_INDEX] <= closest_scan_id: continue if self.corner_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.corner_last[j, self.RING_INDEX] - point_sel)) if point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j for j in range(closest_ind-1, -1, -1): if self.corner_last[j, self.RING_INDEX] >= closest_scan_id: continue if self.corner_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.corner_last[j, self.RING_INDEX] - point_sel)) if point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j if min_ind2 >= 0: curr_points.append(corner_sharp[i, :3]) points_a.append(self.corner_last[closest_ind, :3]) points_b.append(self.corner_last[min_ind2, :3]) return curr_points, points_a, points_b def get_surf_correspondences(self, surf_flat): curr_points = [] points_a = [] points_b = [] points_c = [] surf_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.surf_last[:, :3])) for i in range(surf_flat.shape[0]): point_sel = self.transform_to_start(surf_flat[i,:3]) [_, ind, dist] = surf_last_tree.search_knn_vector_3d(point_sel, 1) closest_ind = -1 min_ind2 = -1 min_ind3 = -1 if dist[0] < self.DIST_THRES: closest_ind = ind[0] closest_scan_id = self.surf_last[ind[0], self.RING_INDEX] min_sq_dist2 = self.DIST_THRES min_sq_dist3 = self.DIST_THRES for j in range(closest_ind+1, self.surf_last.shape[0]): if self.surf_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) if self.surf_last[j, self.RING_INDEX] <= closest_scan_id and point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j elif self.surf_last[j, self.RING_INDEX] > closest_scan_id and point_sq_dist < min_sq_dist3: min_sq_dist3 = point_sq_dist min_ind3 = j for j in range(closest_ind-1, -1, -1): if self.surf_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) if self.surf_last[j, self.RING_INDEX] <= closest_scan_id and point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j elif self.surf_last[j, self.RING_INDEX] > closest_scan_id and point_sq_dist < min_sq_dist3: min_sq_dist3 = point_sq_dist min_ind3 = j if min_ind2 >= 0 and min_ind3 >= 0: ab_dist = np.sum(np.square(self.surf_last[min_ind2, :3]-self.surf_last[closest_ind, :3])) ac_dist = np.sum(np.square(self.surf_last[min_ind3, :3]-self.surf_last[closest_ind, :3])) if ab_dist < 1e-3 or ac_dist < 1e-3: continue curr_points.append(surf_flat[i, :3]) points_a.append(self.surf_last[closest_ind, :3]) points_b.append(self.surf_last[min_ind2, :3]) points_c.append(self.surf_last[min_ind3, :3]) return curr_points, points_a, points_b, points_c def transform_to_start(self, pt): s = 1.0 if self.DISTORTION: s = 0.5 # TODO: hard code scaled_transform = s * self.transform rot_mat = get_rotation(scaled_transform[0], scaled_transform[1], scaled_transform[2]) translation = scaled_transform[3:6] undistorted_pt = np.transpose(rot_mat).dot(pt.reshape(3,1) - translation.reshape(3,1)) return undistorted_pt
class Odometry: def __init__(self, config=None): self.config = config self.init = False self.surf_last = None self.corner_last = None self.feature_extractor = FeatureExtract(config=config) self.rot_w_curr = np.eye(3) self.trans_w_curr = np.zeros((3,1)) self.transform = np.array([0., 0., 0., 0., 0., 0.]) # rx, ry, rz, tx, ty, tz self.frame_count = 0 if config is None: self.DIST_THRES = 5 self.RING_INDEX = 4 self.NEARBY_SCAN = 2.5 self.OPTIM_ITERATION = 25 self.VOXEL_SIZE = 0.2 else: self.DIST_THRES = config['odometry']['dist_threshold'] self.RING_INDEX = config['odometry']['ring_index'] self.NEARBY_SCAN = config['odometry']['nearby_scan'] self.OPTIM_ITERATION = config['odometry']['optim_iteration'] self.VOXEL_SIZE = config['odometry']['voxel_size'] # For test self.trans_list = [] def grab_frame(self, cloud): corner_sharp, corner_less, surf_flat, surf_less = self.feature_extractor.feature_extract(cloud) is_degenerate = False T_w_curr = np.eye(4) print("Processing frame: ", self.frame_count) if not self.init: self.init = True else: P_mat = np.identity(6) if self.surf_last.shape[0] < 100 or self.corner_last.shape[0] < 10: print("Warning: too few points in last frame") return self.surf_last, self.corner_last, T_w_curr for opt_iter in range(self.OPTIM_ITERATION): if opt_iter % 5 == 0: corner_points, corner_points_a, corner_points_b = self.get_corner_correspondences(corner_sharp) surf_points, surf_points_a, surf_points_b, surf_points_c = self.get_surf_correspondences(surf_flat) edge_A, edge_B = self.get_edge_mat(corner_points, corner_points_a, corner_points_b, opt_iter) surf_A, surf_B = self.get_plane_mat(surf_points, surf_points_a, surf_points_b, surf_points_c, opt_iter) A_mat = np.vstack((edge_A, surf_A)) B_mat = np.vstack((edge_B, surf_B)) * -0.05 # Reference to original LOAM if A_mat.shape[0] < 10: print('Warning: too few matches') continue AtA = np.matmul(A_mat.T, A_mat) AtB = np.matmul(A_mat.T, B_mat) X_mat = np.linalg.solve(AtA, AtB) if opt_iter == 0: vals, vecs = np.linalg.eig(AtA) eigen_vec = vecs.copy() for i in range(6): if vals[i] < 10: print("Warning: Degenerate!") is_degenerate = True eigen_vec[:, i] = np.zeros(6) else: break P_mat = np.matmul(np.linalg.inv(vecs), eigen_vec) if is_degenerate: X_mat = np.matmul(P_mat, X_mat) self.transform += np.squeeze(X_mat) delta_r = np.linalg.norm(np.rad2deg(X_mat[:3])) delta_t = np.linalg.norm(X_mat[3:] * 100) # print("{} frame, {} iter, [{},{},{}] delta translation".format(self.frame_count, opt_iter, self.transform[3], self.transform[4], self.transform[5])) if delta_r < 0.1 and delta_t < 0.1: print("Odometry converged.") break print("Transform: ", self.transform) T_last_curr = np.eye(4) T_w_last = np.eye(4) T_last_curr[0:3, 0:3] = get_rotation(self.transform[0], self.transform[1], self.transform[2]).T T_last_curr[0:3, 3] = -np.matmul(T_last_curr[0:3, 0:3], self.transform[3:].reshape(3,1)).reshape(3) T_w_last[0:3, 0:3] = self.rot_w_curr T_w_last[0:3, 3] = self.trans_w_curr.reshape(3) T_w_curr = np.matmul(T_w_last, T_last_curr) self.rot_w_curr = T_w_curr[0:3, 0:3] self.trans_w_curr = T_w_curr[0:3, 3].reshape(3,1) self.trans_list.append(self.trans_w_curr) # Transform surf_less and corner_less to the end for i in range(surf_less.shape[0]): surf_less[i, :3] = self.transform_to_end(surf_less[i, :3], surf_less[i, -1]).T for i in range(corner_less.shape[0]): corner_less[i, :3] = self.transform_to_end(corner_less[i, :3], corner_less[i, -1]).T if surf_less.shape[0] > 100 and corner_less.shape[0] > 10: self.surf_last = surf_less self.corner_last = corner_less self.frame_count += 1 return self.surf_last, self.corner_last, T_w_curr def get_corner_correspondences(self, corner_sharp): curr_points = [] points_a = [] points_b = [] corner_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.corner_last[:, :3])) for i in range(corner_sharp.shape[0]): point_sel = self.transform_to_start(corner_sharp[i, :3], corner_sharp[i, -1]) [_, ind, dist] = corner_last_tree.search_knn_vector_3d(point_sel, 1) closest_ind = -1 min_ind2 = -1 if dist[0] < self.DIST_THRES: closest_ind = ind[0] closest_scan_id = self.corner_last[ind[0], self.RING_INDEX] min_sq_dist2 = self.DIST_THRES * self.DIST_THRES for j in range(closest_ind+1, corner_sharp.shape[0]): if self.corner_last[j, self.RING_INDEX] <= closest_scan_id: continue if self.corner_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.corner_last[j, :3].reshape(3,1) - point_sel)) if point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j for j in range(closest_ind-1, -1, -1): if self.corner_last[j, self.RING_INDEX] >= closest_scan_id: continue if self.corner_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.corner_last[j, :3].reshape(3,1) - point_sel)) if point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j if min_ind2 >= 0: ab_dist = np.sum(np.square(self.corner_last[min_ind2, :3]-self.corner_last[closest_ind, :3])) if ab_dist < 1e-3: continue curr_points.append(corner_sharp[i, :]) points_a.append(self.corner_last[closest_ind, :]) points_b.append(self.corner_last[min_ind2, :]) return curr_points, points_a, points_b def get_surf_correspondences(self, surf_flat): curr_points = [] points_a = [] points_b = [] points_c = [] surf_last_tree = o3d.geometry.KDTreeFlann(np.transpose(self.surf_last[:, :3])) for i in range(surf_flat.shape[0]): point_sel = self.transform_to_start(surf_flat[i,:3], surf_flat[i, -1]) [_, ind, dist] = surf_last_tree.search_knn_vector_3d(point_sel, 1) closest_ind = -1 min_ind2 = -1 min_ind3 = -1 if dist[0] < self.DIST_THRES: closest_ind = ind[0] closest_scan_id = self.surf_last[ind[0], self.RING_INDEX] min_sq_dist2 = self.DIST_THRES * self.DIST_THRES min_sq_dist3 = self.DIST_THRES * self.DIST_THRES for j in range(closest_ind+1, surf_flat.shape[0]): if self.surf_last[j, self.RING_INDEX] > closest_scan_id + self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) if self.surf_last[j, self.RING_INDEX] <= closest_scan_id and point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j elif point_sq_dist < min_sq_dist3: min_sq_dist3 = point_sq_dist min_ind3 = j for j in range(closest_ind-1, -1, -1): if self.surf_last[j, self.RING_INDEX] < closest_scan_id - self.NEARBY_SCAN: break point_sq_dist = np.sum(np.square(self.surf_last[j, :3].reshape(3,1) - point_sel)) if self.surf_last[j, self.RING_INDEX] >= closest_scan_id and point_sq_dist < min_sq_dist2: min_sq_dist2 = point_sq_dist min_ind2 = j elif point_sq_dist < min_sq_dist3: min_sq_dist3 = point_sq_dist min_ind3 = j if min_ind2 >= 0 and min_ind3 >= 0: ab_dist = np.sum(np.square(self.surf_last[min_ind2, :3]-self.surf_last[closest_ind, :3])) ac_dist = np.sum(np.square(self.surf_last[min_ind3, :3]-self.surf_last[closest_ind, :3])) if ab_dist < 1e-3 or ac_dist < 1e-3: continue curr_points.append(surf_flat[i, :]) points_a.append(self.surf_last[closest_ind, :]) points_b.append(self.surf_last[min_ind2, :]) points_c.append(self.surf_last[min_ind3, :]) return curr_points, points_a, points_b, points_c def get_downsample_cloud(self, cloud): o3d_cloud = o3d.geometry.PointCloud() o3d_cloud.points = o3d.utility.Vector3dVector(cloud[:, :3]) max_bound = o3d_cloud.get_max_bound() + self.VOXEL_SIZE * 0.5 min_bound = o3d_cloud.get_min_bound() - self.VOXEL_SIZE * 0.5 out = o3d_cloud.voxel_down_sample_and_trace(self.VOXEL_SIZE, min_bound, max_bound, False) index_ds = [cubic_index[0] for cubic_index in out[2]] return index_ds def transform_to_start(self, pt, s=1.0): scaled_transform = s * self.transform rot_mat = get_rotation(scaled_transform[0], scaled_transform[1], scaled_transform[2]) translation = scaled_transform[3:6] if len(pt.shape) == 1: pt = pt.reshape(3,-1) if pt.shape[0] != 3: pt = pt.T undistorted_pt = np.transpose(rot_mat).dot(pt - translation.reshape(3,1)) return undistorted_pt def transform_to_end(self, pt, s=1.0): un_point = self.transform_to_start(pt, s) rot_mat = get_rotation(self.transform[0], self.transform[1], self.transform[2]) translation = self.transform[3:] pt_end = rot_mat.dot(un_point) + translation.reshape(3,1) return pt_end def get_plane_mat(self, surf_points, surf_points_a, surf_points_b, surf_points_c, iter_num): A_mat = [] B_mat = [] srx = np.sin(self.transform[0]) crx = np.cos(self.transform[0]) sry = np.sin(self.transform[1]) cry = np.cos(self.transform[1]) srz = np.sin(self.transform[2]) crz = np.cos(self.transform[2]) tx = self.transform[3] ty = self.transform[4] tz = self.transform[5] weight = 1.0 for i in range(len(surf_points)): s = surf_points[i][-1] pt = surf_points[i][:3].reshape(3,1) pt_a = surf_points_a[i][:3].reshape(3,1) pt_b = surf_points_b[i][:3].reshape(3,1) pt_c = surf_points_c[i][:3].reshape(3,1) pt_sel = self.transform_to_start(pt, s) plane_norm = np.cross((pt_b - pt_a), (pt_c - pt_a), axis=0) norm = np.linalg.norm(plane_norm) plane_norm = plane_norm / norm dist = np.dot(np.transpose(plane_norm),(pt_sel - pt_a)) if iter_num >= 5: weight = 1 - 1.8 * abs(dist) / np.sqrt(np.linalg.norm(pt_sel)) if norm < 1e-5 or weight <= 0.1: continue plane_norm = weight * plane_norm A_tmp = np.zeros((1,6)) B_tmp = np.zeros((1,1)) B_tmp[0, 0] = dist * weight A_tmp[0, 0] = (-crx*sry*srz*pt[0] + crx*crz*sry*pt[1] + srx*sry*pt[2] \ + tx*crx*sry*srz - ty*crx*crz*sry - tz*srx*sry) * plane_norm[0] \ + (srx*srz*pt[0] - crz*srx*pt[1] + crx*pt[2] \ + ty*crz*srx - tz*crx - tx*srx*srz) * plane_norm[1] \ + (crx*cry*srz*pt[0] - crx*cry*crz*pt[1] - cry*srx*pt[2] \ + tz*cry*srx + ty*crx*cry*crz - tx*crx*cry*srz) * plane_norm[2] A_tmp[0, 1] = ((-crz*sry - cry*srx*srz)*pt[0] \ + (cry*crz*srx - sry*srz)*pt[1] - crx*cry*pt[2] \ + tx*(crz*sry + cry*srx*srz) + ty*(sry*srz - cry*crz*srx) \ + tz*crx*cry) * plane_norm[0] \ + ((cry*crz - srx*sry*srz)*pt[0] \ + (cry*srz + crz*srx*sry)*pt[1] - crx*sry*pt[2] \ + tz*crx*sry - ty*(cry*srz + crz*srx*sry) \ - tx*(cry*crz - srx*sry*srz)) * plane_norm[2] A_tmp[0, 2] = ((-cry*srz - crz*srx*sry)*pt[0] + (cry*crz - srx*sry*srz)*pt[1] \ + tx*(cry*srz + crz*srx*sry) - ty*(cry*crz - srx*sry*srz)) * plane_norm[0] \ + (-crx*crz*pt[0] - crx*srz*pt[1] \ + ty*crx*srz + tx*crx*crz) * plane_norm[1] \ + ((cry*crz*srx - sry*srz)*pt[0] + (crz*sry + cry*srx*srz)*pt[1] \ + tx*(sry*srz - cry*crz*srx) - ty*(crz*sry + cry*srx*srz)) * plane_norm[2] A_tmp[0, 3] = -(cry*crz - srx*sry*srz) * plane_norm[0] + crx*srz * plane_norm[1] \ - (crz*sry + cry*srx*srz) * plane_norm[2] A_tmp[0, 4] = -(cry*srz + crz*srx*sry) * plane_norm[0] - crx*crz * plane_norm[1] \ - (sry*srz - cry*crz*srx) * plane_norm[2] A_tmp[0, 5] = crx*sry * plane_norm[0] - srx * plane_norm[1] - crx*cry * plane_norm[2] A_mat.append(A_tmp) B_mat.append(B_tmp) if len(A_mat) != 0 and len(B_mat) !=0: A_mat = np.vstack(A_mat) B_mat = np.vstack(B_mat) else: A_mat = np.zeros((0,6)) B_mat = np.zeros((0,1)) return A_mat, B_mat def get_edge_mat(self, corner_points, corner_points_a, corner_points_b, iter_num): A_mat = [] B_mat = [] srx = np.sin(self.transform[0]) crx = np.cos(self.transform[0]) sry = np.sin(self.transform[1]) cry = np.cos(self.transform[1]) srz = np.sin(self.transform[2]) crz = np.cos(self.transform[2]) tx = self.transform[3] ty = self.transform[4] tz = self.transform[5] weight = 1.0 for i in range(len(corner_points)): s = corner_points[i][-1] pt = corner_points[i][:3].reshape(3,1) pt_a = corner_points_a[i][:3].reshape(3,1) pt_b = corner_points_b[i][:3].reshape(3,1) pt_sel = self.transform_to_start(pt, s) edge_normal = np.cross((pt_sel - pt_a), (pt_sel - pt_b), axis=0) ab = pt_a - pt_b ab_norm = np.linalg.norm(ab) edge_norm = np.linalg.norm(edge_normal) if iter_num >= 5: weight = 1 - 1.8 * abs(edge_norm / ab_norm) if edge_norm < 1e-5 or weight <= 0.1: continue la = weight * (ab[1]*edge_normal[2] + ab[2]*edge_normal[1]) / (ab_norm*edge_norm) lb = -weight * (ab[0]*edge_normal[2] + ab[2]*edge_normal[0]) / (ab_norm*edge_norm) lc = weight * (ab[0]*edge_normal[1] - ab[1]*edge_normal[0]) / (ab_norm*edge_norm) A_tmp = np.zeros((1,6)) B_tmp = np.zeros((1,1)) B_tmp[0, 0] = weight * (edge_norm / ab_norm) A_tmp[0, 0] = (-crx*sry*srz*pt[0] + crx*crz*sry*pt[1] + srx*sry*pt[2] \ + tx*crx*sry*srz - ty*crx*crz*sry - tz*srx*sry) * la \ + (srx*srz*pt[0] - crz*srx*pt[1] + crx*pt[2] \ + ty*crz*srx - tz*crx - tx*srx*srz) * lb \ + (crx*cry*srz*pt[0] - crx*cry*crz*pt[1] - cry*srx*pt[2] \ + tz*cry*srx + ty*crx*cry*crz - tx*crx*cry*srz) * lc A_tmp[0, 1] = ((-crz*sry - cry*srx*srz)*pt[0] \ + (cry*crz*srx - sry*srz)*pt[1] - crx*cry*pt[2] \ + tx*(crz*sry + cry*srx*srz) + ty*(sry*srz - cry*crz*srx) \ + tz*crx*cry) * la \ + ((cry*crz - srx*sry*srz)*pt[0] \ + (cry*srz + crz*srx*sry)*pt[1] - crx*sry*pt[2] \ + tz*crx*sry - ty*(cry*srz + crz*srx*sry) \ - tx*(cry*crz - srx*sry*srz)) * lc A_tmp[0, 2] = ((-cry*srz - crz*srx*sry)*pt[0] + (cry*crz - srx*sry*srz)*pt[1] \ + tx*(cry*srz + crz*srx*sry) - ty*(cry*crz - srx*sry*srz)) * la \ + (-crx*crz*pt[0] - crx*srz*pt[1] \ + ty*crx*srz + tx*crx*crz) * lb \ + ((cry*crz*srx - sry*srz)*pt[0] + (crz*sry + cry*srx*srz)*pt[1] \ + tx*(sry*srz - cry*crz*srx) - ty*(crz*sry + cry*srx*srz)) * lc A_tmp[0, 3] = -(cry*crz - srx*sry*srz) * la + crx*srz * lb \ - (crz*sry + cry*srx*srz) * lc A_tmp[0, 4] = -(cry*srz + crz*srx*sry) * la - crx*crz * lb \ - (sry*srz - cry*crz*srx) * lc A_tmp[0, 5] = crx*sry * la - srx * lb - crx*cry * lc A_mat.append(A_tmp) B_mat.append(B_tmp) if len(A_mat) != 0 and len(B_mat) !=0: A_mat = np.vstack(A_mat) B_mat = np.vstack(B_mat) else: A_mat = np.zeros((0,6)) B_mat = np.zeros((0,1)) return A_mat, B_mat def get_trans_list(self): return self.trans_list
if args["show"] == "True": show = True limit = 2 if args["limit"] is not None: limit = args["limit"] start = time.time() data = dataset.get_data(args["data"]) end = time.time() print("Database simulation: " + str(end - start)) query_image = cv2.imread(args["image"]) start = time.time() feat_extract = FeatureExtract(query_image) kp, des = feat_extract.extract_features() bf = cv2.BFMatcher() res = [] for obj in data: matches = bf.knnMatch(des, obj["des"], k=2) good = [] for match_1, match_2 in matches: if match_1.distance < 0.75 * match_2.distance: good.append([match_1]) res.append({ "image": obj["image"], "file_name": obj["file_name"], "match": len(good) })