def _fun_distance_transform(params_, dist_map_, points3d):
theta_x_, theta_y_, theta_z_, fx_, tx_, ty_, tz_ = params_
h_, w_ = dist_map_.shape[0:2]
n_ = points3d.shape[0]
cx_, cy_ = float(dist_map_.shape[1]) / 2.0, float(dist_map_.shape[0]) / 2.0
R_ = transf_utils.Rz(theta_z_).dot(transf_utils.Ry(theta_y_)).dot(
transf_utils.Rx(theta_x_))
A_ = np.eye(3, 3)
A_[0, 0], A_[1, 1], A_[0, 2], A_[1, 2] = fx_, fx_, cx_, cy_
T_ = np.array([[tx_], [ty_], [tz_]])
# 2D projection
p2_ = A_.dot(R_.dot(points3d.T) + np.tile(T_, (1, n_)))
p2_ /= p2_[2, :]
p2_ = p2_.T[:, 0:2]
p2_ = np.round(p2_).astype(int)
# Return points2d and index that stay inside a frame.
_, valid_id_ = cam_utils.inside_frame(p2_, h_, w_)
residual = np.zeros((n_, )) + 0.0
residual[valid_id_] = dist_map_[p2_[valid_id_, 1], p2_[valid_id_, 0]]
# print("Sum Residual : ", np.sum(residual))
return np.sum(residual)
def _calibrate_camera_dist_transf(A, R, T, dist_transf, points3d):
theta_x, theta_y, theta_z = transf_utils.get_angle_from_rotation(R)
fx, fy, cx, cy = A[0, 0], A[1, 1], A[0, 2], A[1, 2]
params = np.hstack((theta_x, theta_y, theta_z, fx, T[0, 0], T[1, 0], T[2,
0]))
res_ = minimize(_fun_distance_transform,
params,
args=(dist_transf, points3d),
method='Powell',
options={
'disp': False,
'maxiter': 5000
})
result = res_.x
theta_x_, theta_y_, theta_z_, fx_, tx_, ty_, tz_ = result
cx_, cy_ = float(dist_transf.shape[1]) / 2.0, float(
dist_transf.shape[0]) / 2.0
R__ = transf_utils.Rz(theta_z_).dot(transf_utils.Ry(theta_y_)).dot(
transf_utils.Rx(theta_x_))
T__ = np.array([[tx_], [ty_], [tz_]])
A__ = np.eye(3, 3)
A__[0, 0], A__[1, 1], A__[0, 2], A__[1, 2] = fx_, fx_, cx_, cy_
return A__, R__, T__
def filter_parameters(path, filter_type='mean', kernel_size=5):
if filter_type == 'mean':
mean_filter = 1
elif filter_type == 'median':
mean_filter = 0
else:
print('Using mean filter.')
mean_filter = 1
# plot unfiltered values
plot_smooth_dof(path, filtered=0)
# load estimations from 'calib' folder
files = os.listdir(path)
files.sort()
numfiles = len(files)
A_tmp = []
R_tmp = []
T_tmp = []
for i in range(numfiles):
ld = np.load(join(path, files[i])).item()
A_, R_, T_ = ld['A'], ld['R'], ld['T']
A_tmp.append(A_)
R_tmp.append(R_)
T_tmp.append(T_)
t0 = np.array(T_tmp)
A_tmp = np.array(A_tmp)
n = len(t0)
smooth_t = t0
# convert rotation matrix to rotation angles
Rx0, Ry0, Rz0 = [], [], []
for j in range(numfiles):
Rangle = np.array(transform_util.get_angle_from_rotation(R_tmp[j]))
Rx0 = np.append(Rx0, Rangle[0])
Ry0 = np.append(Ry0, Rangle[1])
Rz0 = np.append(Rz0, Rangle[2])
smooth_rx = Rx0
smooth_ry = Ry0
smooth_rz = Rz0
kernel_size_i = int(kernel_size / 2) # only for even kernel size
for k in range(1, n - 1):
if k < kernel_size_i:
div = 2 * k + 1
kernel_size = k
elif k > n - 1 - kernel_size_i:
div = 2 * (n - 1 - k) + 1
kernel_size = n - k - 1
else:
div = 2 * kernel_size_i + 1
kernel_size = kernel_size_i
# mean filtering
if mean_filter:
x_m = np.sum(t0[k - kernel_size:k + kernel_size + 1, 0]) / div
y_m = np.sum(t0[k - kernel_size:k + kernel_size + 1, 1]) / div
z_m = np.sum(t0[k - kernel_size:k + kernel_size + 1, 2]) / div
rx_m = np.sum(Rx0[k - kernel_size:k + kernel_size + 1]) / div
ry_m = np.sum(Ry0[k - kernel_size:k + kernel_size + 1]) / div
rz_m = np.sum(Rz0[k - kernel_size:k + kernel_size + 1]) / div
# median filtering
else:
x_m = np.median(t0[k - kernel_size:k + kernel_size + 1, 0])
y_m = np.median(t0[k - kernel_size:k + kernel_size + 1, 1])
z_m = np.median(t0[k - kernel_size:k + kernel_size + 1, 2])
rx_m = np.median(Rx0[k - kernel_size:k + kernel_size + 1])
ry_m = np.median(Ry0[k - kernel_size:k + kernel_size + 1])
rz_m = np.median(Rz0[k - kernel_size:k + kernel_size + 1])
smooth_t[k, 0] = x_m
smooth_t[k, 1] = y_m
smooth_t[k, 2] = z_m
smooth_rx[k] = rx_m
smooth_ry[k] = ry_m
smooth_rz[k] = rz_m
# save filtered values to calib files
for l in range(n):
smooth_R = transform_util.Rz(smooth_rz[l]).dot(
transform_util.Ry(smooth_ry[l])).dot(
transform_util.Rx(smooth_rx[l]))
np.save(join(path, files[l]), {
'A': A_tmp[l, :],
'R': smooth_R,
'T': smooth_t[l, :]
})
# plot filtered parameters
plot_smooth_dof(path, filtered=1)
