def interp_transforms(T1, T2, alpha):
    assert alpha <= 1
    T_avg = alpha * T1 + (1 - alpha) * T2
    q1 = quaternion_from_matrix(T1)
    q2 = quaternion_from_matrix(T2)
    q3 = quaternion_slerp(q1, q2, alpha)
    R = quaternion_matrix(q3)
    T_avg[0:3, 0:3] = R[0:3, 0:3]
    return T_avg

 def matrix_slerp(matA, matB, alpha=0.6):
     if matA is None:
         return matB
     import transformations
     qA = transformations.quaternion_from_matrix(matA)
     qB = transformations.quaternion_from_matrix(matB)
     qC =transformations.quaternion_slerp(qA, qB, alpha)
     mat = matB.copy()
     mat[:3,3] = (alpha)*matA[:3,3] + (1-alpha)*matB[:3,3]
     mat[:3,:3] = transformations.quaternion_matrix(qC)[:3,:3]
     return mat

    def getBlendedPose(self, poses, weights, additiveBlending=True):
        import transformations as tm

        REST_QUAT = np.asarray([1,0,0,0], dtype=np.float32)

        if isinstance(poses[0], basestring):
            f_idxs = [self.getPoseNames().index(pname) for pname in poses]
        else:
            f_idxs = poses

        if not additiveBlending:
            # normalize weights
            if not isinstance(weights, np.ndarray):
                weights = np.asarray(weights, dtype=np.float32)
            t = sum(weights)
            if t < 1:
                # Fill up rest with neutral pose (neutral pose is assumed to be first frame)
                weights = np.asarray(weights + [1.0-t], dtype=np.float32)
                f_idxs.append(0)
            weights /= t

        #print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)

        result = emptyPose(self.nBones)
        m = np.identity(4, dtype=np.float32)
        m1 = np.identity(4, dtype=np.float32)
        m2 = np.identity(4, dtype=np.float32)

        if len(f_idxs) == 1:
            for b_idx in xrange(self.nBones):
                m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[0]))
                result[b_idx] = tm.quaternion_matrix( q )[:3,:4]
        else:
            for b_idx in xrange(self.nBones):
                m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
                q1 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m1, True), float(weights[0]))
                q2 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m2, True), float(weights[1]))
                quat = tm.quaternion_multiply(q2, q1)

                for i,f_idx in enumerate(f_idxs[2:]):
                    i += 2
                    m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
                    q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[i]))
                    quat = tm.quaternion_multiply(q, quat)

                result[b_idx] = tm.quaternion_matrix( quat )[:3,:4]

        return Pose(self.name+'-blended', result)

 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 get_roll_to(head, tail, normal):
    """
    Compute the roll angle for a bone to make the bone's local x axis align with
    the specified normal.
    """
    p1 = toZisUp3(head)
    p2 = toZisUp3(tail)
    xvec = normal

    pvec = matrix.normalize(p2-p1)
    xy = np.dot(xvec,pvec)
    yvec = matrix.normalize(pvec-xy*xvec)
    zvec = matrix.normalize(np.cross(xvec, yvec))
    mat = np.asarray((xvec,yvec,zvec), dtype=np.float32)

    try:
        assertOrthogonal(mat)
    except Exception as e:
        log.warning("Calculated matrix is not orthogonal (%s)" % e)
    quat = tm.quaternion_from_matrix(mat)
    if abs(quat[0]) < 1e-4:
        return 0
    else:
        roll = math.pi - 2*math.atan(quat[2]/quat[0])

    if roll < -math.pi:
        roll += 2*math.pi
    elif roll > math.pi:
        roll -= 2*math.pi
    return roll

    def inframe(self, pose, frame):
        """ Transform a pose from one frame to another one.

        Uses transformation matrices. Could be refactored to use directly
        quaternions.
        """
        pose = self.get(pose)

        if pose['frame'] == "map":
            orig = numpy.identity(4)
        else:
            orig = self._to_mat4(self.get(pose["frame"]))

        if frame == "map":
            dest = numpy.identity(4)
        else:
            dest = numpy.linalg.inv(self._to_mat4(self.get(frame)))

        poseMatrix = self._to_mat4(pose)

        transf = numpy.dot(dest, orig)
        transformedPose = numpy.dot(transf, poseMatrix)

        qx,qy,qz,qw = transformations.quaternion_from_matrix(transformedPose)
        x,y,z = transformations.translation_from_matrix(transformedPose)

        return {"x":x,
                "y":y,
                "z":z,
                "qx":qx,
                "qy":qy,
                "qz":qz,
                "qw":qw,
                "frame": frame}

 def getRotation(self):
     """
     Get rotation matrix of rotation of this bone in local space.
     """
     qw,qx,qy,qz = tm.quaternion_from_matrix(self.matPose)
     ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
     return (1000*qw,1000*qx,1000*qy,1000*qz, ax/D,ay/D,az/D)

