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)