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 add_keyframe(self, img, camera, frame_index): """ add new key frame. @todo now have not changed the KeyFrame's parameter to camera object. @todo Many places need to be changed if this change. :param img: image :param camera: camera object for key frame :param frame_index: frame index in sequence """ c = camera.camera_center r = camera.base_rotation u = camera.principal_point[0] v = camera.principal_point[1] pan = camera.pan tilt = camera.tilt focal_length = camera.focal_length new_keyframe = KeyFrame(img, frame_index, c, r, u, v, pan, tilt, focal_length) if frame_index == 0: self.keyframe_map.add_first_keyframe(new_keyframe, verbose=True) else: self.keyframe_map.add_keyframe_with_ba(new_keyframe, "./bundle_result/", verbose=True) self.new_keyframe = False
def relocalize(self, img, camera, enable_rf=False, bounding_box=None): """ :param img: image to relocalize :param camera: lost camera to relocaize :return: camera after relocalize """ if enable_rf: c = camera.camera_center r = camera.base_rotation u = camera.principal_point[0] v = camera.principal_point[1] pan = camera.pan tilt = camera.tilt focal_length = camera.focal_length relocalize_frame = KeyFrame(img, -1, c, r, u, v, pan, tilt, focal_length) kp, des = detect_compute_sift_array(img, 500) if bounding_box is not None: masked_index = keypoints_masking(kp, bounding_box) kp = kp[masked_index] des = des[masked_index] relocalize_frame.feature_pts = kp relocalize_frame.feature_des = des ptz = self.rf_map.relocalize(relocalize_frame, [pan, tilt, focal_length]) camera.set_ptz(ptz) else: if len(self.keyframe_map.keyframe_list) > 1: lost_pose = camera.pan, camera.tilt, camera.focal_length relocalize_pose = relocalization_camera( self.keyframe_map, img, lost_pose) camera.set_ptz(relocalize_pose) else: print("Warning: Not enough keyframes for relocalization.") self.tracking_lost = False return camera
def add_keyframe(self, img, camera, frame_index, enable_rf=False): """ add new key frame. @todo now have not changed the KeyFrame's parameter to camera object. @todo Many places need to be changed if this change. :param img: image :param camera: camera object for key frame :param frame_index: frame index in sequence """ c = camera.camera_center r = camera.base_rotation u = camera.principal_point[0] v = camera.principal_point[1] pan = camera.pan tilt = camera.tilt focal_length = camera.focal_length new_keyframe = KeyFrame(img, frame_index, c, r, u, v, pan, tilt, focal_length) if enable_rf: # new_keyframe.feature_pts, new_keyframe.feature_des = detect_compute_sift_array(img, 1500) new_keyframe.feature_pts = self.previous_keypoints new_keyframe.feature_des = self.des[ self.previous_keypoints_index.astype(np.int)] self.rf_map.add_keyframe(new_keyframe) self.new_keyframe = False else: if frame_index == 0: self.keyframe_map.add_first_keyframe(new_keyframe, verbose=True) else: self.keyframe_map.add_keyframe_with_ba(new_keyframe, "./bundle_result/", verbose=True) self.new_keyframe = False
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)