from scipy.spatial import distance
from scipy.optimize import linear_sum_assignment
from SlamUtils.transformation import pos_quats2SEs, pos_quats2SE_matrices, pose2motion, SEs2ses, line2mat, tartan2kitti
from SlamUtils.visualization import getVisualizationBB, getKeyframe
from SlamUtils.Loader.TartanAir import getRootDir, getDataSequences, getDataLists, tartan_camExtr
rootDIR = getRootDir()
path = getDataSequences(root=rootDIR,
scenario='office',
level='Easy',
seq_num=4)
# path = getDataSequences(root=rootDIR, scenario='seasidetown', level='Easy', seq_num=0)
# path = getDataSequences(root=rootDIR, scenario='neighborhood', level='Easy', seq_num=0)
files_rgb_left, files_rgb_right, files_depth_left, poselist = getDataLists(
dir=path, skip=5)
poses_mat44 = pos_quats2SE_matrices(poselist)
config_cam = {
'width': 640,
'height': 480,
'fx': 320,
'fy': 320,
'cx': 320,
'cy': 240
}
camIntr = camera.PinholeCameraIntrinsic(**config_cam) #open3d if need
camExtr = tartan_camExtr
K = Cal3_S2(320, 320, 0.0, 320, 240)
def convertBags(_pathTartan, _pathBag, _bagName='tmp.bag'):
import rospy, rosbag
from rospy import Time
from sensor_msgs.msg import Imu, Image, CompressedImage
from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion
from nav_msgs.msg import Odometry
from cv_bridge import CvBridge, CvBridgeError
files_rgb_left, files_rgb_right, files_depth_left, poselist = getDataLists(
dir=_pathTartan, skip=1)
imulist_t = np.loadtxt(_pathBag + 'imu.txt')
poselist_t = np.loadtxt(_pathBag + 'pose_gt.txt')
# if have offset
data_ids = np.loadtxt(_pathBag + 'data_id.txt').astype(np.int32).tolist()
files_rgb_left = [files_rgb_left[i] for i in data_ids]
files_rgb_right = [files_rgb_right[i] for i in data_ids]
# bag_name = 'office_' + 'easy_' + '004' + '.bag'
rospy.init_node('writer', anonymous=True)
bridge = CvBridge()
bag = rosbag.Bag(_pathBag + _bagName, 'w')
st = datetime.datetime(year=2022,
month=2,
day=22,
hour=22,
minute=22,
second=22)
for i in range(imulist_t.shape[0]):
# cTime = (st + datetime.timedelta(milliseconds=(i * delta_imu))).timestamp()
imu = imulist_t[i]
cTime = rospy.Time.from_seconds(imu[0])
m_Imu = Imu()
m_Imu.header.seq = i
m_Imu.header.stamp = cTime
m_Imu.angular_velocity.x = imu[1]
m_Imu.angular_velocity.y = imu[2]
m_Imu.angular_velocity.z = imu[3]
m_Imu.linear_acceleration.x = imu[4]
m_Imu.linear_acceleration.y = imu[5]
m_Imu.linear_acceleration.z = imu[6]
bag.write(topic='/imu0', msg=m_Imu, t=cTime)
for i in range(poselist_t.shape[0]):
pose = poselist_t[i]
print('img ', i, ' ...')
m_Pose = PoseStamped()
cTime = rospy.Time.from_seconds(pose[0])
m_Pose.header.seq = i
m_Pose.header.stamp = cTime
m_Pose.pose.position.x = pose[1]
m_Pose.pose.position.y = pose[2]
m_Pose.pose.position.z = pose[3]
m_Pose.pose.orientation.x = pose[4]
m_Pose.pose.orientation.y = pose[5]
m_Pose.pose.orientation.z = pose[6]
m_Pose.pose.orientation.w = pose[7]
bag.write(topic='/groundtruth/pose', msg=m_Pose, t=cTime)
imgL = cv.imread(files_rgb_left[i])
imgR = cv.imread(files_rgb_right[i])
msg_L = bridge.cv2_to_imgmsg(cvim=imgL, encoding="bgr8")
msg_L.header.seq = i
msg_L.header.stamp = cTime
msg_R = bridge.cv2_to_imgmsg(cvim=imgR, encoding="bgr8")
msg_R.header.seq = i
msg_R.header.stamp = cTime
bag.write(topic='/cam0/image_raw', msg=msg_L, t=cTime)
bag.write(topic='/cam1/image_raw', msg=msg_R, t=cTime)
bag.close()