def calculatePoseError(tf0, tf1):
    numDimMat = 16
    numDimEul = 6
    numDimQuat = 7
    
    numData = len(tf0) # should be the same for tf1
    matrixPoseError = np.empty((numData, numDimMat))
    translationPoseError = np.empty((numData, numDimMat))
    rotationPoseError = np.empty((numData, numDimMat))
    
    
    for i_data in range(numData):
        subtractedTau = tf0 - tf1
        deltaTau = np.lin_alg.inverse(tf0[i_data]).dot(tf1[i_data])
        diffTranslation = deltaTau[:3,3]
        diffRotation = np.eye(4,4)
        diffRotation[:3,:3] = deltaTau[:3,:3]
        
        diffQuat = quaternion_from_matrix(diffRotation)
        diffEuler = euler_from_matrix(diffRotation)
        
        # flip quaternions on the wrong side of the hypersphere
        if diffQuat[3] < 0:
            diffQuat = -diffQuat
            
        pose_error[i_data,:] = np.r_[diff_translation, diff_rot_rep]
    return pose_error

	def __modify_quaternion( self , q , d ) :
		glPushMatrix()
		glLoadMatrixf( tr.quaternion_matrix( q ) )
		glRotatef( d[0] , 0 , 1 , 0 )
		glRotatef( d[1] , 1 , 0 , 0 )
		q = tr.quaternion_from_matrix( glGetDoublev(GL_MODELVIEW_MATRIX) )
		glPopMatrix()
		return q

 def fromMatrix(self, matrix):
     q0 = tm.quaternion_from_matrix(matrix)
     t = matrix[:3,3]
     self.even = q0
     q1 = self.odd
     q1[0] = -0.5*(t[0]*q0[1] + t[1]*q0[2] + t[2]*q0[3])
     q1[1] = 0.5*( t[0]*q0[0] + t[1]*q0[3] - t[2]*q0[2])
     q1[2] = 0.5*(-t[0]*q0[3] + t[1]*q0[0] + t[2]*q0[1])
     q1[3] = 0.5*( t[0]*q0[2] - t[1]*q0[1] + t[2]*q0[0])

def rvec2quat(rvec, cam2body):
    Rned2cam, jac = cv2.Rodrigues(rvec)
    Rned2body = cam2body.dot(Rned2cam)
    Rbody2ned = np.matrix(Rned2body).T
    # make 3x3 rotation matrix into 4x4 homogeneous matrix
    hIR = np.concatenate( (np.concatenate( (Rbody2ned, np.zeros((3,1))),1),
                           np.mat([0,0,0,1])) )
    quat = transformations.quaternion_from_matrix(hIR)
    return quat

def _plot_img(img, K, T, z=0.1):
    obj = mlab.imshow(img.T)
    quat = tf.quaternion_from_matrix(T)
    angle, axis = angle_axis_from_quaternion(quat)
    obj.actor.rotate_wxyz(angle * 180 / np.pi, *axis)
    h, w = img.shape
    xmax_pixel, ymax_pixel = w, h
    point3d = projectionto3d(K, (xmax_pixel, ymax_pixel))[0]
    origin3d = projectionto3d(K, (0, 0))[0]
    point3d = point3d * z / point3d[2]
    origin3d = origin3d * z / origin3d[2]
    center3d = (point3d + origin3d) / 2.
    xmax, ymax, _ = point3d - origin3d
    obj.actor.scale = np.array([xmax / xmax_pixel, ymax / ymax_pixel, 1.0])
    obj.actor.position = utils.apply_transform(T, center3d)
    mlab.view(distance=20 * z)
    return obj

