Esempio n. 1
0
def dba_db(wa, wb):
    A = exp(wa)**-1
    B = exp(wb)
    AB = A * B
    w = log(AB)

    vb = v(wb)
    vba_i = v(w)**-1

    db_db = dw_dw(wb)
    dba_db = so3.dba_db(wa[:3, :], wb[:3, :])

    result = sy.zeros(7, 7)
    result[:3, :3] = dba_db
    result[3:6, 3:6] = vba_i * A[:3, :3] * B[3, 3]
    result[6, 6] = 1

    for i in range(3):
        dvba_db = dv(w, dba_db[:, i])
        dvb_db = dv(wb, db_db[:3, i])

        result[3:6, i] = vba_i * (-dvba_db * w[3:6, :] + so3.exp(-wa[:3, :]) *
                                  (dvb_db * wb[3:6, :] + vb * db_db[3:6, i]))

    dvba_dlambda = dv_dlambda(w)
    dvb_dlambda = dv_dlambda(wb)

    result[3:6, 6] = vba_i * (-dvba_dlambda * w[3:6,:] + \
            so3.exp(-wa[:3,:]) * (dvb_dlambda * wb[3:6,:] + vb * db_db[3:6,6]) - B[3,3] * A[:3, 3])

    return result
Esempio n. 2
0
def exp(w):
    result = sy.eye(4)

    result[:3, :3] = so3.exp(w)
    result[:3, 3] = v(w[:3, :]) * w[3:6, :]

    return result
Esempio n. 3
0
def exp(v):
    """
    'Exponential' map of twist [w, t]
    :param v:
    :return:
    """
    t = tf.expand_dims(v[3:6], 1)
    return tf.concat([so3.exp(v[0:3]), t], axis=1)
Esempio n. 4
0
def PhotometricError(iref, inew, R, T, points, D):
    # points is a tuple ([y], [x]); convert to homogeneous
    siz = iref.shape
    npoints = len(points[0])
    f = siz[1]  # focal length, FIXME
    Xref = np.vstack((
        (points[1] - siz[1] * 0.5) / f,  # x
        (siz[0] * 0.5 - points[0]) / f,  # y (left->right hand)
        np.ones(npoints)))  # z = 1
    # this is confusingly written -- i am broadcasting the translation T to
    # every column, but numpy broadcasting only works if it's rows, hence all
    # the transposes
    # print D * Xref
    Xnew = (np.dot(so3.exp(R), (D * Xref)).T + T).T
    # print Xnew
    # right -> left hand projection
    proj = Xnew[0:2] / Xnew[2]
    p = (-proj[1] * f + siz[0] * 0.5, proj[0] * f + siz[1] * 0.5)
    margin = 10  # int(siz[0] / 5)
    inwindow_mask = ((p[0] >= margin) & (p[0] < siz[0] - margin - 1) &
                     (p[1] >= margin) & (p[1] < siz[1] - margin - 1))
    npts_inw = sum(inwindow_mask)
    if npts_inw < 10:
        return 1e6, np.zeros(6 + npoints)
    # todo: filter points which are now out of the window
    oldpointidxs = (points[0][inwindow_mask], points[1][inwindow_mask])
    newpointidxs = (p[0][inwindow_mask], p[1][inwindow_mask])
    origpointidxs = np.nonzero(inwindow_mask)[0]
    E = InterpolatedValues(inew, newpointidxs) - iref[oldpointidxs]
    # dE/dk ->
    # d/dk r_p^2 = d/dk (Inew(w(r, T, D, p)) - Iref(p))^2
    # = -2r_p dInew/dp dp/dw dw/dX dX/dk
    # = -2r_p * g(w(r, T, D, p)) * dw(r, T, D, p)
    # intensity gradients for each point
    Ig = InterpolatedGradients(inew, newpointidxs)
    # TODO: use tensors for this
    # gradients for R, T, and D
    gradient = np.zeros(6 + npoints)
    for i in range(npts_inw):
        # print 'newidx (y,x) = ', newpointidxs[0][i], newpointidxs[1][i]
        # Jacobian of w
        oi = origpointidxs[i]
        Jw = dw(Xref[0][oi], Xref[1][oi], D[oi], R, T)
        # scale back up into pixel space, right->left hand coords to get
        # Jacobian of p
        Jp = f * np.vstack((-Jw[1], Jw[0]))
        # print origpointidxs[i], 'Xref', Xref[:, i], 'Ig', Ig[:, i], \
        #     'dwdRz', Jw[:, 2], 'dpdRz', Jp[:, 2]
        # full Jacobian = 2*E + Ig * Jp
        J = np.sign(E[i]) * np.dot(Ig[:, i], Jp)
        # print '2 E[i]', 2*E[i], 'Ig*Jp', np.dot(Ig[:, i], Jp)
        gradient[:6] += J[:6]
        # print J[:6]
        gradient[6 + origpointidxs[i]] += J[6]

    print R, T, np.sum(np.abs(E)), npts_inw
    # return ((0.2*(npoints - npts_inw) + np.dot(E, E)), gradient)
    return np.sum(np.abs(E)) / (npts_inw), gradient / (npts_inw)
