#!/usr/bin/env python2
import rospy
from nav_msgs.msg import Odometry
import os
import argparse
from tf.transformations import euler_from_quaternion, quaternion_from_euler
pitch = yaw = roll = 0.0
f = open('./vin_camera_pose.csv', 'w')
#f.write('timestamp,x,y,z,pitch,roll,yaw\n')
f.write('timestamp,x,y,z,q1,q2,q3,q0\n')
def extract():
rospy.init_node('get_vins_path', anonymous=True)
rospy.Subscriber('/vins_estimator/camera_pose', Odometry, callback)
rospy.spin()
def callback(data):
orientation_list = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
f.write('%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f\n' %
(
data.header.stamp.to_sec(),
data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z,
data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w
# roll,
# pitch,
# yaw
)
)
if __name__ == '__main__':
extract()
vins-get-pose/camera
猜你喜欢
转载自blog.csdn.net/Darlingqiang/article/details/102393236
今日推荐
周排行