def handle(self):
        global rx, ry, rz, crx, cry, crz, R, cR, paramInit

        data = self.request[0].strip()
        print data
        (rx,ry,rz,crx,cry,crz) = map(lambda x: float(x), data.split(','))

        R = euler_matrix(rx,ry,rz)[0:3,0:3].transpose()
        cR = euler_matrix(crx, cry, crz)[0:3,0:3]
        paramInit = True

 def test_euler_from_matrix_random_angles_all_axes(self):
     angles = (4*math.pi) * (numpy.random.random(3) - 0.5)
     successes = []
     failures = []
     for axes in transformations._AXES2TUPLE.keys():
         R0 = transformations.euler_matrix(axes=axes, *angles)
         R1 = transformations.euler_matrix(axes=axes, *transformations.euler_from_matrix(R0, axes))
         if numpy.allclose(R0, R1):
             successes.append(axes)
         else:
             failures.append(axes)
     self.assertEquals(0, len(failures), "Failed for:\n%sand succeeded for:\n%s" % (failures, successes))

def GetQ50CameraParams():
    cam = {}
    for i in [601, 604]:
        cam[i] = {}
        cam[i]['R_to_c_from_i'] = np.array([[-1, 0, 0],
                                            [0, 0, -1],
                                            [0, -1, 0]])

        if i == 601: # right camera
            R_to_c_from_l_in_camera_frame = euler_matrix(0.022500,-0.006000,-0.009500)[0:3,0:3]
            # R_to_c_from_l_in_camera_frame = euler_matrix(0.037000,0.025500,-0.001000)[0:3,0:3]
            cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame
            cam[i]['displacement_from_l_to_c_in_lidar_frame'] = \
                np.array([-0.522, 0.2925, 0.273])
            cam[i]['E'] = np.eye(4)
            cam[i]['width'] = 1280
            cam[i]['height'] = 800
            cam[i]['fx'] = 1.95205250e+03
            cam[i]['fy'] = 1.95063141e+03
            cam[i]['cu'] = 6.54927553e+02
            cam[i]['cv'] = 4.01883041e+02
            cam[i]['distort'] = np.array([-3.90035052e-01, 6.85186191e-01, 3.22989088e-03, 1.02017567e-03])
            # cam[i]['fx'] = 2.03350359e+03
            # cam[i]['fy'] = 2.03412303e+03
            # cam[i]['cu'] = 5.90322443e+02
            # cam[i]['cv'] = 4.30538351e+02
            # cam[i]['distort'] = np.array([-4.36806404e-01, 2.16674616e+00, -1.02905742e-02, 1.81030435e-03, -1.05642273e+01])

        elif i == 604: # left camera
            R_to_c_from_l_in_camera_frame = euler_matrix(0.025000,-0.011500,0.000000)[0:3,0:3]
            cam[i]['R_to_c_from_l_in_camera_frame'] = R_to_c_from_l_in_camera_frame
            cam[i]['displacement_from_l_to_c_in_lidar_frame'] = \
                np.array([-0.522, -0.2925, 0.273])
            cam[i]['E'] = np.eye(4)
            cam[i]['width'] = 1280
            cam[i]['height'] = 800
            cam[i]['fx'] = 1.95205250e+03
            cam[i]['fy'] = 1.95063141e+03
            cam[i]['cu'] = 6.54927553e+02
            cam[i]['cv'] = 4.01883041e+02
            cam[i]['distort'] = np.array([-3.90035052e-01, 6.85186191e-01, 3.22989088e-03, 1.02017567e-03])

        cam[i]['KK'] = np.array([[cam[i]['fx'], 0.0, cam[i]['cu']],
                                 [0.0, cam[i]['fy'], cam[i]['cv']],
                                 [0.0, 0.0, 1.0]])
        cam[i]['f'] = (cam[i]['fx'] + cam[i]['fy']) / 2

    return cam

