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)
def plot_smooth_dof(path, filtered=0):
path_base = path.replace("/calib", "")
savepath = join(path_base, 'result')
if not exists(savepath):
os.mkdir(savepath)
if filtered: suffix = 'smooth'
else: suffix = ''
files_cal = os.listdir(path)
files_cal.sort()
numfiles = len(files_cal)
Rx, Ry, Rz, fx = [], [], [], []
T = np.empty([0, 3])
for i in range(numfiles):
# load estimations from 'calib' folder
calib_val = np.load(join(path, files_cal[i])).item()
A, R, t = calib_val['A'], calib_val['R'], calib_val['T']
rx, ry, rz = transform_util.get_angle_from_rotation(R)
fx = np.append(fx, A[1, 1])
Rx = np.append(Rx, rx)
Ry = np.append(Ry, ry)
Rz = np.append(Rz, rz)
T = np.append(T, t.T, axis=0)
# plot rotations
fig, ax = plt.subplots()
ind = np.arange(0, numfiles, 1)
ax1 = plt.subplot(1, 3, 1)
ax1.plot(ind, Rx)
ax1.set_xlabel('Frame')
ax1.set_title('Rotation x')
ax2 = plt.subplot(1, 3, 2)
ax2.plot(ind, Ry)
ax2.set_xlabel('Frame')
ax2.set_title('Rotation y')
ax3 = plt.subplot(1, 3, 3)
ax3.plot(ind, Rz)
ax3.set_xlabel('Frame')
ax3.set_title('Rotation z')
plt.savefig(savepath + '/R' + suffix + '.png')
# plot translations
fig, ax = plt.subplots()
ax4 = plt.subplot(1, 3, 1)
ax4.plot(ind, T[:, 0])
ax4.set_xlabel('Frame')
ax4.set_title('Translation x')
ax5 = plt.subplot(1, 3, 2)
ax5.plot(ind, T[:, 1])
ax5.set_xlabel('Frame')
ax5.set_title('Translation y')
ax6 = plt.subplot(1, 3, 3)
ax6.plot(ind, T[:, 2])
ax6.set_xlabel('Frame')
ax6.set_title('Translation z')
plt.savefig(savepath + '/T' + suffix + '.png')
# plot focal length
if not filtered:
fig, ax = plt.subplots()
ax7 = plt.subplot(1, 1, 1)
ax7.plot(ind, fx)
ax7.set_xlabel('Frame')
ax7.set_title('Focal Length')
plt.savefig(savepath + '/F' + suffix + '.png')