Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
def triangulate_midpoint(features, poses):
    """
    Triangulate a landmark from two or more views using the midpoint method.
    """
    assert len(features) > 0
    assert len(features) == len(poses)
    jtj, jtr = np.zeros((3, 3)), np.zeros(3)
    for z, pose in zip(features, poses):
        h = householder(unpr(z))
        a = dots(h, pose.orientation)
        b = dots(h, pose.orientation, pose.position)
        jtj += np.dot(a.T, a)
        jtr += np.dot(a.T, b)
    return np.linalg.solve(jtj, jtr)