def computeRoll(head, tail, normal, bone=None):
    if normal is None:
        return 0

    p1 = m2b(head)
    p2 = m2b(tail)
    xvec = normal
    pvec = getUnitVector(p2-p1)
    xy = np.dot(xvec,pvec)
    yvec = getUnitVector(pvec-xy*xvec)
    zvec = getUnitVector(np.cross(xvec, yvec))
    if zvec is None:
        return 0
    else:
        mat = np.array((xvec,yvec,zvec))

    checkOrthogonal(mat)
    quat = tm.quaternion_from_matrix(mat)
    if abs(quat[0]) < 1e-4:
        return 0
    else:
        roll = math.pi - 2*math.atan(quat[2]/quat[0])

    if roll < -math.pi:
        roll += 2*math.pi
    elif roll > math.pi:
        roll -= 2*math.pi
    return roll

    if bone and bone.name in ["forearm.L", "forearm.R"]:
        log.debug("B  %s" % bone.name)
        log.debug(" H  %.4g %.4g %.4g" % tuple(head))
        log.debug(" T  %.4g %.4g %.4g" % tuple(tail))
        log.debug(" N  %.4g %.4g %.4g" % tuple(normal))
        log.debug(" P  %.4g %.4g %.4g" % tuple(pvec))
        log.debug(" X  %.4g %.4g %.4g" % tuple(xvec))
        log.debug(" Y  %.4g %.4g %.4g" % tuple(yvec))
        log.debug(" Z  %.4g %.4g %.4g" % tuple(zvec))
        log.debug(" Q  %.4g %.4g %.4g %.4g" % tuple(quat))
        log.debug(" R  %.4g" % roll)

    return roll

def contacts_to_baxter_hand_pose(contact1, contact2):
    c1 = np.array(contact1)
    c2 = np.array(contact2)

    # compute gripper center and axis
    center = 0.5 * (c1 + c2)
    y_axis = c2 - c1
    y_axis = y_axis / np.linalg.norm(y_axis)
    z_axis = np.array([y_axis[1], -y_axis[0], 0])  # the z axis will always be in the table plane for now
    z_axis = z_axis / np.linalg.norm(z_axis)
    x_axis = np.cross(y_axis, z_axis)

    # convert to hand pose
    R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
    t_obj_gripper = center
    T_obj_gripper = np.eye(4)
    T_obj_gripper[:3, :3] = R_obj_gripper
    T_obj_gripper[:3, 3] = t_obj_gripper
    q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)

    return t_obj_gripper, q_obj_gripper