def GPSPos(GPSData, Camera, start_frame):
    roll_start = -deg2rad(start_frame[7]);
    pitch_start = deg2rad(start_frame[8]);
    yaw_start = -deg2rad(start_frame[9]+90);

    psi = pitch_start; 
    cp = cos(psi);
    sp = sin(psi);
    theta = roll_start;
    ct = cos(theta);
    st = sin(theta);
    gamma = yaw_start;
    cg = cos(gamma);
    sg = sin(gamma);

    R_to_i_from_w = \
            array([[cg*cp-sg*st*sp, -sg*ct, cg*sp+sg*st*cp],
                  [sg*cp+cg*st*sp, cg*ct, sg*sp-cg*st*cp],
                  [-ct*sp, st, ct*cp]]).transpose()


    pts = WGS84toENU(start_frame[1:4], GPSData[:, 1:4])
    world_coordinates = pts;
    pos_wrt_imu = dot(R_to_i_from_w, world_coordinates);
    R_to_c_from_i = Camera['R_to_c_from_i']
    R_camera_pitch = euler_matrix(Camera['rot_x'], Camera['rot_y'],\
            Camera['rot_z'], 'sxyz')[0:3,0:3]
    R_to_c_from_i = dot(R_camera_pitch, R_to_c_from_i) 

    pos_wrt_camera = dot(R_to_c_from_i, pos_wrt_imu);

    pos_wrt_camera[0,:] += Camera['t_x'] #move to left/right
    pos_wrt_camera[1,:] += Camera['t_y'] #move up/down image
    pos_wrt_camera[2,:] += Camera['t_z'] #move away from cam
    return pos_wrt_camera

 def setRotationIndex(self, index, angle, useQuat):
     """
     Set the rotation for one of the three rotation channels, either as
     quaternion or euler matrix. index should be 1,2 or 3 and represents
     x, y and z axis accordingly
     """
     if useQuat:
         quat = tm.quaternion_from_matrix(self.matPose)
         log.debug("%s", str(quat))
         quat[index] = angle/1000
         log.debug("%s", str(quat))
         _normalizeQuaternion(quat)
         log.debug("%s", str(quat))
         self.matPose = tm.quaternion_matrix(quat)
         return quat[0]*1000
     else:
         angle = angle*D
         ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
         if index == 1:
             ax = angle
         elif index == 2:
             ay = angle
         elif index == 3:
             az = angle
         mat = tm.euler_matrix(ax, ay, az, axes='sxyz')
         self.matPose[:3,:3] = mat[:3,:3]
         return 1000.0

def GetQ50LidarParams():
    params = { }
    params['R_from_i_to_l'] = euler_matrix(-0.04, -0.0146, -0.0165)[0:3,0:3]
    params['T_from_l_to_i'] = np.eye(4)
    params['T_from_l_to_i'][0:3,0:3] = params['R_from_i_to_l'].transpose()
    params['height'] = 2.0
    return params

 def _unpack(x):
     trans = x[:3]
     eulxyz = x[3:]
     T = tf.concatenate_matrices(tf.translation_matrix(trans),
                                 tf.euler_matrix(eulxyz[0], eulxyz[1],
                                                 eulxyz[2], 'sxyz'))
     return T

def test_wrist_ik(goal_roll, goal_pitch, goal_yaw, or_obj = None):
    from math import cos, sin, atan2
    from transformations import euler_matrix
    # try:
    #     import tf
    #     R = tf.transformations.euler_matrix(goal_roll, goal_pitch, goal_yaw)
    # except ImportError:
    #     print 'TF module not found, using local euler_matrix()'
    #     R = euler_matrix(goal_roll, goal_pitch, goal_yaw)

    R = euler_matrix(goal_roll, goal_pitch, goal_yaw)

    # do the math
    theta_4 = atan2(-R[1,2], -R[0,2])
    
    s_theta_5 = -R[0,2] * cos(theta_4) - R[1,2] * sin(theta_4)
    c_theta_5 = R[2,2]
    theta_5 = atan2(s_theta_5, c_theta_5)

    s_theta_6 = -R[0,0] * sin(theta_4) + R[1,0] * cos(theta_4)
    c_theta_6 = -R[0,1] * sin(theta_4) + R[1,1] * cos(theta_4)
    theta_6 = atan2(s_theta_6, c_theta_6)

    print 'Solution:', (theta_4, theta_5, theta_6)

    # visualize if an OpenRave object is provided
    if or_obj:
        import show_axes

        # show goal
        show_axes.show_axes('goal', R, or_obj, 'small')

        show_wrist_fk(theta_4, theta_5, theta_6, or_obj)

    return 

    def update_local_transformation(self):
        """
        update local transformation w.r.t element's parent

        .. seealso:: VRML transform calculation http://www.web3d.org/x3d/specifications/vrml/ISO-IEC-14772-VRML97/part1/nodesRef.html#Transform
        """
        if self.jointType in ["free", "freeflyer"]:
            self.localTransformation = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])

            scale = [1,1,1]
            if self.parent:
                scale = self.cumul_scale()
            scale_translation = [self.translation[i]*scale[i] for i in range(3)]
            self.localTransformation[0:3,3]=numpy.array(scale_translation)+\
                numpy.dot(numpy.eye(3)-self.localTransformation[0:3,:3],numpy.array(self.center))

        elif ( type(self) == Joint and self.jointType in [ "rotate", "rotation", "revolute"]
               and self.jointId >= 0):
            if self.jointAxis in ["x","X"]:
                axis = [1, 0, 0]
            elif self.jointAxis in ["y","Y"]:
                axis = [0, 1, 0]
            elif self.jointAxis in ["z","Z"]:
                axis = [0, 0, 1]
            else:
                raise RuntimeError("Invalid joint axis {0}".format(self.jointAxis))
            self.localR2= tf.rotation_matrix(self.angle, axis)[:3,:3]
            self.localTransformation[0:3,0:3] = numpy.dot(self.localR1, self.localR2)

