def ut_basketball_estimated_map():
sequence = SequenceManager("../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/images",
"../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/bounding_box.mat")
keyframe_list = []
for i in range(9):
keyframe = sio.loadmat("../../map/" + str(i) + ".mat")
keyframe_list.append(keyframe)
camera = sequence.get_camera(0)
images = []
for keyframe in keyframe_list:
img_index = keyframe['img_index'].squeeze()
img = sequence.get_image(img_index, 0)
images.append(img)
ptz_list = []
for keyframe in keyframe_list:
ptz = keyframe['camera_pose'].squeeze()
ptz_list.append(ptz)
panorama = generate_panoramic_image(camera, images, ptz_list)
cv.imshow("test", panorama)
cv.imwrite("../../map/ttt2.jpg", panorama)
cv.waitKey(0)
def ut_basketball():
"""this for basketball"""
sequence = SequenceManager(
"../../dataset/basketball/seq1/ground_truth.mat",
"../../dataset/basketball/seq1/images",
"../../dataset/basketball/seq1/ground_truth.mat",
"../../dataset/basketball/seq1/player_bounding_box.mat")
slam = PtzSlam()
first_img = sequence.get_image_gray(index=0, dataset_type=0)
first_camera = sequence.get_camera(0)
first_bounding_box = sequence.get_bounding_box_mask(0)
slam.init_system(first_img, first_camera, first_bounding_box)
slam.add_keyframe(first_img, first_camera, 0)
for i in range(1, sequence.length):
img = sequence.get_image_gray(index=i, dataset_type=0)
bounding_box = sequence.get_bounding_box_mask(i)
slam.tracking(next_img=img,
bad_tracking_percentage=80,
bounding_box=bounding_box)
if slam.tracking_lost:
relocalized_camera = slam.relocalize(img, slam.current_camera)
slam.init_system(img, relocalized_camera, bounding_box)
print("do relocalization!")
elif slam.new_keyframe:
slam.add_keyframe(img, slam.current_camera, i)
print("add keyframe!")
print("=====The ", i, " iteration=====")
print("%f" % (slam.cameras[i].pan - sequence.ground_truth_pan[i]))
print("%f" % (slam.cameras[i].tilt - sequence.ground_truth_tilt[i]))
print("%f" %
(slam.cameras[i].focal_length - sequence.ground_truth_f[i]))
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)
from util import save_camera_pose
"""
PTZ SLAM experiment code for the soccer sequence.
system parameters:
init_system and add_rays: detect 300 orb keypoints each frame
good new keyframe: 10 ~ 15
images have been blurred. So there is no need to add mask to bounding_box
"""
sequence = SequenceManager("../dataset/soccer/ground_truth.mat",
"../dataset/soccer/images",
"../dataset/soccer/ground_truth.mat",
"../dataset/soccer/player_bounding_box.mat")
slam = PtzSlam()
first_img = sequence.get_image_gray(index=0, dataset_type=1)
first_camera = sequence.get_camera(0)
first_bounding_box = sequence.get_bounding_box_mask(0)
slam.init_system(first_img, first_camera, bounding_box=first_bounding_box)
slam.add_keyframe(first_img, first_camera, 0, enable_rf=False)
# slam.add_keyframe_random_forest(first_img, first_camera, 0)
pan_list = [first_camera.get_ptz()[0]]
tilt_list = [first_camera.get_ptz()[1]]
zoom_list = [first_camera.get_ptz()[2]]
for i in range(1, sequence.length):
img = sequence.get_image_gray(index=i, dataset_type=1)
bounding_box = sequence.get_bounding_box_mask(i)
slam.tracking(next_img=img,
bad_tracking_percentage=80,