def show_image_sequence(self, pts):
for i in range(self.annotation.size):
img = np.zeros((720, 1280, 3), np.uint8)
img.fill(255)
pan, tilt, f = self.annotation[0][i]['ptz'].squeeze()
self.draw_soccer_line(img, pan, tilt, f)
# draw the feature points in images
for j in range(len(pts)):
p = np.array(pts[j])
res = TransFunction.from_3d_to_2d(self.u, self.v, f, pan, tilt, self.proj_center, self.base_rotation, p)
res2 = TransFunction.from_pan_tilt_to_2d(self.u, self.v, f, pan, tilt, rays[j][0], rays[j][1])
if 0 < res[0] < 1280 and 0 < res[1] < 720:
print(p)
print("ray", rays[j][0], rays[j][1])
print("res:, ", res)
print("res2: ", res2)
print("==========")
cv.circle(img, (int(res[0]), int(res[1])), color=(0, 0, 0), radius=8, thickness=2)
cv.circle(img, (int(res2[0]), int(res2[1])), color=(255, 0, 0), radius=8, thickness=2)
cv.imshow("synthesized image", img)
cv.waitKey(0)
def compute_new_jacobi(self, camera_pan, camera_tilt, foc, rays):
ray_num = len(rays)
delta_angle = 0.001
delta_f = 0.1
jacobi_h = np.ndarray([2 * ray_num, 3 + 2 * ray_num])
for i in range(ray_num):
x_delta_pan1, y_delta_pan1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan - delta_angle, camera_tilt, rays[i][0], rays[i][1])
x_delta_pan2, y_delta_pan2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan + delta_angle, camera_tilt, rays[i][0], rays[i][1])
x_delta_tilt1, y_delta_tilt1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt - delta_angle, rays[i][0], rays[i][1])
x_delta_tilt2, y_delta_tilt2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt + delta_angle, rays[i][0], rays[i][1])
x_delta_f1, y_delta_f1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc - delta_f, camera_pan, camera_tilt, rays[i][0], rays[i][1])
x_delta_f2, y_delta_f2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc + delta_f, camera_pan, camera_tilt, rays[i][0], rays[i][1])
x_delta_theta1, y_delta_theta1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt, rays[i][0] - delta_angle, rays[i][1])
x_delta_theta2, y_delta_theta2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt, rays[i][0] + delta_angle, rays[i][1])
x_delta_phi1, y_delta_phi1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt, rays[i][0], rays[i][1] - delta_angle)
x_delta_phi2, y_delta_phi2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt, rays[i][0], rays[i][1] + delta_angle)
jacobi_h[2 * i][0] = (x_delta_pan2 - x_delta_pan1) / (2 * delta_angle)
jacobi_h[2 * i][1] = (x_delta_tilt2 - x_delta_tilt1) / (2 * delta_angle)
jacobi_h[2 * i][2] = (x_delta_f2 - x_delta_f1) / (2 * delta_f)
jacobi_h[2 * i + 1][0] = (y_delta_pan2 - y_delta_pan1) / (2 * delta_angle)
jacobi_h[2 * i + 1][1] = (y_delta_tilt2 - y_delta_tilt1) / (2 * delta_angle)
jacobi_h[2 * i + 1][2] = (y_delta_f2 - y_delta_f1) / (2 * delta_f)
for j in range(ray_num):
if j == i:
jacobi_h[2 * i][3 + 2 * j] = (x_delta_theta2 - x_delta_theta1) / (2 * delta_angle)
jacobi_h[2 * i][3 + 2 * j + 1] = (x_delta_phi2 - x_delta_phi1) / (2 * delta_angle)
jacobi_h[2 * i + 1][3 + 2 * j] = (y_delta_theta2 - y_delta_theta1) / (2 * delta_angle)
jacobi_h[2 * i + 1][3 + 2 * j + 1] = (y_delta_phi2 - y_delta_phi1) / (2 * delta_angle)
else:
jacobi_h[2 * i][3 + 2 * j] = jacobi_h[2 * i][3 + 2 * j + 1] = \
jacobi_h[2 * i + 1][3 + 2 * j] = jacobi_h[2 * i + 1][3 + 2 * j + 1] = 0
return jacobi_h
def _recompute_matching_ray(keyframe, img, feature_method):
"""
:param keyframe: keyframe object to match
:param img: image to relocalize
:return: points [N, 2] array in img, rays [N, 2] array in keyframe
"""
bounding_box = np.ones([720, 1280])
bounding_box[13:51, 303:976] = 0
if feature_method == 'sift':
kp, des = detect_compute_sift_array(img, 1000, norm=False)
keyframe_kp, keyframe_des = detect_compute_sift_array(keyframe.img,
1000,
norm=False)
bounding_box_mask_index = keypoints_masking(kp, bounding_box)
kp = kp[bounding_box_mask_index]
des = des[bounding_box_mask_index]
bounding_box_mask_index = keypoints_masking(keyframe_kp, bounding_box)
keyframe_kp = keyframe_kp[bounding_box_mask_index]
keyframe_des = keyframe_des[bounding_box_mask_index]
# kp = add_gauss_cv_keypoints(kp, 5, 1280, 720)
# keyframe_kp = add_gauss_cv_keypoints(keyframe_kp, 5, 1280, 720)
# kp = add_outliers_cv_keypoints(kp, 1, 1280, 720, 40)
# keyframe_kp = add_outliers_cv_keypoints(keyframe_kp, 1, 1280, 720, 40)
pt1, index1, pt2, index2 = match_sift_features(kp,
des,
keyframe_kp,
keyframe_des,
pts_array=True)
elif feature_method == 'orb':
kp, des = detect_compute_orb(img, 6000)
keyframe_kp, keyframe_des = detect_compute_orb(keyframe.img, 6000)
pt1, index1, pt2, index2 = match_orb_features(kp, des, keyframe_kp,
keyframe_des)
elif feature_method == 'latch':
kp, des = detect_compute_latch(img, 5000)
keyframe_kp, keyframe_des = detect_compute_latch(keyframe.img, 5000)
pt1, index1, pt2, index2 = match_latch_features(
kp, des, keyframe_kp, keyframe_des)
else:
assert False
# vis = draw_matches(img, keyframe.img, pt1, pt2)
# cv.imshow("test", vis)
# cv.waitKey(0)
rays = np.ndarray([len(index2), 2])
for i in range(len(index2)):
rays[i, 0], rays[i, 1] = TransFunction.from_image_to_ray(
keyframe.u, keyframe.v, keyframe.f, keyframe.pan, keyframe.tilt,
pt2[i, 0], pt2[i, 1])
return pt1, rays
def get_observation_from_ptz(self, pan, tilt, f):
points = np.ndarray([len(self.rays), 2])
for j in range(len(self.rays)):
pos = np.array(self.pts[j])
points[j] = TransFunction.from_3d_to_2d(self.u, self.v, f, pan,
tilt, self.c,
self.base_rotation, pos)
return points
def get_rays_from_observation(self, pan, tilt, f, points):
rays = np.ndarray([0, points.shape[1]])
for i in range(len(points)):
angles = TransFunction.from_2d_to_pan_tilt(self.u, self.v, f, pan,
tilt, points[i][0],
points[i][1])
rays = np.row_stack(
(rays, np.concatenate([angles, points[i][2:18]], axis=0)))
return rays
def get_observation_from_rays(self, pan, tilt, f, rays):
points = np.ndarray([0, rays.shape[1]])
inner_rays = np.ndarray([0, rays.shape[1]])
index = np.ndarray([0])
for j in range(len(rays)):
tmp = TransFunction.from_pan_tilt_to_2d(self.u, self.v, f, pan, tilt, rays[j][0], rays[j][1])
if 0 < tmp[0] < self.width and 0 < tmp[1] < self.height:
inner_rays = np.row_stack((inner_rays, rays[j]))
points = np.row_stack((points, np.concatenate([np.asarray(tmp), rays[j][2:18]], axis=0)))
index = np.concatenate((index, [j]), axis=0)
return points, inner_rays, index
def ut_ptz_camera():
obj = SequenceManager("./basketball/basketball/basketball_anno.mat",
"./basketball/basketball/images",
"./basketball_ground_truth.mat",
"./objects_basketball.mat")
print(obj.camera.project_3Dpoint([0, 0, 0]))
print(
TransFunction.from_3dpoint_to_image(
obj.camera.principal_point[0], obj.camera.principal_point[1],
obj.camera.focal_length, obj.camera.pan, obj.camera.tilt,
obj.camera.camera_center, obj.camera.base_rotation, [0, 0, 0]))
print(obj.camera.project_ray([5, 1]))
print(
TransFunction.from_ray_to_image(obj.camera.principal_point[0],
obj.camera.principal_point[1],
obj.camera.focal_length,
obj.camera.pan, obj.camera.tilt, 5, 1))
print(obj.camera.back_project_to_3D_point(-1726.9998, 1295.25688))
def get_image(self, index):
pan, tilt, f = self.get_ptz(index)
points = np.ndarray([0, self.all_rays.shape[1]])
for i in range(len(self.all_rays)):
x, y = TransFunction.from_pan_tilt_to_2d(self.u, self.v, f, pan, tilt, self.all_rays[i][0],
self.all_rays[i][1])
x += random.uniform(-2, 2)
y += random.uniform(-2, 2)
if 0 < x < self.width and 0 < y < self.height:
points = np.row_stack((points, np.concatenate([np.asarray([x, y]), self.all_rays[i][2:18]], axis=0)))
return points
def extended_kalman_filter(self, previous_x, previous_p, z_k):
# 1. predict step
predict_x = previous_x + [
self.delta_pan, self.delta_tilt, self.delta_zoom
]
print("\n-----predict_x-----\n", predict_x)
q_k = 1.0 * np.diag([0.01, 0.01, 1])
predict_p = previous_p + q_k
# 2. update step
hx = np.ndarray([len(self.rays), 2])
for j in range(len(self.rays)):
hx[j] = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, predict_x[2], predict_x[0], predict_x[1],
self.rays[j][0], self.rays[j][1])
print("\n-----hx-----\n", hx)
inner_index = self.get_points_in_image(z_k, hx, 1280, 720)
y = np.ndarray([2 * len(inner_index), 1])
jacobi = np.ndarray([2 * len(inner_index), 3])
for i in range(len(inner_index)):
pts_index = inner_index[i]
y[2 * i] = z_k[pts_index][0] - hx[pts_index][0]
y[2 * i + 1] = z_k[pts_index][1] - hx[pts_index][1]
jacobi[2 * i:2 * i + 2,
0:3] = self.compute_new_jacobi(predict_x[0], predict_x[1],
predict_x[2],
self.rays[pts_index])
print("\n-----jacobi-----\n", jacobi)
print("\n-----H*P*H^t-----\n",
np.dot(np.dot(jacobi, predict_p), jacobi.T))
s = np.dot(np.dot(jacobi, predict_p),
jacobi.T) + 0.1 * np.eye(2 * len(inner_index))
k = np.dot(np.dot(predict_p, jacobi.T), np.linalg.inv(s))
print("\n-----y-----\n", y.shape)
print("\n-----k-----\n", k.shape)
print("\n-----k*y-----\n", np.dot(k, y))
updated_x = predict_x + np.dot(k, y).squeeze()
updated_p = np.dot((np.eye(3) - np.dot(k, jacobi)), predict_p)
return updated_x, updated_p
def _compute_residual(pose, rays, points, u, v):
"""
:param pose: shape [3] array of camera pose
:param rays: [N, 2] array of corresponding rays
:param points: [N, 2] array of corresponding points
:param u: camera u
:param v: camera v
:return: reprojection error of these points
"""
residual = np.ndarray([2 * len(rays)])
for i in range(len(rays)):
reproject_x, reproject_y = TransFunction.from_ray_to_image(
u, v, pose[2], pose[0], pose[1], rays[i, 0], rays[i, 1])
residual[2 * i] = reproject_x - points[i, 0]
residual[2 * i + 1] = reproject_y - points[i, 1]
return residual
def _recompute_matching_ray(keyframe, img, feature_method):
"""
:param keyframe: keyframe object to match
:param img: image to relocalize
:return: points [N, 2] array in img, rays [N, 2] array in keyframe
"""
if feature_method == 'sift':
kp, des = detect_compute_sift(img, 0)
keyframe_kp, keyframe_des = detect_compute_sift(keyframe.img, 0)
pt1, index1, pt2, index2 = match_sift_features(kp, des, keyframe_kp,
keyframe_des)
elif feature_method == 'orb':
kp, des = detect_compute_orb(img, 6000)
keyframe_kp, keyframe_des = detect_compute_orb(keyframe.img, 6000)
pt1, index1, pt2, index2 = match_orb_features(kp, des, keyframe_kp,
keyframe_des)
elif feature_method == 'latch':
kp, des = detect_compute_latch(img, 5000)
keyframe_kp, keyframe_des = detect_compute_latch(keyframe.img, 5000)
pt1, index1, pt2, index2 = match_latch_features(
kp, des, keyframe_kp, keyframe_des)
else:
assert False
# vis = draw_matches(img, keyframe.img, pt1, pt2)
# cv.imshow("test", vis)
# cv.waitKey(0)
rays = np.ndarray([len(index2), 2])
for i in range(len(index2)):
rays[i, 0], rays[i, 1] = TransFunction.from_image_to_ray(
keyframe.u, keyframe.v, keyframe.f, keyframe.pan, keyframe.tilt,
pt2[i, 0], pt2[i, 1])
return pt1, rays
def compute_new_jacobi(self, camera_pan, camera_tilt, foc, ray):
jacobi_h = np.ndarray([2, 3])
delta_angle = 0.001
delta_f = 1.0
# c_pan = radians(camera_pan)
# c_tilt = radians(camera_tilt)
# theta = radians(ray[0])
# phi = radians(ray[1])
x_delta_pan1, y_delta_pan1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan - delta_angle, camera_tilt, ray[0],
ray[1])
x_delta_pan2, y_delta_pan2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan + delta_angle, camera_tilt, ray[0],
ray[1])
x_delta_tilt1, y_delta_tilt1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt - delta_angle, ray[0],
ray[1])
x_delta_tilt2, y_delta_tilt2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc, camera_pan, camera_tilt + delta_angle, ray[0],
ray[1])
x_delta_f1, y_delta_f1 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc - delta_f, camera_pan, camera_tilt, ray[0],
ray[1])
x_delta_f2, y_delta_f2 = TransFunction.from_pan_tilt_to_2d(
self.u, self.v, foc + delta_f, camera_pan, camera_tilt, ray[0],
ray[1])
jacobi_h[0][0] = (x_delta_pan2 - x_delta_pan1) / (2 * delta_angle)
jacobi_h[0][1] = (x_delta_tilt2 - x_delta_tilt1) / (2 * delta_angle)
jacobi_h[0][2] = (x_delta_f2 - x_delta_f1) / (2 * delta_f)
jacobi_h[1][0] = (y_delta_pan2 - y_delta_pan1) / (2 * delta_angle)
jacobi_h[1][1] = (y_delta_tilt2 - y_delta_tilt1) / (2 * delta_angle)
jacobi_h[1][2] = (y_delta_f2 - y_delta_f1) / (2 * delta_f)
return jacobi_h
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)
def computer_all_ray(self, points):
all_rays = []
for i in range(0, len(points)):
ray = TransFunction.compute_rays(self.proj_center, pts[i], self.base_rotation)
all_rays.append(ray)
return all_rays