def get_a2b(a, b, rep_out='rotmat'):
    """Returns an SO3 quantity that will align vector a with vector b.
    The output will be in the representation selected by rep_out
    ('rotmat', 'quaternion', or 'rotvec').

    >>> a = trns.random_vector(3)
    >>> b = trns.random_vector(3)
    >>> R = get_a2b(a, b)
    >>> p1 = R.dot(a)
    >>> print(np.allclose(np.cross(p1, b), [0, 0, 0]))
    True
    >>> p1.dot(b) > 0
    True
    >>> q = get_a2b(a, b, 'quaternion')
    >>> p2 = apply_to_points(q, a)
    >>> print(np.allclose(np.cross(p2, b), [0, 0, 0]))
    True
    >>> r = get_a2b(a, b, 'rotvec')
    >>> p3 = apply_to_points(r, a)
    >>> print(np.allclose(np.cross(p3, b), [0, 0, 0]))
    True

    """
    a = normalize(a)
    b = normalize(b)
    cosine = a.dot(b)
    if np.isclose(abs(cosine), 1):
        R = np.eye(3)
    else:
        axis = np.cross(a, b)
        angle = np.arctan2(npl.norm(axis), cosine)
        R = trns.rotation_matrix(angle, axis)
    if rep_out == 'rotmat':
        return R[:3, :3]
    elif rep_out == 'quaternion':
        return trns.quaternion_from_matrix(R)
    elif rep_out == 'rotvec':
        return get_rotvec(R)
    else:
        raise ValueError("Invalid rep_out. Choose 'rotmat', 'quaternion', or 'rotvec'.")

    def prepair_data(self, image_list, matches_list, K):
        # iterate through the image list and build the camera pose dictionary
        # (and a simple list of camera locations for plotting)
        f = open( self.root + '/sba-cams.txt', 'w' )
        for image in image_list:
            body2cam = image.get_body2cam()
            ned2body = image.get_ned2body()
            Rtotal = body2cam.dot( ned2body )
            q = transformations.quaternion_from_matrix(Rtotal)
            rvec, tvec = image.get_proj()
            s = "%.8f %.8f %.8f %.8f %.8f %.8f %.8f\n" % (q[0], q[1], q[2], q[3],
                                                          tvec[0,0], tvec[1,0], tvec[2,0])
            f.write(s)
        f.close()

        # iterate through the matches dictionary to produce a list of matches
        f = open( self.root + '/sba-points.txt', 'w' )
        for match in matches_list:
            ned = np.array(match[0])
            s = "%.4f %.4f %.4f " % (ned[0], ned[1], ned[2])
            f.write(s)
            s = "%d " % (len(match[1:]))
            f.write(s)
            for p in match[1:]:
                image_num = p[0]
                # kp = image_list[image_num].kp_list[p[1]].pt # undistorted
                kp = image_list[image_num].uv_list[p[1]]      # distorted
                s = "%d %.2f %.2f " % (image_num, kp[0], kp[1])
                f.write(s)
            f.write('\n')
        f.close()

        # generate the calibration matrix (K) file
        f = open( self.root + '/sba-calib.txt', 'w' )
        s = "%.4f %.4f %.4f\n" % (K[0,0], K[0,1], K[0,2])
        f.write(s)
        s = "%.4f %.4f %.4f\n" % (K[1,0], K[1,1], K[1,2])
        f.write(s)
        s = "%.4f %.4f %.4f\n" % (K[2,0], K[2,1], K[2,2])
        f.write(s)

 def setRotationIndex(self, index, angle, useQuat):
     if useQuat:
         quat = tm.quaternion_from_matrix(self.matrixPose)
         log.debug("%s", str(quat))
         quat[index] = angle/1000
         log.debug("%s", str(quat))
         normalizeQuaternion(quat)
         log.debug("%s", str(quat))
         self.matrixPose = tm.quaternion_matrix(quat)
         return quat[0]*1000    
     else:
         angle = angle*D
         ax,ay,az = tm.euler_from_matrix(self.matrixPose, 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.matrixPose[:3,:3] = mat[:3,:3]
         return 1000.0

def contacts_to_baxter_hand_pose(c1, c2):
    # compute gripper center and axis
    center = 0.5 * (c1 + c2)
    y_axis = c2 - c1
    print y_axis
    y_axis = y_axis / np.linalg.norm(y_axis)
    print y_axis
    x = np.array([y_axis[1], -y_axis[0], 0]) # the x axis will always be in the table plane for now
    x = x / np.linalg.norm(x)
    z = np.cross(x, y_axis)
    if z[2] < 0:
        x = -x
        z = np.cross(x, y_axis)

    # convert to hand pose
    R_obj_gripper = np.array([x, y_axis, z]).T
    t_obj_gripper = center
    T_obj_gripper = np.eye(4)
    T_obj_gripper[:3,:3] = R_obj_gripper
    T_obj_gripper[:3,3] = t_obj_gripper
    q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)

    return t_obj_gripper, q_obj_gripper 

 def listPose(self):
     for bone in self.boneList:
         quat = tm.quaternion_from_matrix(bone.matrixPose)
         log.debug("  %s %s", bone.name, quat)

