Python transformations.euler_from_matrix函数代码示例

def doTest2(listBones):            
    fp = open(thePosesFile, "w")
    for bone,coords in listBones:
        fp.write("\n%s %.4f\n" % (bone.name, bone.roll)) 
        writeMat(fp, "Rest", bone.matrixRest)
        writeMat(fp, "Global", bone.matrixGlobal)
        pose = bone.getPoseFromGlobal()
        writeMat(fp, "Pose", pose)
        xyz = tm.euler_from_matrix(pose, axes='sxyz')
        fp.write("XYZ %.4g %.4g %.4g\n" % tuple(xyz))
        zyx = tm.euler_from_matrix(pose, axes='szyx')
        fp.write("ZYX %.4g %.4g %.4g\n" % tuple(zyx))
        #writeMat(fp, "Pose0", bone.matrixPose)
    fp.close()

    def predictSinglePose(self, armName, curPose, prevPose, dt=1.0):
        if dt <= 0:
            print 'Error: Illegal timestamp'
            return None

        # Convert pose to numpy matrix
        curTrans = tft.translation_matrix([curPose.position.x, curPose.position.y, curPose.position.z])
        curRot = tft.quaternion_matrix([curPose.orientation.x, curPose.orientation.y ,curPose.orientation.z, curPose.orientation.w])
        curPoseMatrix = np.dot(curTrans, curRot)

        prevTrans = tft.translation_matrix([prevPose.position.x, prevPose.position.y, prevPose.position.z])
        prevRot = tft.quaternion_matrix([prevPose.orientation.x, prevPose.orientation.y ,prevPose.orientation.z, prevPose.orientation.w])
        prevPoseMatrix = np.dot(prevTrans, prevRot)
        
        deltaPoseMatrix = np.linalg.inv(prevPoseMatrix).dot(curPoseMatrix)
        deltaAngles = euler_from_matrix(deltaPoseMatrix[:3,:3])
        deltaPos = deltaPoseMatrix[:3,3]

        #deltaAngles = np.array([a / dt for a in deltaAngles])
        deltaPos = deltaPos / dt
        #deltaPoseMatrix = euler_matrix(deltaAngles[0], deltaAngles[1], deltaAngles[2])
        deltaPoseMatrix[:3,3] = deltaPos

        gpList, sysList = self.predict(armName, [curPoseMatrix], [deltaPoseMatrix])
        return tfx.pose(gpList[0], frame=curPose.frame, stamp=curPose.stamp), tfx.pose(sysList[0], frame=curPose.frame, stamp=curPose.stamp)

 def componentwiseError(self, estimatedPoses, truePoses):
     n_poses = len(estimatedPoses)
     n_dim = estimatedPoses[0].shape[0] * estimatedPoses[0].shape[1]
     n_trans = 3
     n_rot = 3
     translationError = np.zeros((n_poses, n_trans))
     rotationError = np.zeros((n_poses, n_rot))
     
     for i in range(n_poses):
         estimatedPose = estimatedPoses[i]
         truePose = truePoses[i]
         deltaPose = np.linalg.inv(estimatedPose).dot(truePose)
         
         deltaRotation = np.eye(4,4)
         deltaRotation[:3,:3] = deltaPose[:3,:3]
         
         translationError[i,:] = deltaPose[:3,3].T
         rotationError[i,:] = euler_from_matrix(deltaRotation)
         
         #if np.abs(translationError[i,1]) > 0.01:
         #    print 'Outlier', i
         
     combinedError = np.c_[translationError, rotationError]
     meanError = np.mean(np.abs(combinedError), axis=0)
     medianError = np.median(np.abs(combinedError), axis=0)
     rmsError = np.sqrt(np.mean(np.square(combinedError), axis=0))
     maxError = np.max(np.abs(combinedError), axis=0)
     
     predictionError = PredictionError(medianError, meanError, rmsError, maxError)
     return predictionError

    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        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[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3,0:4]=[0,0,0,1]

	def determine_obj_frame(self):
		if self.detected_artags != None:

			eng_poses = []
			ar_tfs = []
			for da in self.detected_artags.markers:
				ar_pose = th.aruco_to_pose_stamp(da)

				eng_pose = self.get_engine_pose_from_ps(ar_pose)

				eng_poses.append(eng_pose)
				ar_tfs.append(th.pose_stamp_to_tf(eng_pose))

			ar_tf_sum = np.zeros((4,4))
			for tf in ar_tfs:
				ar_tf_sum = ar_tf_sum + tf/len(ar_tfs)
				
			engine_frame_combined = ar_tf_sum
			
			tran = transformations.translation_from_matrix(engine_frame_combined), 
			eul = transformations.euler_from_matrix(engine_frame_combined)
			
			
			self.updated_engine_pose = str(tran[0][0]) + ' ' + str(tran[0][1]) + ' ' + str(tran[0][2]) + ' ' + str(eul[0]) + ' ' + str(eul[1]) + ' ' + str(eul[2]) 
			
			ps = th.tf_to_pose_stamp(engine_frame_combined, 'engine_frame_combined')
			
			th.broadcast_transform(ps, global_frame)
			return engine_frame_combined
		else: 
			return None

    def fromSkeleton(self, skel, animationTrack = None):
        """
        Construct a BVH object from a skeleton structure and optionally an 
        animation track. If no animation track is specified, a dummy animation
        of one frame will be added.
        NOTE: Make sure that the skeleton has only one root.
        """

        # Traverse skeleton joints in depth-first order
        for jointName in skel.getJointNames():
            bone = skel.getBone(jointName)
            if bone.parent:
                joint = self.addJoint(bone.parent.name, jointName)
                joint.channels = ["Zrotation", "Xrotation", "Yrotation"]
            else:
                joint = self.addRootJoint(bone.name)
                joint.channels = ["Xposition", "Yposition", "Zposition", "Zrotation", "Xrotation", "Yrotation"]
    
            offset = bone.getRestOffset()
            self.__calcPosition(joint, offset)
            if not bone.hasChildren():
                endJoint = self.addJoint(jointName, 'End effector')
                offset = bone.getRestTailPos() - bone.getRestHeadPos()
                self.__calcPosition(endJoint, offset)

        self.__cacheGetJoints()
        nonEndJoints = [ joint for joint in self.getJoints() if not joint.isEndConnector() ]

        if animationTrack:
            self.frameCount = animationTrack.nFrames
            self.frameTime = 1.0/animationTrack.frameRate

            jointToBoneIdx = {}
            for joint in nonEndJoints:
                jointToBoneIdx[joint.name] = skel.getBone(joint.name).index

            for fIdx in xrange(animationTrack.nFrames):
                offset = fIdx * animationTrack.nBones
                for jIdx,joint in enumerate(nonEndJoints):
                    bIdx = jointToBoneIdx[joint.name]
                    poseMat = animationTrack.data[offset + bIdx]
                    if len(joint.channels) == 6:
                        # Add transformation
                        tx, ty, tz = poseMat[:3,3]
                        joint.frames.extend([tx, ty, tz])
                    ay,ax,az = tm.euler_from_matrix(poseMat, "syxz")
                    joint.frames.extend([az/D, ax/D, ay/D])
        else:
            # Add bogus animation with one frame
            self.frameCount = 1
            self.frameTime = 1.0
            for jIdx,joint in enumerate(nonEndJoints):
                if len(joint.channels) == 6:
                    # Add transformation
                    joint.frames.extend([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
                else:
                    joint.frames.extend([0.0, 0.0, 0.0])

        for joint in self.getJoints():
            joint.calculateFrames()

def writeAnimation(fp, bone, action, config):
    aname = "Action%s" % bone.name
    points = action[bone.name]

    for channel,default in [
            ("T", 0),
            ("R", 0),
            ("S", 1)
        ]:
        writeAnimationCurveNode(fp, bone, channel, default)

    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    translations = []
    eulers = []
    R = 180/math.pi
    for quat in points:
        mat = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat)
        translations.append(mat[:3,3])
        eul = tm.euler_from_matrix(mat, axes='sxyz')
        eulers.append((eul[0]*R, eul[1]*R, eul[2]*R))
    scales = len(points)*[(1,1,1)]

    for channel,data in [
            ("T", translations),
            ("R", eulers),
            ("S", scales)
        ]:
        for idx,coord in enumerate(["X","Y","Z"]):
            writeAnimationCurve(fp, idx, coord, bone, channel, data)

    def updateRadar(self):
        # Taken from testDrawRadarOnMap.py
        fren = self.iren.GetRenderWindow().GetRenderers().GetFirstRenderer()
        t = self.start + self.step * self.count
        radar_data = loadRDR(self.rdr_pts[t])[0]

        if radar_data.shape[0] > 0:
            #Convert from radar to lidar ref-frame
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], self.radar_params)

            #Convert from lidar to IMU ref-frame
            radar_data[:, :3] = np.dot(self.lidar_params['T_from_l_to_i'][:3, :3],
                radar_data[:, :3].transpose()).transpose()

            h_radar_data = np.hstack((radar_data[:, :3], np.ones((radar_data.shape[0], 1))))

            radar_data[:, :3] = np.dot(self.imu_transforms[t],
                h_radar_data.transpose()).transpose()[:, :3]

            for i in xrange(len(self.radar_actors)):
                fren.RemoveActor(self.radar_actors[i])

            self.radar_actors = []
            self.radar_clouds = []

            for i in xrange(radar_data.shape[0]):
                self.radar_clouds.append(VtkBoundingBox(radar_data[i, :]))
                (ax, ay, az) = euler_from_matrix(self.imu_transforms[t])
                box = self.radar_clouds[i].get_vtk_box(rot=az*180/np.pi)
                self.radar_actors.append(box)
                fren.AddActor(self.radar_actors[i])

