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,