def cal_Point_Project_TartanCam(cam: PinholeCameraCal3_S2, pw: Point3): q = cam.pose().transformTo(pw)[[1, 2, 0]] # pn = cam.Project(q) pi = cam.calibration().uncalibrate(pn) # d = 1. / q[2] # print('debug: ',pn, pi, q, d) return np.rint(pi).astype(np.int32), q[2]
def getMatchPrecision(source_id, target_id): source = getFrameInfo(source_id) target = getFrameInfo(target_id) # source_cam = PinholeCameraCal3_S2(Pose3(source['transform']), K) target_cam = PinholeCameraCal3_S2(Pose3(target['transform']), K) src_gray = cv.cvtColor(source['color'], cv.COLOR_RGB2GRAY) src_p3d = getPoint3D(source) tar_gray = cv.cvtColor(target['color'], cv.COLOR_RGB2GRAY) pts0 = getSuperPoints_v2(src_gray) pts1 = getSuperPoints_v2(tar_gray) kpt0_gt = [] kpt0_valid = [] for kp in pts0['pts']: x, y = kp[0], kp[1] p0w = Point3(src_p3d[int(y * 640 + x)]) p0_target, d_prj = cal_Point_Project_TartanCam(target_cam, p0w) kpt0_gt.append(p0_target) if (0 < p0_target[0] < 640) & (0 < p0_target[1] < 480): d_gt = target['depth'][p0_target[1], p0_target[0]] dif = abs(d_prj / d_gt) if (0.66 < dif < 1.5): kpt0_valid.append(True) else: # print("WARNING DIFF: ", dif, d_gt) kpt0_valid.append(False) else: kpt0_valid.append(False) num_matchable = kpt0_valid.count(True) norm_self, norm_cross = getNormMatrices(pts0, pts1) match_id = linear_sum_assignment(cost_matrix=norm_cross) match_score = norm_cross[match_id] matches = np.stack((match_id[0], match_id[1], match_score), axis=1) idx_sorted = np.argsort(match_score) matches = matches[idx_sorted, :] kpt0 = pts0['pts'].astype(int) kpt1 = pts1['pts'].astype(int) kpt0_gt = np.asarray(kpt0_gt) match_state = [] for id0, id1, mVal in matches: # p0 = kpt0[int(id0)] p0_gt = kpt0_gt[int(id0)] valid = kpt0_valid[int(id0)] p1 = kpt1[int(id1)] dis = np.linalg.norm(p0_gt - p1) if (mVal < 0.7): if (dis < 8): match_state.append('TRUE') else: match_state.append('FALSE') else: if valid: match_state.append('SKIP_BUT_VALID') else: match_state.append('SKIP') precision = 0 recall = 0 if (match_state.count('TRUE') > 0): precision = match_state.count('TRUE') / (match_state.count('TRUE') + match_state.count('FALSE')) recall = match_state.count('TRUE') / ( match_state.count('TRUE') + match_state.count('SKIP_BUT_VALID')) print('Precision = ', precision, '; Recall', recall) return precision, recall
'intr': camIntr, 'extr': camExtr } id = 36 frame1 = { 'color': io.imread(files_rgb_left[id]), # 'gray': [], 'depth': np.load(files_depth_left[id]), 'transform': poses_mat44[id], 'intr': camIntr, 'extr': camExtr } K = Cal3_S2(320, 320, 0.0, 320, 240) cam0 = PinholeCameraCal3_S2(Pose3(frame0['transform']), K) cam1 = PinholeCameraCal3_S2(Pose3(frame1['transform']), K) rgbd0 = RGBDImage.create_from_color_and_depth(color=Image(frame0['color']), depth=Image(frame0['depth']), depth_scale=1.0, depth_trunc=np.inf, convert_rgb_to_intensity=False) cloud0 = PointCloud.create_from_rgbd_image(image=rgbd0, intrinsic=camIntr, extrinsic=tartan_camExtr) cloud0.transform(frame0['transform']) point3D = np.asarray(cloud0.points) input_0 = cv.cvtColor(frame0['color'], cv.COLOR_RGB2GRAY) sobel = getImgSobel(input_0)
K = Cal3_S2(320, 320, 0.0, 320, 240) def getFrameInfo(id): frame = { 'color': io.imread(files_rgb_left[id]), 'color_right': io.imread(files_rgb_right[id]), 'depth': np.load(files_depth_left[id]), 'transform': poses_mat44[id], 'intr': camIntr, 'extr': camExtr } return frame source = getFrameInfo(32) target = getFrameInfo(36) generate3D(source) src_p3d = source['point3D'] target_cam = PinholeCameraCal3_S2(Pose3(target['transform']), K) pts0 = getSuperPoints_v2(cv.cvtColor(source['color'], cv.COLOR_RGB2GRAY)) pts1 = getSuperPoints_v2(cv.cvtColor(target['color'], cv.COLOR_RGB2GRAY)) # get stereo map src_disparity = getDisparityTartanAIR(source['color'], source['color_right']) showDepth(src_disparity) # get kpt0_gt num_detected = pts0['pts'].shape[0] kpt0_gt, kpt0_valid = getPointsProject2D(pts_2d=pts0['pts'], pts_3d=source['point3D'], target_cam=target_cam)
def getMatchPrecision(source_id,target_id): source = getFrameInfo(source_id) target = getFrameInfo(target_id) source_cam = PinholeCameraCal3_S2(Pose3(source['transform']), K) target_cam = PinholeCameraCal3_S2(Pose3(target['transform']), K) src_gray = cv.cvtColor(source['color'], cv.COLOR_RGB2GRAY) src_p3d = getPoint3D(source) tar_gray = cv.cvtColor(target['color'], cv.COLOR_RGB2GRAY) # FRAME 0 frame_tensor = frame2tensor(src_gray, dnn_device) keys = ['keypoints', 'scores', 'descriptors'] last_data = matching.superpoint({'image': frame_tensor}) last_data = {k + '0': last_data[k] for k in keys} last_data['image0'] = frame_tensor last_frame = src_gray # FRAME 1 frame_tensor = frame2tensor(tar_gray, dnn_device) pred = matching({**last_data, 'image1': frame_tensor}) kpts0 = last_data['keypoints0'][0].cpu().numpy() kpts1 = pred['keypoints1'][0].cpu().numpy() matches = pred['matches0'][0].cpu().numpy() confidence = pred['matching_scores0'][0].cpu().numpy() kpt0_gt = [] kpt0_valid = [] for kp in kpts0: x, y = kp p0w = Point3(src_p3d[int(y * 640 + x)]) p0_target, d_prj = cal_Point_Project_TartanCam(target_cam, p0w) kpt0_gt.append(p0_target) if (0 < p0_target[0] < 640) & (0 < p0_target[1] < 480): d_gt = target['depth'][p0_target[1],p0_target[0]] dif = abs(d_prj/d_gt) if (0.66 < dif < 1.5): kpt0_valid.append(True) else: # print("WARNING DIFF: ", dif, d_gt) kpt0_valid.append(False) else: kpt0_valid.append(False) match_state = [] for id0, id1 in enumerate(matches): p0_gt = kpt0_gt[int(id0)] valid = kpt0_valid[int(id0)] p1 = kpts1[int(id1)] dis = np.linalg.norm(p0_gt - p1) # print(id0,id1, p1,p0_gt,valid) if (id1 > -1): if (dis < 8): match_state.append('TRUE') else: match_state.append('FALSE') else: if valid: match_state.append('SKIP_BUT_VALID') else: match_state.append('SKIP') precision = 0 recall = 0 if (match_state.count('TRUE') > 0): precision = match_state.count('TRUE') / (match_state.count('TRUE') + match_state.count('FALSE')) recall = match_state.count('TRUE') / (match_state.count('TRUE') + match_state.count('SKIP_BUT_VALID')) print('Precision = ', precision, '; Recall', recall) return precision, recall