def householder(x):
"""Compute a 2x3 matrix where the rows are orthogonal to x and orthogonal to each other."""
assert len(x) == 3, 'x=%s' % x
assert np.linalg.norm(x) > 1e-8
a = (np.arange(3) == np.argmin(np.abs(x))).astype(float)
u = normalized(np.cross(x, a))
v = normalized(np.cross(x, u))
return np.array([u, v])
def triangulate_directional_relative_pair(z0, z1, relative_pose):
"""
Triangulate a landmark from a relative pose between two cameras.
"""
r, t = relative_pose.inverse().rt
y0 = normalized(unpr(z0))
y1 = normalized(unpr(z1))
tlen = np.linalg.norm(t)
tdir = normalized(t)
tperp = normalized(np.cross(tdir, y0))
if np.linalg.norm(tperp) < 1e-8:
raise Exception("observation is in direction of epipole")
lhs = np.array([
y0 - tdir * np.dot(tdir, y0),
tperp,
tdir
])
a = np.dot(lhs, y0)
b = np.dot(lhs, np.dot(r, y1))
bhead = b[0] * b[0] + b[1] * b[1]
c = a[0] * a[0] - bhead
d = np.sqrt(c * c + 4 * a[0] * a[0] * b[0] * b[0])
e = 2 * b[0] * b[2] * a[0] + a[2] * (a[0] * a[0] - bhead - d)
if abs(b[1]) < 1e-8:
f = -b[0] / (a[0] * b[2] - a[2] * b[0])
else:
f = (a[0] * a[0] - bhead - d) * b[1] / (e * b[1])
rhs = np.array([
f * (a[0] / (2. * d) * (a[0] * a[0] + b[0] * b[0] - b[1] * b[1] + d)),
f * (a[0] / (2. * d) * (2. * b[0] * b[1])),
f * a[2]
])
return np.linalg.solve(lhs, rhs) * tlen
def solve_depth_midpoint(base_feature, base_pose, features, poses):
"""
Solve for depth of a landmark in a priveleged view using the midpoint method.
"""
assert len(features) > 0
assert len(features) == len(poses)
z0 = normalized(unpr(base_feature))
jtj, jtr = 0., 0.
for z, pose in zip(features, poses):
h = householder(unpr(z))
a = dots(h, pose.orientation, base_pose.orientation.T, z0)
b = dots(h, pose.orientation, pose.position - base_pose.position)
jtj += np.dot(a.T, a)
jtr += np.dot(a.T, b)
return jtr / jtj