Esempio n. 5
0
def PhotometricError(iref, inew, R, T, points, D):
    # points is a tuple ([y], [x]); convert to homogeneous
    siz = iref.shape
    npoints = len(points[0])
    f = siz[1]  # focal length, FIXME
    Xref = np.vstack(((points[1] - siz[1]*0.5) / f,  # x
                      (siz[0]*0.5 - points[0]) / f,  # y (left->right hand)
                      np.ones(npoints)))             # z = 1
    # this is confusingly written -- i am broadcasting the translation T to
    # every column, but numpy broadcasting only works if it's rows, hence all
    # the transposes
    # print D * Xref
    Xnew = (np.dot(so3.exp(R), (D * Xref)).T + T).T
    # print Xnew
    # right -> left hand projection
    proj = Xnew[0:2] / Xnew[2]
    p = (-proj[1]*f + siz[0]*0.5, proj[0]*f + siz[1]*0.5)
    margin = 10  # int(siz[0] / 5)
    inwindow_mask = ((p[0] >= margin) & (p[0] < siz[0]-margin-1) &
                     (p[1] >= margin) & (p[1] < siz[1]-margin-1))
    npts_inw = sum(inwindow_mask)
    if npts_inw < 10:
        return 1e6, np.zeros(6 + npoints)
    # todo: filter points which are now out of the window
    oldpointidxs = (points[0][inwindow_mask],
                    points[1][inwindow_mask])
    newpointidxs = (p[0][inwindow_mask], p[1][inwindow_mask])
    origpointidxs = np.nonzero(inwindow_mask)[0]
    E = InterpolatedValues(inew, newpointidxs) - iref[oldpointidxs]
    # dE/dk ->
    # d/dk r_p^2 = d/dk (Inew(w(r, T, D, p)) - Iref(p))^2
    # = -2r_p dInew/dp dp/dw dw/dX dX/dk
    # = -2r_p * g(w(r, T, D, p)) * dw(r, T, D, p)
    # intensity gradients for each point
    Ig = InterpolatedGradients(inew, newpointidxs)
    # TODO: use tensors for this
    # gradients for R, T, and D
    gradient = np.zeros(6 + npoints)
    for i in range(npts_inw):
        # print 'newidx (y,x) = ', newpointidxs[0][i], newpointidxs[1][i]
        # Jacobian of w
        oi = origpointidxs[i]
        Jw = dw(Xref[0][oi], Xref[1][oi], D[oi], R, T)
        # scale back up into pixel space, right->left hand coords to get
        # Jacobian of p
        Jp = f * np.vstack((-Jw[1], Jw[0]))
        # print origpointidxs[i], 'Xref', Xref[:, i], 'Ig', Ig[:, i], \
        #     'dwdRz', Jw[:, 2], 'dpdRz', Jp[:, 2]
        # full Jacobian = 2*E + Ig * Jp
        J = np.sign(E[i]) * np.dot(Ig[:, i], Jp)
        # print '2 E[i]', 2*E[i], 'Ig*Jp', np.dot(Ig[:, i], Jp)
        gradient[:6] += J[:6]
        # print J[:6]
        gradient[6+origpointidxs[i]] += J[6]

    print R, T, np.sum(np.abs(E)), npts_inw
    # return ((0.2*(npoints - npts_inw) + np.dot(E, E)), gradient)
    return np.sum(np.abs(E)) / (npts_inw), gradient / (npts_inw)
Esempio n. 6
0
 def test_exp_log(self):
     v0 = tf.placeholder(dtype=tf.float32, shape=(3, ))
     R = exp(v0)
     v = log(R)
     with self.test_session() as sess:
         for i in range(100):
             init_val = np.random.random(3)
             result = sess.run({'v': v}, feed_dict={v0: init_val})
             self.assertNDArrayNear(result['v'], init_val, EPS)
Esempio n. 7
0
    def test_exp0_log0(self):
        R0 = exp(tf.zeros((3, ), dtype=tf.float32))
        v0 = log(R0)

        with self.test_session():
            print 'R0=', R0.eval()
            self.assertNDArrayNear(R0.eval(), np.identity(3, dtype=np.float32),
                                   EPS)
            print 'v0=', v0.eval()
            self.assertNDArrayNear(v0.eval(), np.zeros(3), EPS)
Esempio n. 8
0
def dw(px, py, d, r, T):
    R = so3.exp(r)
    x0 = np.array([px, py, 1])
    X = np.dot(R, d * x0) + T
    # derivative of projection w = xy/z, as a matrix
    # dw/dk = dw/dX * dX/dk
    dwdX = np.array([[X[2], 0, -X[0]], [0, X[2], -X[1]]]) / (X[2] * X[2])
    dXdR = so3.diff(r, R, d * x0)
    dXdT = np.eye(3)
    dXdd = np.dot(R, x0).reshape((3, 1))
    return np.dot(dwdX, np.hstack((dXdR, dXdT, dXdd)))
