def ut_add_keyframe_with_ba():
input = SequenceManager(
"/Users/jimmy/Desktop/ptz_slam_dataset/basketball/basketball_anno.mat",
"/Users/jimmy/Desktop/ptz_slam_dataset/basketball/images",
"/Users/jimmy/PycharmProjects/ptz_slam/Camera-Calibration/basketball/objects_basketball.mat"
)
camera_center = input.get_camera_center()
base_rotation = input.get_base_rotation()
u = 1280 / 2
v = 720 / 2
image_index = [0] # 680, 690, 700, 730, 800
im = input.get_image(image_index[0])
ptz = input.get_ptz(image_index[0])
keyframe = KeyFrame(im, image_index[0], camera_center, base_rotation, u, v,
ptz[0], ptz[1], ptz[2])
a_map = Map('orb')
a_map.add_first_keyframe(keyframe, False)
# test the result frames
for i in range(1, 3600, 5):
ptz = input.get_ptz(i)
im = input.get_image(i)
keyframe = KeyFrame(im, i, camera_center, base_rotation, u, v, ptz[0],
ptz[1], ptz[2])
if a_map.good_new_keyframe(ptz, 10, 25, 1280, False):
print('add key frame from index %d, pan angle %f' % (i, ptz[0]))
a_map.add_keyframe_with_ba(keyframe, '.', True)
print('number of keyframe is %d' % (len(a_map.keyframe_list)))
def ut_add_first_key_frame():
input = SequenceManager(
"/Users/jimmy/Desktop/ptz_slam_dataset/basketball/basketball_anno.mat",
"/Users/jimmy/Desktop/ptz_slam_dataset/basketball/images",
"/Users/jimmy/PycharmProjects/ptz_slam/Camera-Calibration/basketball/objects_basketball.mat"
)
camera_center = input.get_camera_center()
base_rotation = input.get_base_rotation()
u = 1280 / 2
v = 720 / 2
image_index = [0] # 680, 690, 700, 730, 800
im = input.get_image(image_index[0])
ptz = input.get_ptz(image_index[0])
keyframe = KeyFrame(im, image_index[0], camera_center, base_rotation, u, v,
ptz[0], ptz[1], ptz[2])
a_map = Map()
a_map.add_first_keyframe(keyframe, True)
def ut_relocalization():
"""unit test for relocalization"""
obj = SequenceManager(
"../../dataset/basketball/basketball_anno.mat",
"../../dataset/basketball/images",
"../../dataset/basketball/basketball_ground_truth.mat",
"../../dataset/basketball/objects_basketball.mat")
img1 = 0
img2 = 390
gray1 = obj.get_image_gray(img1)
gray2 = obj.get_image_gray(img2)
pose1 = obj.get_ptz(img1)
pose2 = obj.get_ptz(img2)
mask1 = obj.get_bounding_box_mask(img1)
mask2 = obj.get_bounding_box_mask(img2)
camera = obj.get_camera(0)
keyframe1 = KeyFrame(gray1, img1, camera.camera_center,
camera.base_rotation, camera.principal_point[0],
camera.principal_point[1], pose1[0], pose1[1],
pose1[2])
keyframe2 = KeyFrame(gray2, img2, camera.camera_center,
camera.base_rotation, camera.principal_point[0],
camera.principal_point[1], pose2[0], pose2[1],
pose2[2])
kp1, des1 = detect_compute_sift(gray1, 100)
after_removed_index1 = keypoints_masking(kp1, mask1)
kp1 = list(np.array(kp1)[after_removed_index1])
des1 = des1[after_removed_index1]
kp2, des2 = detect_compute_sift(gray2, 100)
after_removed_index2 = keypoints_masking(kp2, mask2)
kp2 = list(np.array(kp2)[after_removed_index2])
des2 = des2[after_removed_index2]
keyframe1.feature_pts = kp1
keyframe1.feature_des = des1
keyframe2.feature_pts = kp2
keyframe2.feature_des = des2
kp1_inlier, index1, kp2_inlier, index2 = match_sift_features(
kp1, des1, kp2, des2)
cv.imshow(
"test",
draw_matches(obj.get_image(img1), obj.get_image(img2), kp1_inlier,
kp2_inlier))
cv.waitKey(0)
map = Map()
"""first frame"""
for i in range(len(kp1)):
theta, phi = TransFunction.from_image_to_ray(obj.u, obj.v, pose1[2],
pose1[0], pose1[1],
kp1[i].pt[0],
kp1[i].pt[1])
map.global_ray.append(np.array([theta, phi]))
keyframe1.landmark_index = np.array([i for i in range(len(kp1))])
"""second frame"""
keyframe2.landmark_index = np.ndarray([len(kp2)], dtype=np.int32)
for i in range(len(kp2_inlier)):
keyframe2.landmark_index[index2[i]] = index1[i]
kp2_outlier_index = list(set([i for i in range(len(des2))]) - set(index2))
for i in range(len(kp2_outlier_index)):
theta, phi = TransFunction.from_image_to_ray(
obj.u, obj.v, pose2[2], pose2[0], pose2[1],
kp2[kp2_outlier_index[i]].pt[0], kp2[kp2_outlier_index[i]].pt[1])
map.global_ray.append(np.array([theta, phi]))
keyframe2.landmark_index[kp2_outlier_index[i]] = len(
map.global_ray) - 1
map.keyframe_list.append(keyframe1)
map.keyframe_list.append(keyframe2)
pose_test = obj.get_ptz(142)
optimized = relocalization_camera(map=map,
img=obj.get_image_gray(142),
pose=np.array([20, -16, 3000]))
print(pose_test)
print(optimized.x)