def cameraTlaser(vec):
x, y, z, r1, r2, r3, r4 = vec
disp = np.matrix([x, y, z]).T
rot1 = tr.Rx(math.radians(r1))
rot2 = tr.Rz(math.radians(r2))
rot3 = tr.Rx(math.radians(r3))
rot4 = tr.Rz(math.radians(r4))
rt = rot4 * rot3 * rot2 * rot1
laserTcam = tr.composeHomogeneousTransform(rt, disp)
trans = tr.invertHomogeneousTransform(laserTcam)
return trans
def cameraTlaser(vec):
x, y, z, r1, r2, r3, r4 = vec
disp = np.matrix([x, y, z]).T
rot1 = tr.Rx(math.radians(r1))
rot2 = tr.Rz(math.radians(r2))
rot3 = tr.Rx(math.radians(r3))
rot4 = tr.Rz(math.radians(r4))
rt = rot4 * rot3 * rot2 * rot1
laserTcam = tr.composeHomogeneousTransform(rt, disp)
trans = tr.invertHomogeneousTransform(laserTcam)
return trans
def camTlaser( res = np.zeros(6) ):
rot = tr.Ry( math.radians( 0.0 + res[0] )) * tr.Rz( math.radians( 0.0 + res[1] )) * tr.Rx( math.radians( -90.0 + res[2] )) * tr.Rz( math.radians( -90.0 + res[3]))
disp = np.matrix([ res[4], res[5], res[6] ]).T + np.matrix([ 0.0, 0.0, 0.0 ]).T
return tr.composeHomogeneousTransform(rot, disp)