def writeBoneProp(fp, bone, config):
    id,key = getId("Model::%s" % bone.name)
    fp.write(
'    Model: %d, "%s", "LimbNode" {' % (id,key) +
"""
        Version: 232
        Properties70:  {
            P: "RotationActive", "bool", "", "",1
            P: "InheritType", "enum", "", "",1
            P: "ScalingMax", "Vector3D", "Vector", "",0,0,0
            P: "DefaultAttributeIndex", "int", "Integer", "",0
""")

    mat = bone.getRelativeMatrix(config)
    trans = mat[:3,3]
    e = tm.euler_from_matrix(mat, axes='sxyz')
    fp.write(
'            P: "Lcl Translation", "Lcl Translation", "", "A",%.4f,%.4f,%.4f\n' % tuple(trans) +
'            P: "Lcl Rotation", "Lcl Rotation", "", "A",%.4f,%.4f,%.4f\n' % (e[0]*R, e[1]*R, e[2]*R) +
'            P: "Lcl Scaling", "Lcl Scaling", "", "A",1,1,1\n' +
'            P: "MHName", "KString", "", "", "%s"' % bone.name +
"""
        }
        Shading: Y
        Culling: "CullingOff"
    }
""")

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 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 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 get_SVD(self):
        """
        Get the affine transformation matrix that best matches the moelcules cxns
        to each rod cxn by SVD

        We do a Euclidean (rigid) transform
        """

        print("\n\nCalculating intial affine transformations:")
        print("\n\nM = affine transformation for best fit of mol cxn -> rod cxn:")
        for i in range(len(self.cxns)):
            for it in self.mol.permutations:
                for dc in self.dc_ints: 
                    pass
            a = self.mol.cxns[0:3,:]
            b = self.cxn_xyz[i]

            M = trans.affine_matrix_from_points(a, b, shear = False, scale = False, usesvd = True)

            alpha, beta, gamma = trans.euler_from_matrix(M)
            translations = M[0:3,3]
            conn_start_ind = 1 + len(self.rods) + i*6
            self.opt_vec[conn_start_ind+0] = alpha  
            self.opt_vec[conn_start_ind+1] = beta 
            self.opt_vec[conn_start_ind+2] = gamma 
            self.opt_vec[conn_start_ind+3] = translations[0] 
            self.opt_vec[conn_start_ind+4] = translations[1]
            self.opt_vec[conn_start_ind+5] = translations[2]
            print(M)

    def addObjToCloud(self, objs_wrt_mbly):
        """ Add the mobileye returns to the 3d scene """
        mbly_vtk_boxes = []

        car_rot = self.imu_transforms_mk1[self.t, :, :3]
        objs_wrt_world = self.xformMblyToGlobal(objs_wrt_mbly)

        # Draw each box
        car_rot = euler_from_matrix(car_rot)[2] * 180 / math.pi
        for o in objs_wrt_world:
            # Create the vtk object
            box = VtkBoundingBox(o)
            actor = box.get_vtk_box(car_rot)
            color = self.mbly_obj_colors[int(o[5])]
            actor.GetProperty().SetColor(*color)
            mbly_vtk_boxes.append(box)

        # Remove old boxes
        for vtk_box in self.mbly_vtk_boxes:
            self.cloud_ren.RemoveActor(vtk_box.actor)
        # Update to new actors
        self.mbly_vtk_boxes = mbly_vtk_boxes
        # Draw new boxes
        for vtk_box in self.mbly_vtk_boxes:
            self.cloud_ren.AddActor(vtk_box.actor)

    def execute(self, iren, event):
        fren = iren.GetRenderWindow().GetRenderers().GetFirstRenderer()

        radar_data = loadRDR(self.rdr_pts[self.count])[0]
        if radar_data.shape[0] > 0:
            #Convert from radar to lidar ref-frame
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], self.radar_params)

            #Convert from lidar to IMU ref-frame
            radar_data[:, :3] = np.dot(self.lidar_params['T_from_l_to_i'][:3, :3], \
                radar_data[:, :3].transpose()).transpose()

            h_radar_data = np.hstack((radar_data[:, :3], np.ones((radar_data.shape[0], 1))))

            radar_data[:, :3] = np.dot(self.imu_transforms[self.count], \
                h_radar_data.transpose()).transpose()[:, :3]
            
            #Insert the Q50 position at the beginning
            me_l = 4.1
            me_w = 2.1
            me_x = self.imu_transforms[self.count, 0, 3]-me_l/2.
            me_y = self.imu_transforms[self.count, 1, 3]
            me_z = self.imu_transforms[self.count, 2, 3]
            me = [me_x, me_y, me_z, me_l, me_w, 0, 0, 0]
            radar_data = np.vstack((np.array(me), radar_data))

            for i in xrange(len(self.radar_actors)):
                fren.RemoveActor(self.radar_actors[i])

            self.radar_actors = []
            self.radar_clouds = []

            for i in xrange(radar_data.shape[0]):
                self.radar_clouds.append(VtkBoundingBox(radar_data[i, :]))
                (ax, ay, az) = euler_from_matrix(self.imu_transforms[self.count])
                box = self.radar_clouds[i].get_vtk_box(rot = az*180/math.pi)
                if i == 0:
                    box.GetProperty().SetColor((1, 0, 0))
                self.radar_actors.append(box)
                fren.AddActor(self.radar_actors[i])

            if self.count == 0:
                self.lidar_cloud = VtkPointCloud(self.lidar_data[:, :3], self.lidar_data[:,3])
                self.lidar_actor = self.lidar_cloud.get_vtk_cloud(zMin=0, zMax=255)
                fren.AddActor(self.lidar_actor)
                self.gpsPointCloud = VtkPointCloud(self.imu_transforms[:, 0:3, 3], 
                                                np.zeros(self.imu_transforms.shape[0]))
                fren.AddActor(self.gpsPointCloud.get_vtk_cloud())

            # if self.count == 0:
                # fren.ResetCamera()
                # fren.GetActiveCamera().Zoom(1.6)
            cam_center = [x for x in self.radar_actors[0].GetCenter()]
            cam_center[2] += 150
            fren.GetActiveCamera().SetPosition(cam_center)
            fren.GetActiveCamera().SetFocalPoint(self.radar_actors[0].GetCenter())

        self.count += 5
        iren.GetRenderWindow().Render()