Esempio n. 9
0
def ode(state, measurement):
    rg, bg, vg, ba, pg, a_s = state
    wm, am = measurement
    gravity = np.array([0, 0, -9.81])

    # we could subtract out Earth's coriolis force here, but there's no way
    # we're sensitive enough to notice it
    what = wm - bg
    ahat = (am - ba) * a_s

    dvg = np.dot(so3.exp(-rg), ahat) - gravity
    dpg = vg
    return (what, dvg, dpg)
Esempio n. 10
0
def dw(px, py, d, r, T):
    R = so3.exp(r)
    x0 = np.array([px, py, 1])
    X = np.dot(R, d * x0) + T
    # derivative of projection w = xy/z, as a matrix
    # dw/dk = dw/dX * dX/dk
    dwdX = np.array([
        [X[2], 0, -X[0]],
        [0, X[2], -X[1]]]) / (X[2]*X[2])
    dXdR = so3.diff(r, R, d * x0)
    dXdT = np.eye(3)
    dXdd = np.dot(R, x0).reshape((3, 1))
    return np.dot(dwdX, np.hstack((dXdR, dXdT, dXdd)))
Esempio n. 11
0
def ode(state, measurement):
    rg, bg, vg, ba, pg, a_s = state
    wm, am = measurement
    gravity = np.array([0, 0, -9.81])

    # we could subtract out Earth's coriolis force here, but there's no way
    # we're sensitive enough to notice it
    what = wm - bg
    ahat = (am - ba) * a_s

    dvg = np.dot(so3.exp(-rg), ahat) - gravity
    dpg = vg
    return (what, dvg, dpg)
Esempio n. 12
0
 def test_exp(self):
     v = tf.placeholder(dtype=tf.float32, shape=(3, ))
     R = exp(v)
     with self.test_session() as sess:
         for i in range(NUM_TESTS):
             init_val = np.random.random(3)
             self.assertLess(
                 tf.test.compute_gradient_error(x=v,
                                                x_shape=(3, ),
                                                y=R,
                                                y_shape=(3, 3),
                                                x_init_value=init_val,
                                                delta=EPS * 0.1), EPS)
             Rtf = sess.run(R, feed_dict={v: init_val})
             Rcv, _ = cv2.Rodrigues(init_val)
             print 'Rtf=', Rtf
             print 'Rcv=', Rcv
             self.assertNDArrayNear(Rtf, Rcv, EPS)
Esempio n. 13
0
def dba_db(wa, wb):
    AB = exp(wa)**-1 * exp(wb)
    w = log(AB)

    vb = v(wb)
    vba_i = v(w)**-1

    db_db = dw_dw(wb)
    dba_db = so3.dba_db(wa[:3, :], wb[:3, :])

    result = sy.zeros(6, 6)
    result[:3, :3] = result[3:, 3:] = dba_db

    for i in range(3):
        dvba_db = dv(w, dba_db[:, i])
        dvb_db = dv(wb, db_db[:3, i])

        result[3:, i] = vba_i * (-dvba_db * w[3:, :] + so3.exp(-wa[:3, :]) *
                                 (dvb_db * wb[3:, :] + vb * db_db[3:, i]))

    return result
Esempio n. 14
0
def w(px, py, d, r, T):
    R = so3.exp(r)
    X = np.dot(R, np.array([d * px, d * py, d])) + T
    return X[:2] / X[2]
Esempio n. 15
0
def w(px, py, d, r, T):
    R = so3.exp(r)
    X = np.dot(R, np.array([d * px, d * py, d])) + T
    return X[:2] / X[2]
if __name__ == '__main__':
    w_gt = np.random.random(3) - 0.5
    Rgt, _ = cv2.Rodrigues(w_gt)
    Rgt = Rgt.astype(np.float32)
    src_pc = np.random.random([3, NUM_POINTS])
    tgt_pc = Rgt.dot(src_pc)

    # problem: given two sets of corresponding points
    X_src = tf.placeholder(dtype=tf.float32, shape=(3, NUM_POINTS))
    X_tgt = tf.placeholder(dtype=tf.float32, shape=(3, NUM_POINTS))
    w = tf.Variable(initial_value=w_gt + 0.5 * np.random.randn(3),
                    dtype=tf.float32,
                    trainable=True)

    R = so3.exp(w)
    loss = tf.reduce_sum(tf.squared_difference(X_tgt, tf.matmul(R, X_src)))
    err = tf.norm(so3.log(tf.matmul(R, Rgt.T)))

    optimizer = tf.train.GradientDescentOptimizer(
        learning_rate=0.01).minimize(loss)

    with tf.Session() as sess:
        sess.run(tf.global_variables_initializer())
        print 'wgt=', w_gt
        print 'w0=', w.eval()
        for i in range(100):
            print sess.run([w, loss, err],
                           feed_dict={
                               X_src: src_pc,
                               X_tgt: tgt_pc