示例#1
0
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]))
示例#3
0
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,