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()