def predict_feature_measurement(x, camera, features):
"""
:param x:
:param camera:
:param features:
:return:
"""
for i, f in enumerate(features):
mat_r = mathematics.q2r(x[3:7])
if f['type'] == 2:
mi = mathematics.m(phi=x[f['begin'] + 4], theta=x[f['begin'] + 3])
hrl = np.dot(mat_r.T, (x[f['begin']:f['begin'] + 3] -
x[0:3])) * x[f['begin'] + 5] + mi
else:
hrl = np.dot(mat_r.T, (x[f['begin']:f['begin'] + 3] - x[0:3]))
tempx = math.atan2(hrl[0], hrl[2]) * 180 / math.pi
tempy = math.atan2(hrl[1], hrl[2]) * 180 / math.pi
if (tempx > 60) or (tempx < -60) or (tempy > 60) or (tempy < -60):
continue
uvx = camera['u0'] + (hrl[0] / hrl[2]) * camera['fku']
uvy = camera['v0'] + (hrl[1] / hrl[2]) * camera['fkv']
uv = np.array([uvx, uvy])
uvd = mathematics.distort_fm(uv, camera)
if np.all(uvd > 0) and np.all(uvd < camera['size']):
f['h'] = uvd
return features
def compute_hypothesis_support_fast(x, inparams, pattern,
features, thresh):
"""
:param x:
:param inparams:
:param pattern:
:param features:
:param thresh:
:return:
"""
hyp_support = 0
pos_id = np.zeros(pattern.shape[0], dtype=np.float32)
pos_xyz = np.zeros(pattern.shape[0], dtype=np.float32)
for i, f in enumerate(features):
if f['z'] is not None:
if f['type'] == 1:
mat_r = mathematics.q2r(x[3:7])
ri_minus_rwc = x[f['begin']:f['begin'] + 3] - x[0:3]
hc = np.dot(mat_r.T, ri_minus_rwc).astype(dtype=np.float32)
h_norm = np.zeros(2, dtype=np.float32)
else:
mat_r = mathematics.q2r(x[3:7])
ri_minus_rwc = x[f['begin']:f['begin'] + 3] - x[0:3]
ri_minus_rwc_by_rhoi = ri_minus_rwc*x[f['begin']+5]
mi = mathematics.m(f['begin']+3, f['begin']+4)
hc = np.dot(mat_r.T, (ri_minus_rwc_by_rhoi + mi))
h_norm = np.array([hc[0]/hc[2], hc[1]/hc[2]])
h_image = np.zeros(2, dtype=np.float32)
h_image[0] = h_norm[0]+inparams['fku'] + inparams['u0']
h_image[1] = h_norm[1]+inparams['fkv'] + inparams['v0']
h_image = mathematics.distort_fm(h_image, inparams)
nu = f['z'] - h_image
residual = np.linalg.norm(nu)
if f['type'] == 1:
pos_xyz[f['begin']] = np.float32(residual > thresh)
hyp_support += pos_xyz[f['begin']]
else:
pos_id[f['begin']] = np.float32(residual > thresh)
hyp_support += pos_id[f['begin']]
return hyp_support, pos_id, pos_xyz
def predict_features_appearance(self, inparams, x):
"""
:param inparams:
:param x:
:return:
"""
r = x[0:3]
mat_r = mathematics.q2r(x[3:7])
for i in range(len(self.features)):
if self.features[i]['h'] is not None:
begin = self.features[i]['begin']
if self.features[i]['type'] == 1:
xyz_w = x[begin:begin + 3]
else:
xyz_w = mathematics.id2cartesian(x[begin:begin + 6])
self.features[i]['patch_wm'] = self.pred_patch_fc(self.features[i], r, mat_r, xyz_w, inparams)
def compute_new_feature(x, cov, xy, camera):
"""
:param x:
:param cov:
:param xy:
:param camera:
:return:
"""
rho = 0.1
std_rho = 0.5
# State vector
xf = mathematics.hinv(xy, x, camera, rho)
x = np.concatenate([x, xf])
# Covariance
mat_r = mathematics.q2r(x[3, 7])
uv_u = mathematics.undistort_fm(xy, camera)
h_c = np.array([
-(camera['u0'] - uv_u[0]) / camera['fku'],
-(camera['v0'] - uv_u[1]) / camera['fkv'], 1.0
])
h_w = np.dot(mat_r, h_c)
dxf_dxv = compute_dxf_dxv(h_w, h_c, x)
dxf_dhd = compute_dxf_dhd(h_w, mat_r, xy, camera)
padd = np.diag([camera['sd']**2, camera['sd']**2,
std_rho]).astype(np.float64)
shp0 = cov.shape[0]
shp1 = cov.shape[1]
mat_p2 = np.zeros([shp0 + 6, shp1 + 6], dtype=np.float64)
mat_p2[0:shp0, 0:shp1] = cov
mat_p2[shp0:shp0 + 6, 0:shp1] = np.dot(dxf_dxv, cov[0:13, :])
mat_p2[0:shp0, shp1:shp1 + 6] = np.dot(cov[:, 0:13], dxf_dxv.T)
mat_p2[shp0:shp0 + 6, shp1:shp1 +
6] = (np.dot(np.dot(dxf_dxv, cov[0:13, 0:13]), dxf_dxv.T) +
np.dot(np.dot(dxf_dhd, padd), dxf_dhd.T))
cov = mat_p2
return x, cov, xf
def predict_camera_measurements(self, inparams, x):
"""
:param inparams:
:param x:
:return:
"""
r = x[0:3]
mat_r = mathematics.q2r(x[3:7])
for i in range(len(self.features)):
begin = self.features[i]['begin']
if self.features[i]['type'] == 1:
yi = x[begin:begin + 3]
hi = self.hi(yi, r, mat_r, inparams)
else:
yi = x[begin:begin + 6]
hi = self.hi(yi, r, mat_r, inparams)
self.features[i]['h'] = hi
pass
def pred_patch_fc(f, r, mat_r, xyz, inparams):
"""
:param f:
:param r:
:param mat_r:
:param xyz:
:param inparams:
:return:
"""
uv_p = f['h']
half_patch_wm = f['half_size_wm']
if ((uv_p[0] > half_patch_wm)
and (uv_p[0] < inparams['width'] - half_patch_wm)
and (uv_p[1] > half_patch_wm)
and (uv_p[1] < inparams['height'] - half_patch_wm)):
uv_p_f = f['init_measurement']
mat_r_wk_p_f = mathematics.q2r(f['rotation'])
vec_r_wk_p_f = f['position']
temp1 = np.identity(4, dtype=np.float64)
temp2 = np.identity(4, dtype=np.float64)
temp1[0:3, 0:3] = mat_r_wk_p_f
temp2[0:3, 3] = vec_r_wk_p_f
mat_h_wk_p_f = np.dot(temp1, temp2)
temp1[0:3, 0:3] = mat_r
temp2[0:3, 3] = r
mat_h_wk = np.dot(temp1, temp2)
mat_h_kpf_k = np.dot(mat_h_wk_p_f.T, mat_h_wk)
patch_p_f = f['patch_wi']
half_patch_wi = f['half_size_wi']
n1 = np.zeros(3, dtype=np.float64)
n2 = np.zeros(3, dtype=np.float64)
n1[:] = [
uv_p_f[0] - inparams['u0'], uv_p_f[1] - inparams['v0'],
-inparams['fku']
]
n2[:] = [
uv_p[0] - inparams['u0'], uv_p[1] - inparams['v0'],
-inparams['fku']
]
temp = np.zeros(4, dtype=np.float64)
temp[0:3] = n2
temp[3] = 1
temp = np.dot(mat_h_kpf_k, temp)
temp = temp / temp[3]
n2 = temp[0:3]
n1 = n1 / np.linalg.norm(n1)
n2 = n2 / np.linalg.norm(n2)
n = n1 + n2
n = n / np.linalg.norm(n)
temp[0:3] = xyz
temp[3] = 1
xyz_kpf = np.dot(np.linalg.inv(mat_h_wk_p_f), temp)
xyz_kpf = xyz_kpf / xyz_kpf[3]
d = n[0] * xyz_kpf[0] - n[1] * xyz_kpf[1] - n[2] * xyz_kpf[2]
h3x3 = mat_h_kpf_k[0:3, 0:3]
h3x1 = mat_h_kpf_k[0:3, 3]
uv_p_pred_patch = mathematics.rotate_with_dist_fc_c2c1(
uv_p_f, h3x3, h3x1, n, d, inparams)
uv_c1 = uv_p_pred_patch - half_patch_wm
uv_c2 = mathematics.rotate_with_dist_fc_c1c2(
uv_c1, h3x3, h3x1, n, d, inparams)
uv_c2[:] = np.floor(uv_c2 - uv_p_f - half_patch_wi - 1) - 1
uv_c2[0] = max(uv_c2[0], 0)
uv_c2[1] = max(uv_c2[1], 0)
if uv_c2[0] >= patch_p_f.shape[1] - 2 * half_patch_wm:
uv_c2[0] -= (2 * half_patch_wm + 2)
if uv_c2[1] >= patch_p_f.shape[0] - 2 * half_patch_wm:
uv_c2[1] -= (2 * half_patch_wm + 2)
patch = patch_p_f[uv_c2[1]:uv_c2[1] + 2 * half_patch_wm - 1,
uv_c2[0]:uv_c2[0] + 2 * half_patch_wm - 1]
else:
patch = None
return patch
def add_feature_covariance_id(uvd, x, mat_p, inparams, std_rho):
"""
:param uvd:
:param x:
:param mat_p:
:param inparams:
:param std_rho:
:return:
"""
mat_r = mathematics.q2r(x[3:7])
uv_u = mathematics.undistort_fm(uvd, inparams)
xyz_c = np.zeros(3, np.float64)
x_c = (-(inparams['u0'] - uv_u[0]) / inparams['fku'])
y_c = (-(inparams['v0'] - uv_u[1]) / inparams['fkv'])
xyz_c[:] = [x_c, y_c, 1]
xyz_w = np.dot(mat_r, xyz_c)
dtheta_dgw = np.zeros(3, np.float64)
dtheta_dgw[:] = [(xyz_w[2] / (xyz_w[0] ** 2 + xyz_w[2] ** 2)), 0, (-xyz_w[0] / (xyz_w[0] ** 2 + xyz_w[2] ** 2))]
dphi_dgw = np.zeros(3, np.float64)
dphi_dgw[:] = [(xyz_w[0] * xyz_w[1]) / ((np.sum(xyz_w ** 2)) * np.sqrt(xyz_w[0] ** 2 + xyz_w[2] ** 2)),
-np.sqrt(xyz_w[0] ** 2 + xyz_w[2] ** 2) / (np.sum(xyz_w ** 2)),
(xyz_w[2] * xyz_w[1]) / ((np.sum(xyz_w ** 2)) * np.sqrt(xyz_w[0] ** 2 + xyz_w[2] ** 2))]
dgw_dqwr = mathematics.d_r_q_times_a_by_dq(x[3:7], xyz_c)
dtheta_dqwr = np.dot(dtheta_dgw.T, dgw_dqwr)
dphi_dqwr = np.dot(dphi_dgw.T, dgw_dqwr)
dy_dqwr = np.zeros([6, 4], dtype=np.float64)
dy_dqwr[3, 0:4] = dtheta_dqwr.T
dy_dqwr[4, 0:4] = dphi_dqwr.T
dy_drw = np.zeros([6, 3], dtype=np.float64)
dy_drw[0:3, 0:3] = np.identity(3, dtype=np.float64)
dy_dxv = np.zeros([6, 13], dtype=np.float64)
dy_dxv[0:6, 0:3] = dy_drw
dy_dxv[0:6, 4:8] = dy_dqwr
dyprima_dgw = np.zeros([5, 3], dtype=np.float64)
dyprima_dgw[3, 0:3] = dtheta_dgw.T
dyprima_dgw[4, 0:3] = dphi_dgw.T
dgw_dgc = mat_r
dgc_dhu = np.zeros([3, 2], dtype=np.float64)
dgc_dhu[0:2, 0:2] = np.diag([1 / inparams['fku'], 1 / inparams['fkv']])
dhu_dhd = mathematics.jacob_undistort_fm(uvd, inparams)
dyprima_dhd = np.dot(np.dot(np.dot(dyprima_dgw, dgw_dgc), dgc_dhu), dhu_dhd)
dy_dhd = np.zeros([6, 3], dtype=np.float64)
dy_dhd[0:5, 0:2] = dyprima_dhd
dy_dhd[5, 2] = 1
padd = np.zeros([3, 3], dtype=np.float64)
padd[0:2, 0:2] = np.identity(2, dtype=np.float64) * inparams['sd'] ** 2
padd[2, 2] = std_rho
mat_p2 = np.zeros([mat_p.shape[0] + 6, mat_p.shape[1] + 6], dtype=np.float64)
mat_p2[0:mat_p.shape[0], 0:mat_p.shape[1]] = mat_p
mat_p2[mat_p.shape[0]:mat_p.shape[0] + 6, 0:13] = np.dot(dy_dxv, mat_p[0:13, 0:13])
mat_p2[0:13, mat_p.shape[1]:mat_p.shape[1] + 6] = np.dot(mat_p[0:13, 0:13], dy_dxv.T)
shp0 = mat_p.shape[0]
shp1 = mat_p.shape[1]
if shp0 > 13:
mat_p2[shp0:shp0 + 6, 13:shp1] = np.dot(dy_dxv, mat_p[0:13, 13:])
mat_p2[13:shp0, shp1:shp1 + 6] = np.dot(mat_p[13:, 0:13], dy_dxv.T)
mat_p2[shp0:shp0 + 6, shp1:shp1 + 6] = (
np.dot(np.dot(dy_dxv, mat_p[0:13, 0:13]), dy_dxv.T) +
np.dot(np.dot(dy_dhd, padd), dy_dhd.T))
return mat_p2