def publish(self):
        if self.last_pub and (rospy.Time.now() - self.last_pub) < self.interval:
            return

        (Tpose, Ttf, frames, stamp) = self.get_Tpose()
        frame_id = frames[0]
        if Tpose is None:
            return

        self.last_pub = rospy.Time.now()

        Tpose_p = Tpose[0:3, 3]
        Tpose_q = tft.quaternion_from_matrix(Tpose)

        if self.options.pose:
            pub_msg = PoseStamped()
            pub_msg.header.stamp = stamp
            pub_msg.header.frame_id = frame_id
            pub_msg.pose.position = Point(*(Tpose_p.tolist()))
            pub_msg.pose.orientation = Quaternion(*(Tpose_q.tolist()))

            self.pub.publish(pub_msg)

        if self.options.tf:
            from_frame = frames[1]
            to_frame = frames[2]
            tf_p = Ttf[0:3, 3]
            tf_q = tft.quaternion_from_matrix(Ttf)

            if self.options.tf_always_new:
                stamp = rospy.Time.now()

            self.br.sendTransform(tf_p, tf_q, stamp, to_frame, from_frame)

def quaternion_dist(B_a, B_b):
    quat_a = tf_trans.quaternion_from_matrix(B_a)
    quat_b = tf_trans.quaternion_from_matrix(B_b)
    quat_a_norm = quat_a / np.linalg.norm(quat_a)
    quat_b_norm = quat_b / np.linalg.norm(quat_b)
    dot = np.dot(quat_a_norm, quat_b_norm)
    if dot > 0.99999:
        dist = 0
    else:
        dist = np.arccos(dot)
    return dist

def interpolate_cartesian(start_pose, end_pose, num_steps):
    diff_pos = end_pose[:3,3] - start_pose[:3,3]
    start_quat = tf_trans.quaternion_from_matrix(start_pose)
    end_quat = tf_trans.quaternion_from_matrix(end_pose)
    mat_list = []
    for fraction in np.linspace(0, 1, num_steps):
        cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction)
        cur_mat = np.mat(tf_trans.quaternion_matrix(cur_quat))
        cur_mat[:3,3] = start_pose[:3,3] + fraction * diff_pos
        mat_list.append(cur_mat)
    return mat_list

		def fill_from_Orientation_Data(o):
			'''Fill messages with information from 'Orientation Data' MTData2 block.'''
			self.pub_imu = True
			try:
				x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
			except KeyError:
				pass
			try: 
				# FIXME check that Euler angles are in radians
				x, y, z, w = quaternion_from_euler(radians(o['Roll']),
						radians(o['Pitch']), radians(o['Yaw']))
			except KeyError:
				pass
			try:
				a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
						o['e'], o['f'], o['g'], o['h'], o['i']
				m = identity_matrix()
				m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i))
				x, y, z, w = quaternion_from_matrix(m)
			except KeyError:
				pass
			self.imu_msg.orientation.x = x
			self.imu_msg.orientation.y = y
			self.imu_msg.orientation.z = z
			self.imu_msg.orientation.w = w
			self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
					radians(1.), 0., 0., 0., radians(9.))

def numpy_matrix_to_quaternion(np_matrix):
    '''Given a 3x3 rotation matrix, return its geometry_msgs Quaternion'''
    assert np_matrix.shape == (3, 3), "Must submit 3x3 rotation matrix"
    pose_mat = np.eye(4)
    pose_mat[:3, :3] = np_matrix
    np_quaternion = transformations.quaternion_from_matrix(pose_mat)
    return geometry_msgs.Quaternion(*np_quaternion)

def TransformToPose( G ):
    t = array( G )[0:3,3]
    q = tr.quaternion_from_matrix( G )
    orientation = geometry_msgs.msg.Quaternion( q[0], q[1], q[2], q[3] ) 
    position = geometry_msgs.msg.Point( t[0], t[1], t[2] ) 
    pose = geometry_msgs.msg.Pose( position, orientation )
    return pose

