def getPointsProject2D(pts_2d, pts_3d, target_cam):
p_in_target = []
p_valid = []
for kp in pts_2d:
x, y = kp[0], kp[1]
p0w = Point3(pts_3d[int(y * 640 + x)])
p0_target, _ = cal_Point_Project_TartanCam(target_cam, p0w)
p_in_target.append(p0_target)
if (0 < p0_target[0] < 640) & (0 < p0_target[1] < 480):
p_valid.append(True)
else:
p_valid.append(False)
return p_in_target, p_valid
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
sp1 = pts1['pts']
desc1 = pts1['desc']
norm_self = np.sqrt(2 - 2 * np.clip(np.dot(desc0.T, desc0), -1, 1))
norm_cross = np.sqrt(2 - 2 * np.clip(np.dot(desc0.T, desc1), -1, 1))
showNorm(norm_self)
showNorm(norm_cross)
# - 3[-.-]5
num_detected = pts0['pts'].shape[0]
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, _ = cal_Point_Project_TartanCam(target_cam, p0w)
kpt0_gt.append(p0_target)
if (0 < p0_target[0] < 640) & (0 < p0_target[1] < 480):
kpt0_valid.append(True)
else:
kpt0_valid.append(False)
def hungarianMatch(norm_cross):
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, :]
return matches
outImage=None,
color=(255, 0, 0))
prev_kp1 = cv.drawKeypoints(img_prev_1,
pts1['pts_cv'],
outImage=None,
color=(0, 0, 255))
showRGB(prev_kp0)
showRGB(prev_kp1)
# pts0_in1 = []
kpt0_gt = []
for kp in pts0['pts_cv']:
x, y = kp.pt
color = tuple(np.random.random(size=3) * 256)
colorRED = [255, 0, 0]
p0w = Point3(point3D[int(y * 640 + x)])
p0_target, _ = cal_Point_Project_TartanCam(cam1, p0w)
kpt0_gt.append(p0_target)
# pts0_in1.append(cv.KeyPoint(x=float(p0_target[0]), y=float(p0_target[1]), size=8, response=kp.response))
cv.drawMarker(prev_kp1,
p0_target, [255, 0, 0],
markerType=cv.MARKER_CROSS,
markerSize=9,
thickness=1)
# prev_kp1 = cv.drawKeypoints(img_prev_1, pts1['pts_cv'], outImage=None, color=(255, 0, 0))
showRGB(prev_kp1)
# showDepth(frame0['depth'],dmax=10)
# 1. Show GT (DONE)
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