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