python ros多话题对齐 .ApproximateTimeSynchronizer使用 四元素转旋转矩阵

本代码主要是对ORB_SLAM2 提取的关键帧Tcw pose 图片/深度图进行对其然后用python 进行读取和四元数转换

在这里插入代码片
#! /usr/bin/env python
import cv_bridge
import rospy
from std_msgs.msg import String
import cv2
import sys
from sensor_msgs.msg import Image
from cv_bridge import  CvBridge ,CvBridgeError
from message_filters import TimeSynchronizer ,Subscriber
import message_filters
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
import numpy as np

txtfile='/home/bat2/catkin_ws/src/pythonslam/image/pose/'
timefile='/home/bat2/catkin_ws/src/pythonslam/image/time/'
'''手写的四元数转换(原因: 因为一些转换库转化失败)
'''
def quaternion2rotation(quat,t,H):
    # quat:(w,x,y,z) t(x,y,z)
    assert (len(quat) == 4)
    # normalize first
    quat = quat / np.linalg.norm(quat)
    a, b, c, d = quat
    px,py,pz=t
    a2 = a * a
    b2 = b * b
    c2 = c * c
    d2 = d * d
    ab = a * b
    ac = a * c
    ad = a * d
    bc = b * c
    bd = b * d
    cd = c * d

    # s = a2 + b2 + c2 + d2

    m0 = a2 + b2 - c2 - d2
    m1 = 2 * (bc - ad)
    m2 = 2 * (bd + ac)
    m3 = 2 * (bc + ad)
    m4 = a2 - b2 + c2 - d2
    m5 = 2 * (cd - ab)
    m6 = 2 * (bd - ac)
    m7 = 2 * (cd + ab)
    m8 = a2 - b2 - c2 + d2
    n1= np.array([m0, m1, m2, px, m3, m4, m5, py, m6, m7, m8, pz]).reshape(1, 12)  #  R T   3x4 ->12x1

    i=str(H)
    print(type(i))
    txt=txtfile+str(H)+'.txt'
    time=timefile+str(H)+'.txt'  ##以时间戳的方式为名进行存取

    print(time)

    np.savetxt(txt,n1) ###因为找了好久【平时主要用c++ python  不熟悉 主要是因为项目需求】都没找到存在一个txt方式 然后就按文件夹存了 ，希望有大佬能高知
    fo=open(time,'w')
    #fo<