def GetQ50LidarParams():
    params = { } 
    params['R_from_i_to_l'] = euler_matrix(-0.005,-0.0081,-0.0375)[0:3,0:3]
    params['T_from_l_to_i'] = np.eye(4) 
    params['T_from_l_to_i'][0:3,0:3] = params['R_from_i_to_l'].transpose()

    return params

    def frame(self, i):

        rnds = self.rnds
        roll = math.sin(i * .01 * rnds[0] + rnds[1])
        pitch = math.sin(i * .01 * rnds[2] + rnds[3])
        yaw = math.pi * math.sin(i * .01 * rnds[4] + rnds[5])
        x = math.sin(i * 0.01 * rnds[6])
        y = math.sin(i * 0.01 * rnds[7])

        x,y,z = -0.5,0.5,1
        roll,pitch,yaw = (0,0,0)

        z = 4 + 3 * math.sin(i * 0.1 * rnds[8])
        print z

        rz = transformations.euler_matrix(roll, pitch, yaw)
        p = Plane(Vec3(x, y, z), under(Vec3(0,0,-1), rz), under(Vec3(1, 0, 0), rz))

        acc = 0
        for r in self.r:
            (pred, h, norm) = p.hit(r)
            l = numpy.where(pred, texture(p.localxy(r.project(h))), 0.0)
            acc += l
        acc *= (1.0 / len(self.r))

        # print "took", time.time() - st

        img = cv.CreateMat(self.h, self.w, cv.CV_8UC1)
        cv.SetData(img, (clamp(0, acc, 1) * 255).astype(numpy.uint8).tostring(), self.w)
        return img

	def get_transformed_model(self, transforms):
		t = transforms

		scaling_matrix = np.matrix([
			[t['sx']/100, 0, 0, 1],
			[0, t['sy']/100, 0, 1],
			[0, 0, t['sz']/100, 1],
			[0, 0, 0, 1]
		])

		translation_matrix = np.matrix([
			[1, 0, 0, t['tx']],
			[0, 1, 0, t['ty']],
			[0, 0, 1, t['tz']],
			[0, 0, 0, 1   ]
		])

		rotation_matrix = np.matrix(euler_matrix(
			rad(t['rx']),
			rad(t['ry']),
			rad(t['rz'])
		))

		matrix = scaling_matrix * translation_matrix * rotation_matrix

		leds_ = leds.copy()
		leds_ = np.pad(leds_, (0,1), 'constant', constant_values=1)[:-1]
		leds_ = np.rot90(leds_, 3)
		leds_ = np.dot(matrix, leds_)
		leds_ = np.rot90(leds_)
		leds_ = np.array(leds_)

		return leds_

 def setRotation(self, angles):
     """
     Sets rotation of this bone (in local space) as Euler rotation
     angles x,y and z.
     """
     ax,ay,az = angles
     mat = tm.euler_matrix(ax, ay, az, axes='szyx')
     self.matPose[:3,:3] = mat[:3,:3]

 def _x2transform(self, x):
     trans, eulxyz, scale = self._unpack(x)
     origin = [0, 0, 0]
     T = tf.concatenate_matrices(tf.translation_matrix(trans),
                                 tf.euler_matrix(eulxyz[0], eulxyz[1],
                                                 eulxyz[2], 'sxyz'),
                                 tf.scale_matrix(scale, origin))
     return T

	def __modify_euler( self , a , d ) :
		glPushMatrix()
		glLoadMatrixf( tr.euler_matrix( *a ) )
		glRotatef( d[0] , 0 , 1 , 0 )
		glRotatef( d[1] , 1 , 0 , 0 )
		a = np.array( tr.euler_from_matrix( glGetDoublev(GL_MODELVIEW_MATRIX) ) )
		glPopMatrix()
		return a

	def _draw_scene( self ) :
		glPushMatrix()
		glTranslatef( *self.b )
		glPushMatrix()
		glMultMatrixd( tr.euler_matrix( *self.ab ) )
		self.frame.draw()
		glPopMatrix()

		glTranslatef( *self.c )
		glMultMatrixd( tr.euler_matrix( *self.ac ) )
		self.frame.draw()
		glPopMatrix()

		glPushMatrix()
		glTranslatef( *self.e )
		glMultMatrixd( tr.euler_matrix( *self.ae ) )
		self.frame.draw()
		glPopMatrix()

