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
def exp(w):
result = sy.eye(4)
result[:3, :3] = so3.exp(w)
result[:3, 3] = v(w[:3, :]) * w[3:6, :]
return result
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)
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)
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)
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)
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)
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)))
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)
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)))
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)
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)
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
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]
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