def pack_pose(time, child, parent, matrix=None, trans=None, quat=None):

    if matrix is not None and (trans is None and quat is None):
        trans = tfs.translation_from_matrix(matrix)
        quat = tfs.quaternion_from_matrix(matrix)
    elif matrix is None and trans is not None and quat is not None:
        matrix = None  # nothing
    else:
        print 'invalid use'

    t = TransformStamped()
    t.header.frame_id = parent
    t.header.stamp = time
    t.child_frame_id = child
    t.transform.translation.x = trans[0]
    t.transform.translation.y = trans[1]
    t.transform.translation.z = trans[2]

    quat = numpy.array(quat)
    quat = quat / numpy.linalg.norm(quat, ord=2)
    t.transform.rotation.x = quat[0]
    t.transform.rotation.y = quat[1]
    t.transform.rotation.z = quat[2]
    t.transform.rotation.w = quat[3]

    return t

    def transformPose(self, target_frame, ps):
        """
        :param target_frame: the tf target frame, a string
        :param ps: the geometry_msgs.msg.PoseStamped message
        :return: new geometry_msgs.msg.PoseStamped message, in frame target_frame
        :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise

        Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message.
        """
        # mat44 is frame-to-frame transform as a 4x4
        mat44 = self.asMatrix(target_frame, ps.header)

        # pose44 is the given pose as a 4x4
        pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation))

        # txpose is the new pose in target_frame as a 4x4
        txpose = numpy.dot(mat44, pose44)

        # xyz and quat are txpose's position and orientation
        xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
        quat = tuple(transformations.quaternion_from_matrix(txpose))

        # assemble return value PoseStamped
        r = geometry_msgs.msg.PoseStamped()
        r.header.stamp = ps.header.stamp
        r.header.frame_id = target_frame
        r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))
        return r

    def fromMatrix(m):
        """
        :param m: 4x4 array
        :type m: :func:`numpy.array`
        :return: New PoseMath object

        Return a PoseMath object initialized from 4x4 matrix m

        .. doctest::

            >>> import numpy
            >>> import tf_conversions.posemath as pm
            >>> print PoseMath.fromMatrix(numpy.identity(4))
            position:
              x: 0.0
              y: 0.0
              z: 0.0
            orientation:
              x: 0.0
              y: 0.0
              z: 0.0
              w: 1.0

        """
        (x, y, z) = (m[0, 3], m[1, 3], m[2, 3])
        q = transformations.quaternion_from_matrix(m)
        return PoseMath(Pose(Point(x, y, z), Quaternion(*q)))

    def transformQuaternion(self, target_frame, ps):
        """
        :param target_frame: the tf target frame, a string
        :param ps: the geometry_msgs.msg.QuaternionStamped message
        :return: new geometry_msgs.msg.QuaternionStamped message, in frame target_frame
        :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise

        Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns a new QuaternionStamped message.
        """

        # mat44 is frame-to-frame transform as a 4x4
        mat44 = self.asMatrix(target_frame, ps.header)

        # pose44 is the given quat as a 4x4
        pose44 = xyzw_to_mat44(ps.quaternion)

        # txpose is the new pose in target_frame as a 4x4
        txpose = numpy.dot(mat44, pose44)

        # quat is orientation of txpose
        quat = tuple(transformations.quaternion_from_matrix(txpose))

        # assemble return value QuaternionStamped
        r = geometry_msgs.msg.QuaternionStamped()
        r.header.stamp = ps.header.stamp
        r.header.frame_id = target_frame
        r.quaternion = geometry_msgs.msg.Quaternion(*quat)
        return r

    def calibrate3d(self):
        A = np.matrix(self.A)
        B = np.matrix(self.B)

        ret_R, ret_t = rigid_transform_3D(A, B)
        new_col = ret_t.reshape(3, 1)
        tmp = np.append(ret_R, new_col, axis=1)
        aug=np.array([[0.0,0.0,0.0,1.0]])
        translation = np.squeeze(np.asarray(ret_t))
        T = np.append(tmp,aug,axis=0)
        quaternion = quaternion_from_matrix(T)

        # Send the transform to ROS
        self.tf_broadcaster.sendTransform(ret_t ,quaternion , rospy.Time.now(), self.kinect.link_frame,self.base_frame)

        ## Compute inverse of transformation
        invR = ret_R.T
        invT = -invR * ret_t
        B_in_A = np.empty(B.shape)
        for i in xrange(len(B)):
            p = invR*B[i].T + invT
            B_in_A[i] = p.T

        ## Compute the standard deviation
        err = A-B_in_A
        std = np.std(err,axis=0)
        
        self.static_transform = '<node pkg="tf" type="static_transform_publisher" name="'+self.transform_name+'" args="'\
        +' '.join(map(str, translation))+' '+' '.join(map(str, quaternion))+' '+self.base_frame+' '+self.kinect.link_frame+' 100" />'

        self.depth_pt_pub.publish(self.get_prepared_pointcloud(A,self.kinect.link_frame))
        self.world_pt_pub.publish(self.get_prepared_pointcloud(B,self.base_frame))