def compute_error(trial, descriptor_type, niter, percentile=99):

    src_scene_root = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial);
    src_features_dir = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial)+ "/" + descriptor_type + "_30"

    #read "geo-transformatiom"
    #************GEO**************"
    Tfile = "/data/lidar_providence/downtown_offset-1-financial-Hs.txt"
    Tfis = open(Tfile, 'r')
    lines=[];
    lines = Tfis.readlines();
    scale_geo = float(lines[0])
    Ss_geo = tf.scale_matrix(scale_geo);
    quat_line = lines[1].split(" ");
    quat_geo = np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])])
    Rs_geo = tf.quaternion_matrix(quat_geo);
    trans_line = lines[2].split(" ");
    trans_geo = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]);
    Tfis.close();
    Hs_geo = Rs_geo.copy();
    Hs_geo[:3, 3] = trans_geo[:3]
    Hs_geo = Ss_geo.dot(Hs_geo)
  
    LOG.debug( "\n************Geo************** \n Scale: \n%s \nR:\n%s \nT:\n%s \nH:\n%s", Ss_geo, Rs_geo, trans_geo, Hs_geo)
    
      
    #************Hs**************#  
    #read source to target "Ground Truth" Transformation
    Tfile = src_scene_root + "/Hs.txt";
    Tfis = open(Tfile, 'r')
    lines=[];
    lines = Tfis.readlines();
    scale = float(lines[0])
    Ss = tf.scale_matrix(scale);
    quat_line = lines[1].split(" ");
    quat = tf.unit_vector(np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])]))
    Hs = tf.quaternion_matrix(quat);
    trans_line = lines[2].split(" ");
    Ts = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]);
    Tfis.close();
    Rs = Hs.copy()[:3,:3];
    Hs[:3, 3] = Ts[:3]
    Hs=Ss.dot(Hs) 
    
    Rs = Rs;
    Ts = Ts;
  
    LOG.debug( "\n************Hs************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs, Ts, Hs)
  
    #************Hs IA**************
    #read source to target "Initial Alignment" Transformation
    Tfile = src_features_dir + "/ia_transformation_" + str(percentile) + "_" + str(niter) + ".txt";    
    Tfis = open(Tfile, 'r')
    Hs_ia = np.genfromtxt(Tfis, skip_header=1, usecols={0,1,2,3} );
    Tfis.close()
    Tfis = open(Tfile, 'r')
    Ss_ia=np.genfromtxt(Tfis, skip_footer=4, usecols={0} );
    Tfis.close()
    Rs_ia = Hs_ia[:3,:3]*(1.0/Ss_ia)
    Ts_ia = Hs_ia[:3,3]*(1.0/Ss_ia)
    
    LOG.debug( "\n************Hs IA************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_ia, Ts_ia, Hs_ia)
    
    #Initial Aligment errors
    #Rotation error - half angle between the normalized quaterions
    quat_ia = tf.unit_vector(tf.quaternion_from_matrix(Rs_ia));
    Rs_error_ia_norm = math.acos(abs(np.dot(quat_ia, quat)));
    
    #Translation error
    # x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
    # np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
    Ts_error_ia = (Rs_ia.T).dot(Ts_ia) - (Rs.T).dot(Ts)
    Ts_error_ia_norm = scale_geo*scale*LA.norm(Ts_error_ia)
    
    LOG.debug(  "Error (R,T) %s , %s ", Rs_error_ia_norm , Ts_error_ia_norm )
        
        
    #read source to target "Initial Alignment" Transformation
    #************Hs ICP**************
    Tfile = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(niter) + ".txt";    
    Tfis = open(Tfile, 'r')
    Hs_icp = np.genfromtxt(Tfis, usecols={0,1,2,3});
    Tfis.close()
    
    Hs_icp = Hs_icp.dot(Hs_ia)
    Rs_icp = Hs_icp[:3,:3]*(1.0/Ss_ia)
    Ts_icp = Hs_icp[:3,3]*(1.0/Ss_ia)
    
    LOG.debug( "\n************Hs ICP************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_icp, Ts_icp, Hs_icp)

    #ICP errors    
    #Rotation error - half angle between the normalized quaterions
    quat_icp = tf.unit_vector(tf.quaternion_from_matrix(Rs_icp));
    Rs_error_icp_norm = math.acos(abs(np.dot(quat_icp, quat)));
    
    #Translation error
    # x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
    # np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
    Ts_error_icp = (Rs_icp.T).dot(Ts_icp) - (Rs.T).dot(Ts)
    Ts_error_icp_norm = scale_geo*scale*LA.norm(Ts_error_icp)
#.........这里部分代码省略.........