def checkVisibilityBatchWorld(self, T_w_c, pws, margin=0.01):
T_c_w = np.linalg.inv(T_w_c)
pcs = np.array([tu.transformPt(T_c_w, v) for v in pws])
assert pcs.shape[1] == 3
res = self.project3dBatch(pcs, margin)
return np.array([int(v is not None) for v in res])
def duDpw(fx, fy, p_w, T_w_c):
"""
u = proj(Tcw * p_w)
"""
T_c_w = tfs.inverse_matrix(T_w_c)
p_c = tu.transformPt(T_c_w, p_w)
return np.dot(duDpc(fx, fy, p_c), dpt(T_c_w))
def duDse3_global(fx, fy, p_w, T_w_c):
"""
u = proj((Exp(eps) * T_w_c)^(-1)*p_w)
u ~= proj(T_c_w * Exp(-eps)*p_w)
"""
T_c_w = inverse(T_w_c)
p_c = tu.transformPt(T_c_w, p_w)
return np.dot(duDpc(fx, fy, p_c), -dptDse3_local(T_c_w, p_w))
def dptDse3_global(T, pt):
"""
y = Exp(eps) * T * pt
=> dy / deps
"""
jac = np.zeros((3, 6))
jac[0:3, 0:3] = np.eye(3)
jac[0:3, 3:6] = -1.0 * tu.skewv3(tu.transformPt(T, pt))
return jac
def plotCoordinateFrame3d(T, ax, length=1.0, axes_c=['r', 'g', 'b']):
t = T[0:3, 3]
starts = np.tile(t, (3, 1))
ends = np.zeros((3, 3))
I33 = np.eye(3)
for i in range(3):
e = I33[i, :]
ends[i, :] = tu.transformPt(T, e * length)
ends = ends - starts
ax.quiver(starts[:, 0],
starts[:, 1],
starts[:, 2],
ends[:, 0],
ends[:, 1],
ends[:, 2],
colors=axes_c,
arrow_length_ratio=0.0)
def dfDpw(p_w, T_w_c):
T_c_w = inverse(T_w_c)
p_c = tu.transformPt(T_c_w, p_w)
return np.dot(df_dv(p_c), dpt(T_c_w))
def dfDse3_global(p_w, T_w_c):
T_c_w = inverse(T_w_c)
p_c = tu.transformPt(T_c_w, p_w)
return np.dot(df_dv(p_c), -dptDse3_local(T_c_w, p_w))
def dfDpw(p_w, T_w_c):
T_c_w = inverse(T_w_c)
p_c = tu.transformPt(T_c_w, p_w)
return np.dot(df_dv(p_c), dpt(T_c_w))
if __name__ == '__main__':
pt = [2, 3, 4]
T = np.eye(4)
T[0:3, 3] = [1, 2, 3]
#%% basic test
print("Pt:\n{0}, T:\n{1}".format(pt, T))
pt1 = tu.transformPt(T, pt)
print("Transformed point:\n{0}".format(pt1))
dpt1_dse3 = dptDse3_global(T, pt)
print("dpt1/dse:\n{0}".format(dpt1_dse3))
dpt1_dpt = dpt(T)
print("dpt1/dpt:\n{0}".format(dpt1_dpt))
#%% test the jacobian of camera projection
eps = 1e-4
cam = Camera.createTestCam()
pc = np.array([0.1, 0.5, 2.0])
du_dpc_ana = duDpc(cam.fx, cam.fy, pc)
u0 = cam.project3d(pc)
assert u0 is not None
def project3DWorldBatch(self, T_w_c, pws):
T_c_w = np.linalg.inv(T_w_c)
pcs = np.array([tu.transformPt(T_c_w, v) for v in pws])
return self.project3dBatch(pcs)
def project3DWorld(self, T_w_c, pw):
T_c_w = np.linalg.inv(T_w_c)
return self.project3d(tu.transformPt(T_c_w, pw))