def pack_marker(pos, _xaxis):
	marker = Marker()
	marker.header.frame_id = "/map"
	marker.type = marker.ARROW
	marker.action = marker.ADD
	marker.scale.x = 3.0
	marker.scale.y = 0.2
	marker.scale.z = 0.2
	
	# get some likely scale
	marker.color.a = 1.0
	
	# calculate q from xaxis
	xaxis = np.array(_xaxis) / np.linalg.norm(_xaxis)
	yaxis = np.cross(xaxis, [0, 1, 0])
	zaxis = np.cross(xaxis, yaxis)
	R = np.array([list(xaxis), yaxis, zaxis]).T
	T = np.hstack( (R, np.zeros((3,1))) )
	T = np.vstack( (T, np.array([0, 0, 0, 1]).reshape((1,4))) )
	q = transformations.quaternion_from_matrix(T)
	marker.pose.orientation.x = q[0]
	marker.pose.orientation.y = q[1]
	marker.pose.orientation.z = q[2]
	marker.pose.orientation.w = q[3]
	marker.pose.position.x = pos[0]
	marker.pose.position.y = pos[1]
	marker.pose.position.z = pos[2]
	
	return marker

 def interpolate_ep(self, ep_a, ep_b, t_vals):
     pos_a, rot_a = ep_a
     pos_b, rot_b = ep_b
     num_samps = len(t_vals)
     pos_waypoints = np.array(pos_a) + np.array(np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals)
     pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
     rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
     rot_homo_a[:3,:3] = rot_a
     rot_homo_b[:3,:3] = rot_b
     quat_a = tf_trans.quaternion_from_matrix(rot_homo_a)
     quat_b = tf_trans.quaternion_from_matrix(rot_homo_b)
     rot_waypoints = []
     for t in t_vals:
         cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t)
         rot_waypoints.append(np.mat(tf_trans.quaternion_matrix(cur_quat))[:3,:3])
     return zip(pos_waypoints, rot_waypoints)

    def publish_state(self, t):
        state_msg = TransformStamped()
        t_ned_imu = tft.translation_matrix(self.model.get_position())
        R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
        T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
        #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
        if self.T_imu_vicon is None:
            # grab the static transform from imu to vicon frame from param server:
            frames = rospy.get_param("frames", None)
            ypr = frames['vicon_to_imu']['rotation']
            q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
            R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
            t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
#            rospy.loginfo(str(R_vicon_imu))
#            rospy.loginfo(str(t_vicon_imu))
            self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
            self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
            self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
            rospy.loginfo(str(self.T_enu_ned))
            rospy.loginfo(str(self.T_imu_vicon))
            #rospy.loginfo(str(T_vicon_imu))
        # we have /ned -> /imu, need to output /enu -> /vicon:
        T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
        state_msg.header.stamp  = t
        state_msg.header.frame_id = "/enu"
        state_msg.child_frame_id = "/simflyer1/flyer_vicon"
        stt = state_msg.transform.translation
        stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
        stro = state_msg.transform.rotation
        stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
        
        self.pub_state.publish(state_msg)

 def _makePreGraspPose(self, boxMat, axis):
     if axis==0: #x axis
         alpha = 0
         gamma = 0
     else: #y axis
         alpha = pi/2
         gamma = -pi/2
     ik_request = PositionIKRequest()
     ik_request.ik_link_name = self.toolLinkName
     with self._joint_state_lock:
         ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states)
     ik_request.pose_stamped = PoseStamped()
     ik_request.pose_stamped.header.stamp = rospy.Time.now()
     ik_request.pose_stamped.header.frame_id = self.frameID
     beta = pi/2
     while beta < 3*pi/2:
         rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz')
         distance = self.preGraspDistance + self.gripperFingerLength
         preGraspMat = transformations.translation_matrix([0,0,-distance])
         fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat)
         p = transformations.translation_from_matrix(fullMat)
         q = transformations.quaternion_from_matrix(fullMat)
         print "trying {} radians".format(beta)
         try:
             self._ik_server(ik_request, Constraints(), rospy.Duration(5.0))
         except rospy.service.ServiceException:
             beta += 0.1
         else:
             if ik_resp.error_code.val > 0:
                 return beta
             else:
                 #print ik_resp.error_code.val
                 beta += 0.01
     rospy.logerr('No way to pick this up. All IK solutions failed.')
     return 7 * pi / 6

    def set_orientation(self, orientation):
        orientation = np.array(orientation)
        if orientation.shape == (4, 4):
            # We're getting a homogeneous rotation matrix - not a quaternion
            orientation = transformations.quaternion_from_matrix(orientation)

        return PoseEditor2(self.nav, [self.position, orientation])

