def cal_cameras_graph(self):
camera_groups = tuple(itertools.combinations(self.camera_list, 2))
for group in camera_groups:
c1_idx = camera_list.index(group[0])
c2_idx = camera_list.index(group[1])
common_pts_m = np.row_stack(
(self.mask[c1_idx, :], self.mask[c2_idx, :]))
common_pts_num = len(
np.where((common_pts_m[0, :] == 1)
& (common_pts_m[1, :] == 1))[0])
# print("camera%d camera%d common pts %d"%(c1_idx, c2_idx, common_pts_num))
weight = 1 / common_pts_num
self.cameras_graph.add_edge(group[0], group[1], weight)
def reconstruct_ref_pts(self, bridge_camera):
self.ref_re_cameras = np.zeros((3, self.sample_num))
params_db = camerasParamsDatabase()
for camera in self.camera_list:
c_idx = camera_list.index(camera) #camera index
if bridge_camera == camera:
key = bridge_camera + "_params_world_csys"
else:
key = bridge_camera + "_" + camera + "_params_world_csys"
proj = params_db.get_data(key)
if proj:
proj = np.array(proj).reshape((3, 4))
self.proj_cameras_M[c_idx, :, :] = proj
self.proj_cameras_list = \
self.proj_cameras_M.reshape((1,12*self.cameras_num)).tolist()[0]
for p_idx in np.arange(self.sample_num):
X = []
P = []
c_list = []
for c_idx in np.arange(self.cameras_num):
if self.mask[c_idx, p_idx] == 1:
X.append(self.img_filter_cameras[c_idx, :, p_idx])
P.append(self.proj_cameras_M[c_idx, :, :])
c_list.append(c_idx)
if len(X) > 1:
ref_re_pts = triangulate_multi_cameras(X, P)
self.ref_re_cameras[:, p_idx] = ref_re_pts
#ref_pts = self.ref_filter_cameras[c_idx,:,p_idx]
#print("Camera", c_list)
#print(np.abs(ref_re_pts - ref_pts))
self.ref_re_cameras_list = \
self.ref_re_cameras.reshape((1,3*self.sample_num)).tolist()[0]
def get_op_params(camera1,
camera2,
camera_list,
multi_c_p,
params_db,
is_get_f_from_db=False):
"""
get optimize params,
R_T: camera2 coordinate system to camera1 coordinate system
"""
c1_idx, c2_idx = camera_list.index(camera1), camera_list.index(camera2)
common_pts_m = np.row_stack(
(multi_c_p.mask[c1_idx, :], multi_c_p.mask[c2_idx, :]))
common_pts = np.where((common_pts_m[0, :] == 1)
& (common_pts_m[1, :] == 1))[0]
img_data1 = multi_c_p.img_filter_cameras[c1_idx, :, common_pts].T
img_data2 = multi_c_p.img_filter_cameras[c2_idx, :, common_pts].T
if is_get_f_from_db:
f0 = params_db.get_data(camera1 + "_int")
f1 = params_db.get_data(camera2 + "_int")
if f0 and f1:
op_x = get_params_from_stereo_vision(img_data1, img_data2, \
(f0, f1))
else:
op_x = get_params_from_stereo_vision(img_data1, img_data2)
params_db.store_data(camera2 + "_int", op_x[1])
# print("store int params:", camera2, op_x[1])
else:
op_x = get_params_from_stereo_vision(img_data1, img_data2)
params_db.store_data(camera2 + "_int", op_x[1])
# print("store int params:", camera2, op_x[1])
f0 = op_x[0]
f1 = op_x[1]
R_M = Rodrigues(np.array(op_x[2:5]))[0]
t = np.array(np.array(op_x[5:]))
R_T = np.column_stack((R_M, t))
return op_x, f0, f1, R_T
def update_proj_matrix(bridge_camera, transfer_m, camera_list):
params_db = camerasParamsDatabase()
camera1_idx = camera_list.index(bridge_camera)
other_cameras = camera_list[:camera1_idx] + camera_list[camera1_idx+1:]
for camera2_flag in other_cameras:
rt_key = bridge_camera+"_"+camera2_flag
ext_camera_csys = get_ext_params_from_db(bridge_camera, camera2_flag, params_db)
f = params_db.get_data(camera2_flag+"_int")
if ext_camera_csys is not None and f:
K = np.array([[f, 0, 0], [0, f, 0], [0, 0, 1]])
proj_M2 = K.dot(ext_camera_csys).dot(transfer_m)
params_db.store_data(rt_key +"_params_world_csys", proj_M2.tolist())
def load(self, start_idx):
for camera in self.camera_list:
c_idx = camera_list.index(camera) #camera index
camera_db = CameraCalDatabase(camera)
keys = list(camera_db.db.getall())
if len(keys) >= self.sample_num / 3:
p_idx = 0 #point idx
for i in np.arange(int(self.sample_num / 3)):
key = keys[i]
if int(key) > start_idx:
img_coord_camera = camera_db.get_cal_data(key)['2d']
ref_coord_camera = camera_db.get_cal_data(key)['3d']
#make sure the data is not empty dict
# if the image contains required three points
if (len(set(img_coord_camera.keys()))==4) and \
set(self.points_flag).issubset(set(img_coord_camera.keys())):
for point_flag in img_coord_camera.keys():
c1 = coordinate_sys_move(img_coord_camera[point_flag], \
accuracy = self.img_accuracy)
if point_flag in self.points_flag:
#red blue green
self.mask[c_idx, p_idx] = 1
self.img_filter_cameras[c_idx, :,
p_idx] = c1
self.ref_filter_cameras[
c_idx, :,
p_idx] = ref_coord_camera[point_flag]
p_idx += 1
else:
p_idx += 3
for p_idx in np.arange(self.sample_num):
if (np.count_nonzero(self.mask[:, p_idx]) < 2):
self.mask[:, p_idx] = np.zeros((1, self.cameras_num))
cols_allzero = np.where(~self.mask.any(axis=0))[0]
#remove outliers, the pts not in any images
self.mask = np.delete(self.mask, cols_allzero, axis=1)
self.img_filter_cameras = np.delete(self.img_filter_cameras, \
cols_allzero, axis=2)
self.ref_filter_cameras = np.delete(self.ref_filter_cameras, \
cols_allzero, axis=2)
self.sample_num = self.mask.shape[1]
def get_scale_cameras(bridge_camera, camera_list, cameras_graph):
"""
get the scales of other camera's coordinates during compute the
transfer matrix
"""
cameras_graph.get_border_cost(bridge_camera)
if bridge_camera in camera_list:
camera1_idx = camera_list.index(bridge_camera)
other_cameras = camera_list[:camera1_idx] + camera_list[camera1_idx+1:]
else:
other_cameras = camera_list
cost_list = []
cost_sum = 0
for camera2_flag in other_cameras:
cost = 1/cameras_graph.dijkstra(camera2_flag, bridge_camera)[0]
cost_list.append(cost)
cost_sum += cost
scales = [x/cost_sum for x in cost_list]
return scales
def verify_BA(bridge_camera, camera_list, img_filter_cameras, ref_filter_cameras,\
ref_re_cameras, mask, pts_num):
"""
Verify the calibration effect after bundle adjustment
"""
error_ref_pts = np.zeros((3, pts_num))
dist_error_pts = np.zeros((3, int(pts_num / 3)))
cameras_num = len(camera_list)
proj_cameras_M = np.zeros((cameras_num, 3, 4))
params_BA = camerasParamsDatabase(bridge_camera + "_Params_BA.db")
for camera in camera_list:
c_idx = camera_list.index(camera) #camera index
proj = params_BA.get_data(camera)
if proj:
proj = np.array(proj).reshape((3, 4))
proj_cameras_M[c_idx, :, :] = proj
for p_idx in np.arange(pts_num):
X = []
P = []
for c_idx in np.arange(cameras_num):
if mask[c_idx, p_idx] == 1:
last_c_idx = c_idx
X.append(img_filter_cameras[c_idx, :, p_idx])
P.append(proj_cameras_M[c_idx, :, :])
if len(X) > 1:
ref_re_pts_BA = triangulate_multi_cameras(X, P)
ref_pts = ref_filter_cameras[last_c_idx, :, p_idx]
error_ref_pts[:, p_idx] = np.abs(ref_re_pts_BA - ref_pts)
if p_idx % 3 == 0:
counter_3 = 0
red_res = ref_re_pts_BA
elif p_idx % 3 == 1 and counter_3 == 0:
counter_3 += 1
blue_res = ref_re_pts_BA
elif p_idx % 3 == 2 and counter_3 == 1:
counter_3 += 1
green_res = ref_re_pts_BA
if counter_3 == 2:
len_RtoG = abs(
np.linalg.norm(red_res - green_res) -
len_relation["Red_Green"])
len_RtoB = abs(
np.linalg.norm(red_res - blue_res) - len_relation["Red_Blue"])
len_GtoB = abs(
np.linalg.norm(green_res - blue_res) -
len_relation["Blue_Green"])
dist_error_pts[:, int(p_idx / 3)] = np.array(
[len_RtoG, len_RtoB, len_GtoB])
counter_3 = 0
error_plot(dist_error_pts,
titles=["GreenToRed", "RedToBlue", "GreenToBlue"])
error_plot(error_ref_pts)
print("<Mean> Fig1:%f Fig2:%f Fig3:%f"% \
(np.mean(error_ref_pts[0,:]),np.mean(error_ref_pts[1,:]),np.mean(error_ref_pts[2,:])))
print("<Std> Fig1:%f Fig2:%f Fig3:%f"% \
(np.std(error_ref_pts[0,:]),np.std(error_ref_pts[1,:]),np.std(error_ref_pts[2,:])))
result = [np.mean(error_ref_pts[0,:]),\
np.mean(error_ref_pts[1,:]),\
np.mean(error_ref_pts[2,:]),\
np.std(error_ref_pts[0,:]),\
np.std(error_ref_pts[1,:]),\
np.std(error_ref_pts[2,:])]
return result
def get_params_from_muti_cameras(data_processor, bridge_camera, camera_list):
"""
multiple cameras calibration.
1. get the focal length based on the
"""
t0 = time.time()
params_db = camerasParamsDatabase()
if bridge_camera in camera_list:
bridge_camera_idx = camera_list.index(bridge_camera)
other_cameras = camera_list[:bridge_camera_idx] + camera_list[
bridge_camera_idx + 1:]
else:
other_cameras = camera_list
p = data_processor
# for camera in camera_list:
for camera in [bridge_camera]:
best_group = list(p.get_best_group(camera))
if not params_db.get_data(camera + "_int"):
if bridge_camera in best_group:
b_idx = best_group.index(bridge_camera)
best_group = best_group[b_idx], best_group[1 - b_idx]
print("best_group", best_group)
camera1, camera2 = best_group
key = camera1 + "_" + camera2
print("Int param key:", key)
if not params_db.get_data(key):
op_x, f0, f1, R_T = get_op_params(camera1, camera2,
camera_list, p, params_db)
if camera == camera1:
params_db.store_data(camera + "_int", f0)
else:
params_db.store_data(camera + "_int", f1)
params_db.store_data(key, op_x)
for camera in other_cameras:
Best_RT = np.eye(4, 4)
key = bridge_camera + "_" + camera
key_inv = camera + "_" + bridge_camera
path = p.cameras_graph.dijkstra(camera, bridge_camera)[1]
RT_list = []
groups = gen_group(path, 2)
print("Ext groups:", groups)
if (get_ext_params_from_db(bridge_camera, camera, params_db) is None):
for group in groups:
group_key = group[1] + "_" + group[0]
if not params_db.get_data(group_key):
op_x, f0, f1, R_T = get_op_params(group[1], group[0],\
camera_list, p, params_db, True)
params_db.store_data(group_key, op_x)
else:
op_x = params_db.get_data(group_key)
R_M = Rodrigues(np.array(op_x[2:5]))[0]
t = np.array(np.array(op_x[5:]))
R_T = np.column_stack((R_M, t))
RT_list.append(np.row_stack((R_T, np.array([0, 0, 0, 1]))))
for R_T in RT_list:
Best_RT = Best_RT.dot(R_T)
Best_RT_inv = inv(Best_RT)[0:3, :]
Best_RT = Best_RT[0:3, :]
params_db.store_data(key + "_ext_camera_csys", Best_RT.tolist())
params_db.store_data(key_inv + "_ext_camera_csys",
Best_RT_inv.tolist())
t1 = time.time()
print("Mutiple calibrate take {0:.0f} seconds".format(t1 - t0))
def get_other_cameras_coord(bridge_camera, camera2_flag=None, \
camera_list = None, img_accuracy=None):
"""
get cameras origin coordinates in bridge_camera coordinate system
"""
params_db = camerasParamsDatabase()
if camera2_flag:
other_cameras = [camera2_flag]
elif camera_list:
if bridge_camera in camera_list:
camera1_idx = camera_list.index(bridge_camera)
other_cameras = camera_list[:camera1_idx] + camera_list[camera1_idx+1:]
else:
other_cameras = camera_list
proj_list = []
flags = ("Green", "Blue", "Yellow")
img_pts_data = {"Green":[], "Blue":[], "Yellow":[]}
res_pts_3d= np.array([])
ref_pts_3d = []
camera_pts_list = []
for camera2_flag in other_cameras:
camera2_idx = other_cameras.index(camera2_flag)
if camera2_idx ==0:
f0 = params_db.get_data(bridge_camera+"_int")
K1 = np.array([[f0, 0, 0], [0, f0, 0], [0, 0, 1]])
proj_M1 = K1.dot(np.column_stack((np.eye(3, 3), np.zeros((3, 1)))))
proj_list.append(proj_M1)
f1 = params_db.get_data(camera2_flag+"_int")
K2 = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1]])
R_T = get_ext_params_from_db(bridge_camera, camera2_flag, params_db)
R_M = R_T[:,0:3]
t = R_T[:,3]
c2_origin_c1_csys = np.linalg.solve(R_M, -t)
if not np.allclose(np.dot(R_M, c2_origin_c1_csys), -t):
raise ValueError("Can not get the Camera coordinate in CSYS.")
camera_pts_list.append(c2_origin_c1_csys)
proj_M2 = K2.dot(np.column_stack((R_M, t)))
proj_list.append(proj_M2)
img_coord_pts, ref_coord_pts = get_level_calib_data([bridge_camera, camera2_flag], \
img_accuracy)
camera1_ref_2d, camera2_ref_2d = img_coord_pts
for flag in flags:
if camera2_idx ==0:
ref_pts_3d.append(ref_coord_pts[flag])
img_pts_data[flag].append(np.array(camera1_ref_2d[flag]))
img_pts_data[flag].append(np.array(camera2_ref_2d[flag]))
for flag in flags:
res_pt_3d = triangulate_multi_cameras(img_pts_data[flag], proj_list)
res_pts_3d = np.append(res_pts_3d, res_pt_3d)
new_res_pts_3d = res_pts_3d = res_pts_3d.reshape((3, 3)).T
new_ref_pts_3d = ref_pts_3d = np.array(ref_pts_3d).T
for camera2_flag in other_cameras:
c2_origin_c1_csys = camera_pts_list[other_cameras.index(camera2_flag)]
sol, pts_dist = compute_coord_from_triangule_locate(res_pts_3d, \
c2_origin_c1_csys, \
ref_pts_3d)
# print("Sol:", sol)
# sol = [round(x) for x in sol]
# print("Round Sol:", sol)
# error = compute_coord_from_triangule_locate_eqs(sol, ref_pts_3d, pts_dist)
# print(error)
# sol = np.array([-4.0, 8.0, -4.0])
new_res_pts_3d = np.column_stack((new_res_pts_3d, c2_origin_c1_csys))
new_ref_pts_3d = np.column_stack((new_ref_pts_3d, sol))
return new_res_pts_3d, new_ref_pts_3d