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)
"""
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,
bounding_box=bounding_box)
def basketball_test():
sequence = SequenceManager("../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/images",
"../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/bounding_box.mat")
# line_index, points = load_model("../../dataset/soccer_dataset/highlights_soccer_model.mat")
begin_frame = 0
first_frame_ptz = (sequence.ground_truth_pan[begin_frame],
sequence.ground_truth_tilt[begin_frame],
sequence.ground_truth_f[begin_frame])
first_camera = sequence.camera
first_camera.set_ptz(first_frame_ptz)
# 3*4 projection matrix for 1st frame
first_frame_mat = first_camera.projection_matrix
first_frame = sequence.get_image_gray(index=begin_frame, dataset_type=0)
first_bounding_box = sequence.get_bounding_box_mask(begin_frame)
# img = project_with_homography(first_frame_mat, points, line_index, first_frame)
#
# cv.imshow("image", img)
# cv.waitKey()
homography_ekf = HomographyEKF()
homography_ekf.init_system(first_frame, first_frame_mat,
first_bounding_box)
# tracking_obj = HomographyTracking(first_frame, first_frame_mat)
points3d_on_field = uniform_point_sample_on_field(25, 18, 25, 18)
pan = [first_frame_ptz[0]]
tilt = [first_frame_ptz[1]]
f = [first_frame_ptz[2]]
for i in range(1, sequence.length):
next_frame = sequence.get_image_gray(index=i, dataset_type=0)
next_bounding_box = sequence.get_bounding_box_mask(i)
homography_ekf.tracking(next_frame, next_bounding_box)
# img = project_with_homography(
# np.dot(homography_ekf.accumulate_homography[-1], homography_ekf.model_to_image_homography),
# points, line_index, next_frame)
# compute ptz
first_camera.set_ptz((pan[-1], tilt[-1], f[-1]))
current_homography = np.dot(homography_ekf.accumulate_homography[-1],
homography_ekf.model_to_image_homography)
pose = estimate_camera_from_homography(current_homography,
first_camera, points3d_on_field)
print("-----" + str(i) + "--------")
# print("hompgraphy:", homography_ekf.accumulate_homography)
print(pose)
# first_camera.set_ptz(pose)
# img2 = project_with_PTZCamera(first_camera, points, line_index, next_frame)
print("%f" % (pose[0] - sequence.ground_truth_pan[i]))
print("%f" % (pose[1] - sequence.ground_truth_tilt[i]))
print("%f" % (pose[2] - sequence.ground_truth_f[i]))
pan.append(pose[0])
tilt.append(pose[1])
f.append(pose[2])
# cv.imshow("image", img)
# cv.imshow("image2", img2)
# cv.waitKey(0)
save_camera_pose(np.array(pan), np.array(tilt), np.array(f),
"./bs4_result.mat")