def computeTransformation(world, slam, camera):
    # Here we do not care about the slamMworld transformation
    # timing as it is constant.
    tWorld = listener.getLatestCommonTime(world, camera)
    tMap = listener.getLatestCommonTime(slam, camera)
    t = min(tWorld, tMap)

    # Current pose given by the SLAM.
    (slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t)
    slamMcam = np.matrix(transformer.fromTranslationRotation(slamMcam_T,
                                                             slamMcam_Q))

    # Estimation of the camera position given by control.
    #FIXME: order is weird but it works.
    (worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t)
    worldMcam = np.matrix(transformer.fromTranslationRotation(worldMcam_T,
                                                              worldMcam_Q))

    (slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t)
    slamMworld = np.matrix(transformer.fromTranslationRotation(slamMworld_T,
                                                               slamMworld_Q))
    slamMcamEstimated = slamMworld * worldMcam

    # Inverse frames.
    camMslam = np.linalg.inv(slamMcam)
    camMslamEstimated = np.linalg.inv(slamMcamEstimated)

    # Split.
    camMslam_T = translation_from_matrix(camMslam)
    camMslam_Q = quaternion_from_matrix(camMslam)
    camMslam_E = euler_from_matrix(camMslam)

    camMslamEstimated_T = translation_from_matrix(camMslamEstimated)
    camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated)
    camMslamEstimated_E = euler_from_matrix(camMslamEstimated)

    # Compute correction.
    camMslamCorrected_T = [camMslam_T[0],
                           camMslamEstimated_T[1],
                           camMslam_T[2]]
    camMslamCorrected_E = [camMslamEstimated_E[0],
                           camMslam_E[1],
                           camMslamEstimated_E[2]]

    camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E)

    return (camMslamCorrected_T, camMslamCorrected_Q, t)

def flipOrbToNDT (orbPose):
    qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw]
    orbFlip = trafo.concatenate_matrices(
        trafo.quaternion_matrix(qOrb),
        trafo.rotation_matrix(np.pi/2, (1,0,0)),
        trafo.rotation_matrix(np.pi/2, (0,0,1))
    )
    return trafo.quaternion_from_matrix(orbFlip)

 def adjustRobotPose(self, pose):
     Tr2_w = self.poseToTransform(pose)
     Tadj = np.dot(np.dot(self.Tv_w, self.Tw_r1),Tr2_w)
     pose.position.x, pose.position.y, pose.position.z = Tadj[0:3,3]
     q = tr.quaternion_from_matrix(Tadj)
     for i,attr in enumerate(['x', 'y', 'z', 'w']):
         pose.orientation.__setattr__(attr, q[i])
     return pose

 def _unpack_recv_data(self, recv_data):
     name = recv_data[0]
     q = quaternion_from_matrix(self._matrix_from_list(recv_data[1:]))
     return name, Pose(position=Vector3(x=float(recv_data[13])*self.mm2m,
                                        y=float(recv_data[14])*self.mm2m,
                                        z=float(recv_data[15])*self.mm2m),
                       orientation=Quaternion(x=q[0], y=q[1],
                                              z=q[2], w=q[3]))