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 hi(yi, r, mat_r, inparams):
"""
:param yi:
:param r:
:param mat_r:
:param inparams:
:return:
"""
if yi.shape[0] > 3:
theta, phi, rho = yi[[3, 4, 5]]
mv = mathematics.m(phi, theta)
hrl = np.dot(mat_r.T, (yi[0:3] - r)) * rho + mv
else:
hrl = np.dot(mat_r.T, (yi - r))
if ((math.atan2(hrl[0], hrl[2]) * 180 / math.pi < -60) or
(math.atan2(hrl[0], hrl[2]) * 180 / math.pi > 60) or
(math.atan2(hrl[1], hrl[2]) * 180 / math.pi < -60) or
(math.atan2(hrl[1], hrl[2]) * 180 / math.pi > 60)):
return None
uv_u1 = inparams['u0'] + (hrl[0] / hrl[2]) * inparams['fku']
uv_u2 = inparams['v0'] + (hrl[1] / hrl[2]) * inparams['fkv']
uv_u = np.zeros(2, dtype=np.float64)
uv_u[:] = [uv_u1, uv_u2]
uv_d = mathematics.distort_fm(uv_u, inparams)
# is feature visible ?
if ((uv_d[0] > 0) and (uv_d[0] < inparams['width']) and
(uv_d[1] > 0) and (uv_d[1] < inparams['height'])):
return uv_d
else:
return None
def inverse_depth2xyz(features, x, cov):
"""
:param features:
:param x:
:param cov:
:return:
"""
lin_index_thresh = 0.1
convert = 0
for i in range(len(self.features)):
if convert == 1:
features[i]['begin'] -= 3
continue
if features[i]['type'] == 2:
begin = features[i]['begin']
std_rho = np.sqrt(cov[begin + 5, begin + 5])
rho = x[begin + 5]
std_d = std_rho / (rho**2)
theta = x[begin + 3]
phi = x[begin + 4]
mi = mathematics.m(theta, phi)
x_c1 = x[begin:begin + 3]
x_c2 = x[0:3]
xyz = x_c2 + (1 / rho) * mi
temp = xyz - x_c2
temp2 = xyz - x_c1
d_c2p = np.linalg.norm(temp)
cos_alpha = (np.dot(temp.T, temp2) /
(d_c2p * np.linalg.norm(temp2)))
linearity_index = 4 * std_d * cos_alpha / d_c2p
if linearity_index < lin_index_thresh:
x2 = np.zeros(x.shape[0] - 3, dtype=np.float64)
x2[0:begin] = x[0:begin]
x2[begin:begin + 3] = x[begin:begin + 3]
if x.shape[0] > begin + 6:
x2[begin + 3:] = x[begin + 6:]
mat_j = np.zeros([3, 6], dtype=np.float64)
mat_j[0:3, 0:3] = np.identity(3, dtype=np.float64)
mat_j[:, 3] = (1 / rho) * np.array([
np.cos(phi) * np.cos(theta), 0.0,
-np.cos(phi) * np.sin(theta)
])
mat_j[:, 4] = (1 / rho) * np.array([
-np.sin(phi) * np.sin(theta), -np.cos(phi),
-np.sin(phi) * np.cos(theta)
])
mat_j[:, 5] = mi / rho**2
mat_j_all = np.identity(cov.shape[0], dtype=np.float64)
mat_j_all[begin:begin + mat_j.shape[0],
begin:begin + mat_j.shape[1]] = mat_j
cov = np.dot(np.dot(mat_j_all, cov), mat_j_all.T)
convert = 1
features[i]['type'] = 1
return x, cov
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