class UserInterface:
def __init__(self):
# Joystick
self.joy = JoystickInterface()
self.joy.disable_joy()
# Sequence Manager
sequences_dir = rospy.get_param("~sequences_dir")
self.sequence_manager = SequenceManager(sequences_dir)
# Sequence Action Server
self.sequence_action_server = SequenceActionServer(
self.sequence_manager)
self.sequence_action_server.start()
# Sequence Autorun
self.sequence_autorun = SequenceAutorun()
# Matlab node manager
self.matlab_manager = MatlabManager()
self.tcp_server = TcpServer()
self.tcp_server.start()
def shutdown(self):
self.sequence_manager.shutdown()
self.tcp_server.quit()
def __init__(self):
self.gauss_ros_logger = RosLogger()
# Joystick
self.joy = JoystickInterface()
self.joy.disable_joy()
# Sequence Manager
sequences_dir = rospy.get_param("~sequences_dir")
self.sequence_manager = SequenceManager(sequences_dir,
self.gauss_ros_logger)
# Sequence Action Server
self.sequence_action_server = SequenceActionServer(
self.sequence_manager, self.gauss_ros_logger)
self.sequence_action_server.start()
# Sequence Autorun
self.sequence_autorun = SequenceAutorun(self.gauss_ros_logger)
#Matlab node manager
#self.matlab_manager=MatlabManager()
self.gauss_ros_logger.publish_log_status("INFO",
"UserInterface start over")
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_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 __init__(self):
# Joystick
self.joy = JoystickInterface()
self.joy.disable_joy()
# Sequence Manager
sequences_dir = rospy.get_param("~sequences_dir")
self.sequence_manager = SequenceManager(sequences_dir)
# Sequence Action Server
self.sequence_action_server = SequenceActionServer(self.sequence_manager)
self.sequence_action_server.start()
# Sequence Autorun
self.sequence_autorun = SequenceAutorun()
def __init__(self, model_path, annotation_path, image_path):
"""
:param model_path: court model path
:param annotation_path: annotation file path
:param image_path: image folder path
"""
soccer_model = sio.loadmat(model_path)
self.line_index = soccer_model['line_segment_index']
self.points = soccer_model['points']
self.sequence = SequenceManager(annotation_path, image_path)
def reprojection_error():
import sys
sys.path.append('../slam_system')
import copy
import random
from sequence_manager import SequenceManager
sequence = SequenceManager(annotation_path="../../dataset/basketball/synthetic/ground_truth.mat",
image_path="../../dataset/synthesized/images")
camera = sequence.camera
#camera.set_ptz((pan, tilt, fl))
n = pan_gt.shape[0]
reprojection_error_ptz_mean_std = np.zeros((n, 2))
reprojection_error_h_mean_std = np.zeros((n, 2))
for i in range(n):
# randomly generate keypoints
camera_gt = copy.deepcopy(camera)
camera_h = copy.deepcopy(camera)
camera_ptz = copy.deepcopy(camera)
camera_gt.set_ptz((pan_gt[i], tilt_gt[i], fl_gt[i]))
camera_h.set_ptz((pan_h[i], tilt_h[i], fl_h[i]))
camera_ptz.set_ptz((pan_ptz[i], tilt_ptz[i], fl_ptz[i]))
im_w, im_h = 1280, 720
point_num = 100
points = np.zeros((point_num, 2))
for j in range(point_num):
points[j][0] = random.randint(0, im_w-1)
points[j][1] = random.randint(0, im_h-1)
rays = camera_gt.back_project_to_rays(points)
points_h, _ = camera_h.project_rays(rays)
points_ptz, _ = camera_ptz.project_rays(rays)
m1, std1 = mean_std_of_reprojection_error(points, points_h)
m2, std2 = mean_std_of_reprojection_error(points, points_ptz)
#print('{} {} {} {}'.format(m1, std1, m2, std2))
reprojection_error_h_mean_std[i] = m1, std1
reprojection_error_ptz_mean_std[i] = m2, std2
# save this file
sio.savemat('homography_ptz_reprojection_error.mat', {'reprojection_error_h_mean_std':reprojection_error_h_mean_std,
'reprojection_error_ptz_mean_std':reprojection_error_ptz_mean_std})
plt.plot(reprojection_error_h_mean_std[:, 0])
plt.plot(reprojection_error_ptz_mean_std[:,0])
plt.legend(['homography-based', 'PTZ (ours)'])
plt.show()
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_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 compute_relocalization_projection_error():
import sys
sys.path
sys.path.append('../slam_system')
import copy
import random
from sequence_manager import SequenceManager
sequence = SequenceManager(annotation_path="../../dataset/basketball/synthetic/ground_truth.mat",
image_path="../../dataset/synthesized/images")
base_camera = sequence.camera
data_folder = './synthetic/relocalization/'
keyframe_names = ['keyframe-{}.mat'.format(i) for i in range(10, 60, 10)]
rf_names = ['rf-{}.mat'.format(i) for i in range(10, 60, 10)]
def load_ptz(file_name):
data = sio.loadmat(file_name)
pan = data['pan'].squeeze()
tilt = data['tilt'].squeeze()
fl = data['f'].squeeze()
pan = np.reshape(pan, (-1, 1))
tilt = np.reshape(tilt, (-1, 1))
fl = np.reshape(fl, (-1, 1))
return np.hstack((pan, tilt, fl))
gt_ptz = load_ptz(data_folder + 'relocalization_gt.mat')
n = len(keyframe_names)
def compute_angular_error(gt_ptz, pred_ptz):
dif = gt_ptz - pred_ptz
dif = dif[:, 0:2]
dif = np.square(dif)
dif = np.sum(dif, axis=1)
errors = np.sqrt(dif)
return errors
threshold = 2.0
for i in range(n):
# for each outlier level
keyframe_ptz = load_ptz(data_folder + keyframe_names[i])
rf_ptz = load_ptz(data_folder + rf_names[i])
num_camera = gt_ptz.shape[0]
keyframe_angular_errors = compute_angular_error(gt_ptz, keyframe_ptz)
rf_angular_errors = compute_angular_error(gt_ptz, rf_ptz)
#print(np.where(keyframe_angular_errors < threshold)[0].shape[0])
p1 = np.where(keyframe_angular_errors < threshold)[0].shape[0] /num_camera
p2 = np.where(rf_angular_errors < threshold)[0].shape[0] /num_camera
camera_gt = copy.deepcopy(base_camera)
camera_keyframe = copy.deepcopy(base_camera)
camera_rf = copy.deepcopy(base_camera)
# sample rays from image space
im_w, im_h = 1280, 720
point_num = 50
keyframe_reprojection_error = np.zeros(num_camera)
ours_reprojection_error = np.zeros(num_camera)
for j in range(num_camera):
cur_gt_ptz = gt_ptz[j]
cur_keyframe_ptz = keyframe_ptz[j]
cur_rf_ptz = rf_ptz[j]
#print('PTZ parameter: {} {} {}'.format(cur_gt_ptz, cur_keyframe_ptz, cur_rf_ptz))
camera_gt.set_ptz((cur_gt_ptz[0], cur_gt_ptz[1], cur_gt_ptz[2]))
camera_keyframe.set_ptz((cur_keyframe_ptz[0], cur_keyframe_ptz[1], cur_keyframe_ptz[2]))
camera_rf.set_ptz((cur_rf_ptz[0], cur_rf_ptz[1], cur_rf_ptz[2]))
points = np.zeros((point_num, 2))
for k in range(point_num):
points[k][0] = random.randint(0, im_w - 1)
points[k][1] = random.randint(0, im_h - 1)
rays = camera_gt.back_project_to_rays(points)
points_keyframe, _ = camera_keyframe.project_rays(rays)
points_rf, _ = camera_rf.project_rays(rays)
m1, std1 = mean_std_of_reprojection_error(points, points_keyframe)
m2, std2 = mean_std_of_reprojection_error(points, points_rf)
keyframe_reprojection_error[j] = m1
ours_reprojection_error[j] = m2
print('outlier percentage: {}'.format((i+1)*10))
print('mean: keyframe: {}, ours: {}'.format(np.mean(keyframe_reprojection_error), np.mean(ours_reprojection_error)))
print('std: keyframe: {}, ours: {}'.format(np.std(keyframe_reprojection_error),
np.std(ours_reprojection_error)))
print('correct relocalization: keyframe: {}, ours: {}'.format(p1, p2))
def synthesized_test():
sequence = SequenceManager(
annotation_path="../../dataset/basketball/ground_truth.mat",
image_path="../../dataset/synthesized/images")
gt_pan, gt_tilt, gt_f = load_camera_pose(
"../../dataset/synthesized/synthesize_ground_truth.mat", separate=True)
line_index, points = load_model(
"../../dataset/basketball/basketball_model.mat")
begin_frame = 2400
first_frame_ptz = (gt_pan[begin_frame], gt_tilt[begin_frame],
gt_f[begin_frame])
first_camera = sequence.camera
first_camera.set_ptz(first_frame_ptz)
# print(first_camera.project_ray((0, 0)))
# print(first_camera.project_ray((10, 10)))
# print(first_camera.project_ray((10.1, 10)))
# 3*4 projection matrix for 1st frame
first_frame_mat = first_camera.projection_matrix
first_frame = sequence.get_image_gray(index=begin_frame, dataset_type=2)
# first_bounding_box = sequence.get_bounding_box_mask(0)
# img = project_with_homography(first_frame_mat, points, line_index, first_frame)
#
# cv.imshow("image", img)
# cv.waitKey()
# test_camera = copy.deepcopy(first_camera)
# test_camera.set_ptz((gt_pan[5],
# gt_tilt[5],
# gt_f[5]))
# re = compute_reprojection_error(first_frame, first_camera, test_camera)
homography_ekf = HomographyEKF()
homography_ekf.init_system(first_frame, first_frame_mat)
# tracking_obj = HomographyTracking(first_frame, first_frame_mat)
points3d_on_field = uniform_point_sample_on_field(25, 18, 25, 18)
pan = [first_frame_ptz[0]]
tilt = [first_frame_ptz[1]]
f = [first_frame_ptz[2]]
for i in range(2401, 3000, 1):
next_frame = sequence.get_image_gray(index=i, dataset_type=2)
homography_ekf.tracking(next_frame)
# img = project_with_homography(
# np.dot(homography_ekf.accumulate_homography[-1], homography_ekf.model_to_image_homography),
# points, line_index, next_frame)
# compute ptz
first_camera.set_ptz((pan[-1], tilt[-1], f[-1]))
current_homography = np.dot(homography_ekf.accumulate_homography[-1],
homography_ekf.model_to_image_homography)
pose = estimate_camera_from_homography(current_homography,
first_camera, points3d_on_field)
print("-----" + str(i) + "--------")
print(len(homography_ekf.previous_keypoints))
# print("h**o:", homography_ekf.accumulate_homography[-1])
print(pose)
# first_camera.set_ptz(pose)
# img2 = project_with_PTZCamera(first_camera, points, line_index, next_frame)
print("%f" % (pose[0] - gt_pan[i]))
print("%f" % (pose[1] - gt_tilt[i]))
print("%f" % (pose[2] - gt_f[i]))
pan.append(pose[0])
tilt.append(pose[1])
f.append(pose[2])
# cv.imshow("image", img)
# cv.imshow("image2", img2)
# cv.waitKey(0)
save_camera_pose(
np.array(pan), np.array(tilt), np.array(f),
"C:/graduate_design/experiment_result/baseline2/synthesized/new/homography-2400.mat"
)
def ut_single_image():
sequence = SequenceManager(
annotation_path="../../dataset/basketball/ground_truth.mat",
image_path="../../dataset/synthesized/images")
gt_pan, gt_tilt, gt_f = load_camera_pose(
"../../dataset/synthesized/synthesize_ground_truth.mat", separate=True)
# read image and ground truth pose
im = cv.imread('../../dataset/synthesized/images/0.jpg', 0)
pan, tilt, fl = gt_pan[0], gt_tilt[0], gt_f[0]
gt_pose = [pan, tilt, fl]
camera = sequence.camera
camera.set_ptz((pan, tilt, fl))
im_w, im_h = 1280, 720
points = detect_sift(im, 20)
N = points.shape[0]
print(points.shape)
rays = camera.back_project_to_rays(points)
print(rays.shape)
from relocalization import _compute_residual
from scipy.optimize import least_squares
from transformation import TransFunction
def robust_test(variance, camera):
"""
return mean value of reprojection error
:param variance:
:param camera:
:return:
"""
# add noise to pts
noise_pts = add_gauss(points, variance, im_w, im_h)
# add noise to camera pose
init_pose = np.zeros(3)
init_pose[0] = pan + np.random.normal(0, 5.0)
init_pose[1] = tilt + np.random.normal(0, 2.0)
init_pose[2] = fl + np.random.normal(0, 150)
# optimized the camera pose
optimized_pose = least_squares(_compute_residual,
init_pose,
verbose=0,
x_scale='jac',
ftol=1e-4,
method='trf',
args=(rays, noise_pts, im_w / 2,
im_h / 2))
optimzied_ptz = optimized_pose.x
#print('ground truth: {}'.format(gt_pose))
#print('estiamted pose: {}'.format(optimzied_ptz))
# compute reprojection error
estimated_camera = copy.deepcopy(camera)
estimated_camera.set_ptz(optimzied_ptz)
pts1, _ = camera.project_rays(rays)
pts2, _ = estimated_camera.project_rays(rays)
reprojection_error = pts1 - pts2
for i in range(N):
dx, dy = pts1[i] - pts2[i]
reprojection_error[i] = math.sqrt(dx * dx + dy * dy)
# print(reprojection_error[0:10])
m, std = np.mean(reprojection_error), np.std(reprojection_error)
#print('mean std: {} {}'.format(m, std))
return m
variances = [0.5, 1.0, 1.5, 2.0, 2.5, 3.0]
repeat_num = 20
for v in variances:
error_mean = np.zeros(repeat_num)
# repeat
for i in range(repeat_num):
m = robust_test(v, camera)
error_mean[i] = m
m, std = np.mean(error_mean), np.std(error_mean)
print('noise, mean, std: {} {} {}'.format(v, m, std))
def soccer3():
sequence = SequenceManager(
"../../dataset/soccer_dataset/seq3/seq3_ground_truth.mat",
"../../dataset/soccer_dataset/seq3/seq3_330",
"../../dataset/soccer_dataset/seq3/seq3_ground_truth.mat",
"../../dataset/soccer_dataset/seq3/seq3_player_bounding_box.mat")
line_index, points = load_model(
"../../dataset/soccer_dataset/highlights_soccer_model.mat")
first_frame_ptz = (sequence.ground_truth_pan[0],
sequence.ground_truth_tilt[0],
sequence.ground_truth_f[0])
first_camera = sequence.camera
first_camera.set_ptz(first_frame_ptz)
# 3*4 projection matrix for 1st frame
first_frame_mat = first_camera.projection_matrix
first_frame = sequence.get_image_gray(index=0, dataset_type=1)
img = project_with_homography(first_frame_mat, points, line_index,
first_frame)
# cv.imshow("image", img)
# cv.waitKey()
tracking_obj = HomographyTracking(first_frame, first_frame_mat)
points3d_on_field = uniform_point_sample_on_field(118, 70, 50, 25)
pan = [first_frame_ptz[0]]
tilt = [first_frame_ptz[1]]
f = [first_frame_ptz[2]]
for i in range(1, sequence.length):
next_frame = sequence.get_image_gray(index=i, dataset_type=1)
tracking_obj.tracking(next_frame)
# img = project_with_homography(tracking_obj.accumulate_matrix[-1], points, line_index, next_frame)
# compute ptz
first_camera.set_ptz((pan[-1], tilt[-1], f[-1]))
pose = estimate_camera_from_homography(
tracking_obj.accumulate_matrix[-1], first_camera,
points3d_on_field)
print("-----" + str(i) + "--------")
print(tracking_obj.each_homography[-1])
print(pose)
# first_camera.set_ptz(pose)
# img2 = project_with_PTZCamera(first_camera, points, line_index, next_frame)
print("%f" % (pose[0] - sequence.ground_truth_pan[i]))
print("%f" % (pose[1] - sequence.ground_truth_tilt[i]))
print("%f" % (pose[2] - sequence.ground_truth_f[i]))
pan.append(pose[0])
tilt.append(pose[1])
f.append(pose[2])
# cv.imshow("image", img)
# cv.imshow("image2", img2)
# cv.waitKey(0)
save_camera_pose(
np.array(pan), np.array(tilt), np.array(f),
"C:/graduate_design/experiment_result/baseline2/2-gauss.mat")
def synthesized_test():
sequence = SequenceManager(
annotation_path="../../dataset/basketball/ground_truth.mat",
image_path="../../dataset/synthesized/images")
gt_pan, gt_tilt, gt_f = load_camera_pose(
"../../dataset/synthesized/synthesize_ground_truth.mat", separate=True)
line_index, points = load_model(
"../../dataset/basketball/basketball_model.mat")
begin_frame = 2400
first_frame_ptz = (gt_pan[begin_frame], gt_tilt[begin_frame],
gt_f[begin_frame])
first_camera = sequence.camera
first_camera.set_ptz(first_frame_ptz)
# 3*4 projection matrix for 1st frame
first_frame_mat = first_camera.projection_matrix
first_frame = sequence.get_image_gray(index=begin_frame, dataset_type=2)
# img = project_with_homography(first_frame_mat, points, line_index, first_frame)
# cv.imshow("image", img)
# cv.waitKey()
tracking_obj = HomographyTracking(first_frame, first_frame_mat)
points3d_on_field = uniform_point_sample_on_field(25, 18, 25, 18)
pan = [first_frame_ptz[0]]
tilt = [first_frame_ptz[1]]
f = [first_frame_ptz[2]]
for i in range(2400, 3000):
next_frame = sequence.get_image_gray(index=i, dataset_type=2)
tracking_obj.tracking(next_frame)
# img = project_with_homography(tracking_obj.accumulate_matrix[-1], points, line_index, next_frame)
# compute ptz
first_camera.set_ptz((pan[-1], tilt[-1], f[-1]))
pose = estimate_camera_from_homography(
tracking_obj.accumulate_matrix[-1], first_camera,
points3d_on_field)
print("-----" + str(i) + "--------")
# print(tracking_obj.each_homography[-1])
print(pose)
# first_camera.set_ptz(pose)
# img2 = project_with_PTZCamera(first_camera, points, line_index, next_frame)
print("%f" % (pose[0] - gt_pan[i]))
print("%f" % (pose[1] - gt_tilt[i]))
print("%f" % (pose[2] - gt_f[i]))
pan.append(pose[0])
tilt.append(pose[1])
f.append(pose[2])
# cv.imshow("image", img)
# cv.imshow("image2", img2)
# cv.waitKey(0)
save_camera_pose(
np.array(pan), np.array(tilt), np.array(f),
"C:/graduate_design/experiment_result/baseline2/2-gauss.mat")
from sequence_manager import SequenceManager
from ptz_slam import PtzSlam
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)
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 basketball_test():
sequence = SequenceManager("../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/images",
"../../dataset/basketball/ground_truth.mat",
"../../dataset/basketball/bounding_box.mat")
# line_index, points = load_model("../../dataset/soccer_dataset/highlights_soccer_model.mat")
begin_frame = 0
first_frame_ptz = (sequence.ground_truth_pan[begin_frame],
sequence.ground_truth_tilt[begin_frame],
sequence.ground_truth_f[begin_frame])
first_camera = sequence.camera
first_camera.set_ptz(first_frame_ptz)
# 3*4 projection matrix for 1st frame
first_frame_mat = first_camera.projection_matrix
first_frame = sequence.get_image_gray(index=begin_frame, dataset_type=0)
first_bounding_box = sequence.get_bounding_box_mask(begin_frame)
# img = project_with_homography(first_frame_mat, points, line_index, first_frame)
#
# cv.imshow("image", img)
# cv.waitKey()
homography_ekf = HomographyEKF()
homography_ekf.init_system(first_frame, first_frame_mat,
first_bounding_box)
# tracking_obj = HomographyTracking(first_frame, first_frame_mat)
points3d_on_field = uniform_point_sample_on_field(25, 18, 25, 18)
pan = [first_frame_ptz[0]]
tilt = [first_frame_ptz[1]]
f = [first_frame_ptz[2]]
for i in range(1, sequence.length):
next_frame = sequence.get_image_gray(index=i, dataset_type=0)
next_bounding_box = sequence.get_bounding_box_mask(i)
homography_ekf.tracking(next_frame, next_bounding_box)
# img = project_with_homography(
# np.dot(homography_ekf.accumulate_homography[-1], homography_ekf.model_to_image_homography),
# points, line_index, next_frame)
# compute ptz
first_camera.set_ptz((pan[-1], tilt[-1], f[-1]))
current_homography = np.dot(homography_ekf.accumulate_homography[-1],
homography_ekf.model_to_image_homography)
pose = estimate_camera_from_homography(current_homography,
first_camera, points3d_on_field)
print("-----" + str(i) + "--------")
# print("hompgraphy:", homography_ekf.accumulate_homography)
print(pose)
# first_camera.set_ptz(pose)
# img2 = project_with_PTZCamera(first_camera, points, line_index, next_frame)
print("%f" % (pose[0] - sequence.ground_truth_pan[i]))
print("%f" % (pose[1] - sequence.ground_truth_tilt[i]))
print("%f" % (pose[2] - sequence.ground_truth_f[i]))
pan.append(pose[0])
tilt.append(pose[1])
f.append(pose[2])
# cv.imshow("image", img)
# cv.imshow("image2", img2)
# cv.waitKey(0)
save_camera_pose(np.array(pan), np.array(tilt), np.array(f),
"./bs4_result.mat")