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)