def channel_from_euler(roll, pitch, yaw, dodeca_array=ar, verbosity = 0):
    """ Get dodecahedron side from Euler angle """
    rr = euler_matrix(roll, pitch, yaw, 'sxyz')
    rr3 = rr[0:3, 0:3]
    #distance,index = spatial.KDTree(dodeca_array).query(rr3[2])
    index = get_closest(dodeca_array, rr3[2])
    if verbosity > 0: 
        print "{}".format(index)
    return index

def computeReprojection(C, raw_pts, cam):
    pts = raw_pts[:, 0:3].copy()
    pts[:, 0] += C[0]
    pts[:, 1] += C[1]
    pts[:, 2] += C[2]
    R = euler_matrix(C[3], C[4], C[5])[0:3,0:3]
    pts_wrt_cam = np.dot(R, np.dot(R_to_c_from_l_old(cam), pts.transpose()))
    pix = np.around(np.dot(cam['KK'], np.divide(pts_wrt_cam[0:3,:], pts_wrt_cam[2, :])))
    pix = pix.astype(np.int32)
    return (pix, pts_wrt_cam)

    def rotatePoint(self, point, theta, phi):

        R = euler_matrix(0, np.deg2rad(theta), np.deg2rad(phi))

        if len(point) == 3:
            point = np.append(point, 1)

        new_point = np.dot(R, point)[0:3]

        return new_point

def setPersp(scale_factor = 1, distort_location = '/afs/cs.stanford.edu/u/twangcat/scratch/sail-car-log/process/'):    
    distort = { }
    distort['M_1'] = [np.eye(3)] # perspective distrotion in cam1 image space
    distort['M_2'] = [np.eye(3)] # perspective distrotion in cam2 image space
    distort['T'] = [np.eye(4)] # corresponding real-world transformation


    distort['T'].insert(0,translation_matrix(np.array([1.0,0,0])))
    distort['T'].insert(0,translation_matrix(np.array([-1.0,0,0])))
    distort['T'].insert(0,euler_matrix(0, 0.06, 0))
    distort['T'].insert(0,euler_matrix(0, -0.06, 0))
    distort['T'].insert(0,euler_matrix(0, 0.12, 0))
    distort['T'].insert(0,euler_matrix(0, -0.12, 0))


    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam1.pickle'))) # shift 1.0
    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam1.pickle'))) # shift -1.0
    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam1.pickle','r'))) # rotate 0.06
    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam1.pickle','r'))) # rotate -0.06
    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam1.pickle','r'))) # rotate 0.12
    distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam1.pickle','r'))) # rotate -0.12




    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam2.pickle'))) # shift 1.0
    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam2.pickle'))) # shift -1.0
    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam2.pickle','r'))) # rotate 0.06
    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam2.pickle','r'))) # rotate -0.06
    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam2.pickle','r'))) # rotate 0.12
    distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam2.pickle','r'))) # rotate -0.12

    assert len(distort['M_1']) == len(distort['M_2']) and len(distort['M_1'])==len(distort['T'])
    if scale_factor != 1:
      for i in xrange(len(distort['M_1'])-1): # last one is always identity, no need change.
        distort['M_1'][i][:,0:2]/=scale_factor
        distort['M_1'][i][2,:]/=scale_factor
        distort['M_2'][i][:,0:2]/=scale_factor
        distort['M_2'][i][2,:]/=scale_factor


    return distort