├── .gitignore ├── .travis.yml ├── Dockerfile ├── LICENSE ├── README.md ├── docker_entrypoint.sh ├── kitti2bag ├── __init__.py ├── __main__.py └── kitti2bag.py ├── setup.cfg ├── setup.py └── upload.sh /.gitignore: -------------------------------------------------------------------------------- 1 | README 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | env/ 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .coverage 43 | .coverage.* 44 | .cache 45 | nosetests.xml 46 | coverage.xml 47 | *,cover 48 | .hypothesis/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | 58 | # Flask stuff: 59 | instance/ 60 | .webassets-cache 61 | 62 | # Scrapy stuff: 63 | .scrapy 64 | 65 | # Sphinx documentation 66 | docs/_build/ 67 | 68 | # PyBuilder 69 | target/ 70 | 71 | # IPython Notebook 72 | .ipynb_checkpoints 73 | 74 | # pyenv 75 | .python-version 76 | 77 | # celery beat schedule file 78 | celerybeat-schedule 79 | 80 | # dotenv 81 | .env 82 | 83 | # virtualenv 84 | venv/ 85 | ENV/ 86 | 87 | # Spyder project settings 88 | .spyderproject 89 | 90 | # Rope project settings 91 | .ropeproject 92 | 93 | # Pycharm 94 | *.idea 95 | 96 | # Temporary files from text editors 97 | *~ 98 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Generic .travis.yml file for running continuous integration on Travis-CI for 2 | # any ROS package. 3 | # 4 | # Available here: 5 | # - http://felixduvallet.github.io/ros-travis-integration 6 | # - https://github.com/felixduvallet/ros-travis-integration 7 | # 8 | # This installs ROS on a clean Travis-CI virtual machine, creates a ROS 9 | # workspace, resolves all listed dependencies, and sets environment variables 10 | # (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are 11 | # no compilation errors), and runs all the tests. If any of the compilation/test 12 | # phases fail, the build is marked as a failure. 13 | # 14 | # We handle two types of package dependencies specified in the package manifest: 15 | # - system dependencies that can be installed using `rosdep`, including other 16 | # ROS packages and system libraries. These dependencies must be known to 17 | # `rosdistro` and get installed using apt-get. 18 | # - package dependencies that must be checked out from source. These are handled by 19 | # `wstool`, and should be listed in a file named dependencies.rosinstall. 20 | # 21 | # There are two variables you may want to change: 22 | # - ROS_DISTRO (default is indigo). Note that packages must be available for 23 | # ubuntu 14.04 trusty. 24 | # - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo 25 | # root). This should list all necessary repositories in wstool format (see 26 | # the ros wiki). If the file does not exists then nothing happens. 27 | # 28 | # See the README.md for more information. 29 | # 30 | # Author: Felix Duvallet 31 | 32 | # NOTE: The build lifecycle on Travis.ci is something like this: 33 | # before_install 34 | # install 35 | # before_script 36 | # script 37 | # after_success or after_failure 38 | # after_script 39 | # OPTIONAL before_deploy 40 | # OPTIONAL deploy 41 | # OPTIONAL after_deploy 42 | 43 | ################################################################################ 44 | 45 | # Use ubuntu trusty (14.04) with sudo privileges. 46 | dist: 47 | - trusty 48 | sudo: required 49 | language: 50 | - generic 51 | cache: 52 | - apt 53 | 54 | # Configuration variables. All variables are global now, but this can be used to 55 | # trigger a build matrix for different ROS distributions if desired. 56 | env: 57 | global: 58 | - ROS_DISTRO=indigo 59 | - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] 60 | - CI_SOURCE_PATH=$(pwd) 61 | - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall 62 | - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options 63 | - ROS_PARALLEL_JOBS='-j8 -l6' 64 | 65 | ################################################################################ 66 | 67 | # Install system dependencies, namely a very barebones ROS setup. 68 | before_install: 69 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 70 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 71 | - sudo apt-get update -qq 72 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin 73 | - source /opt/ros/$ROS_DISTRO/setup.bash 74 | # Prepare rosdep to install dependencies. 75 | - sudo rosdep init 76 | - rosdep update 77 | 78 | # Create a catkin workspace with the package under integration. 79 | install: 80 | - mkdir -p ~/catkin_ws/src 81 | - cd ~/catkin_ws/src 82 | - catkin_init_workspace 83 | # Create the devel/setup.bash (run catkin_make with an empty workspace) and 84 | # source it to set the path variables. 85 | - cd ~/catkin_ws 86 | - catkin_make 87 | - source devel/setup.bash 88 | # Add the package under integration to the workspace using a symlink. 89 | - cd ~/catkin_ws/src 90 | - ln -s $CI_SOURCE_PATH . 91 | - sudo apt-get install -y python-dev python-numpy python-matplotlib ros-$ROS_DISTRO-tf ros-$ROS_DISTRO-opencv-candidate 92 | 93 | # Install all dependencies, using wstool first and rosdep second. 94 | # wstool looks for a ROSINSTALL_FILE defined in the environment variables. 95 | before_script: 96 | # source dependencies: install using wstool. 97 | - cd ~/catkin_ws/src 98 | - wstool init 99 | - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi 100 | - wstool up 101 | # package depdencies: install using rosdep. 102 | - cd ~/catkin_ws 103 | - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 104 | 105 | # Compile and test (mark the build as failed if any step fails). If the 106 | # CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example 107 | # to blacklist certain packages. 108 | # 109 | # NOTE on testing: `catkin_make run_tests` will show the output of the tests 110 | # (gtest, nosetest, etc..) but always returns 0 (success) even if a test 111 | # fails. Running `catkin_test_results` aggregates all the results and returns 112 | # non-zero when a test fails (which notifies Travis the build failed). 113 | script: 114 | - source /opt/ros/$ROS_DISTRO/setup.bash 115 | - cd ~/catkin_ws 116 | - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) 117 | # Run the tests, ensuring the path is set correctly. 118 | - source devel/setup.bash 119 | - catkin_make run_tests && catkin_test_results 120 | - cd $CI_SOURCE_PATH 121 | - sudo python setup.py install 122 | - mkdir -p $HOME/kitti 123 | - cd $HOME/kitti 124 | - wget http://kitti.is.tue.mpg.de/kitti/raw_data/2011_09_26_drive_0048/2011_09_26_drive_0048_sync.zip 125 | - unzip 2011_09_26_drive_0048_sync.zip 126 | - wget http://kitti.is.tue.mpg.de/kitti/raw_data/2011_09_26_calib.zip 127 | - unzip 2011_09_26_calib.zip 128 | - kitti2bag -t 2011_09_26 -r 0048 raw_synced . 129 | 130 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:lunar-ros-base 2 | 3 | RUN apt-get update \ 4 | && DEBIAN_FRONTEND=noninteractive apt-get -y install \ 5 | ros-lunar-cv-bridge \ 6 | ros-lunar-opencv3 \ 7 | ros-lunar-tf \ 8 | python-pip python-matplotlib \ 9 | && rm -rf /var/lib/apt/lists/* 10 | COPY . /kitti2bag 11 | RUN pip install -e /kitti2bag 12 | 13 | WORKDIR /data 14 | 15 | ENTRYPOINT ["/kitti2bag/docker_entrypoint.sh"] 16 | 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 Tomas Krejci 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kitti2bag 2 | 3 | *Hello everybody! I'm looking for more people that can bring this package to the next level. If you'd like to help you can contact me via the email. I'll be happy for every news contribution!* 4 | 5 | [![Gitter](https://badges.gitter.im/kitti2bag/community.svg)](https://gitter.im/kitti2bag/community?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge) [![Build Status](https://travis-ci.org/tomas789/kitti2bag.svg?branch=master)](https://travis-ci.org/tomas789/kitti2bag) 6 | 7 | Convert [KITTI](http://www.cvlibs.net/datasets/kitti/index.php) dataset to ROS bag file the easy way! 8 | 9 | ![KITTI playback preview](https://raw.githubusercontent.com/tomas789/kitti2bag/gh-pages/img/kitti_playback.png) 10 | 11 | ## Collaboration 12 | 13 | This package enjoyed significant interest from more people then I could have thought at the beginning. I'm really glad to see that. I see many PRs and issues being raised but my day job does not allow me to push this repository further. In order to allow this package to prosper, I'm opening it up for the community. I'm more than happy to add you as a collaborator to this repository. Just send me an email. 14 | 15 | And by the way. To ensure we maintain the quality of the repo you are required to get the PR approval from at least one other collaborator. You can use the [Gitter](https://gitter.im/kitti2bag/community?utm_source=share-link&utm_medium=link&utm_campaign=share-link) to communicate with others. 16 | 17 | ## TODOs 18 | 19 | Help me make this feature rich and complete. Just fork this repo, implement new features (very easy in this case) and make [pull request](https://github.com/tomas789/kitti2bag/pulls). 20 | 21 | Feature request list: 22 | * make [URDF](http://wiki.ros.org/urdf) of a car so transformations between frames are easily done by ROS itself. 23 | * deal with tracklets 24 | * support for unsynced+unrectified version 25 | * provide documentation via [ROS wiki](wiki.ros.org) 26 | * provide simple GUI 27 | * distribute publically available bagfiles (is there a reliable public storage for this purpose?) 28 | * export only subset of sensors 29 | 30 | ## Contributions 31 | 32 | Thanks to the work of @jnitsch, _kitti2bag_ can now export velodyne laser data and dynamic _tf_ transformations. Thanks to @emreay-, this tool can now convert odometry datasets too. Thank you both! 33 | 34 | ## How to install it? 35 | 36 | It is very easy! On the machine with ROS installed, just run 37 | ```bash 38 | pip install kitti2bag 39 | ``` 40 | 41 | ## How to run it? 42 | 43 | One example is better then thousand words so here it is 44 | 45 | ```bash 46 | $ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0002/2011_09_26_drive_0002_sync.zip 47 | $ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip 48 | $ unzip 2011_09_26_drive_0002_sync.zip 49 | $ unzip 2011_09_26_calib.zip 50 | $ kitti2bag -t 2011_09_26 -r 0002 raw_synced . 51 | Exporting static transformations 52 | Exporting time dependent transformations 53 | Exporting IMU 54 | Exporting camera 0 55 | 100% (77 of 77) |##########################| Elapsed Time: 0:00:00 Time: 0:00:00 56 | Exporting camera 1 57 | 100% (77 of 77) |##########################| Elapsed Time: 0:00:00 Time: 0:00:00 58 | Exporting camera 2 59 | 100% (77 of 77) |##########################| Elapsed Time: 0:00:01 Time: 0:00:01 60 | Exporting camera 3 61 | 100% (77 of 77) |##########################| Elapsed Time: 0:00:01 Time: 0:00:01 62 | Exporting velodyne data 63 | 100% (77 of 77) |##########################| Elapsed Time: 0:00:15 Time: 0:00:15 64 | ## OVERVIEW ## 65 | path: kitti_2011_09_26_drive_0002_synced.bag 66 | version: 2.0 67 | duration: 7.8s 68 | start: Sep 26 2011 13:02:44.33 (1317042164.33) 69 | end: Sep 26 2011 13:02:52.16 (1317042172.16) 70 | size: 417.2 MB 71 | messages: 1078 72 | compression: none [308/308 chunks] 73 | types: geometry_msgs/TwistStamped [98d34b0043a2093cf9d9345ab6eef12e] 74 | sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214] 75 | sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743] 76 | sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2] 77 | sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48] 78 | sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181] 79 | tf2_msgs/TFMessage [94810edda583a504dfda3829e70d7eec] 80 | topics: /kitti/camera_color_left/camera_info 77 msgs : sensor_msgs/CameraInfo 81 | /kitti/camera_color_left/image_raw 77 msgs : sensor_msgs/Image 82 | /kitti/camera_color_right/camera_info 77 msgs : sensor_msgs/CameraInfo 83 | /kitti/camera_color_right/image_raw 77 msgs : sensor_msgs/Image 84 | /kitti/camera_gray_left/camera_info 77 msgs : sensor_msgs/CameraInfo 85 | /kitti/camera_gray_left/image_raw 77 msgs : sensor_msgs/Image 86 | /kitti/camera_gray_right/camera_info 77 msgs : sensor_msgs/CameraInfo 87 | /kitti/camera_gray_right/image_raw 77 msgs : sensor_msgs/Image 88 | /kitti/oxts/gps/fix 77 msgs : sensor_msgs/NavSatFix 89 | /kitti/oxts/gps/vel 77 msgs : geometry_msgs/TwistStamped 90 | /kitti/oxts/imu 77 msgs : sensor_msgs/Imu 91 | /kitti/velo/pointcloud 77 msgs : sensor_msgs/PointCloud2 92 | /tf 77 msgs : tf2_msgs/TFMessage 93 | /tf_static 77 msgs : tf2_msgs/TFMessage 94 | ``` 95 | 96 | 97 | That's it. You have file `kitti_2011_09_26_drive_0002_sync.bag` that contains your data. 98 | 99 | Other source files can be found at [KITTI raw data](http://www.cvlibs.net/datasets/kitti/raw_data.php) page. 100 | 101 | If you got an error saying something like _command not found_ it means that your python installation is in bad shape. You might try running 102 | ```python -m kitti2bag -t 2011_09_26 -r 0002 raw_synced .``` 103 | Or maybe use Docker. 104 | 105 | ### Prefer Docker? 106 | 107 | That is easy too. There is a pre-built image `tomas789/kitti2bag`. 108 | 109 | ```bash 110 | $ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0002/2011_09_26_drive_0002_sync.zip 111 | $ wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip 112 | $ unzip 2011_09_26_drive_0002_sync.zip 113 | $ unzip 2011_09_26_calib.zip 114 | $ docker run -v `pwd`:/data -it tomas789/kitti2bag -t 2011_09_26 -r 0002 raw_synced 115 | Exporting static transformations 116 | Exporting time dependent transformations 117 | ... 118 | ``` 119 | 120 | This might also be a better alternative if you are having troubles installing the package. 121 | 122 | ## Bug reporting, support and feature requests. 123 | 124 | I appreciate [pull requests](https://github.com/tomas789/kitti2bag/pulls) with bug fixes and new features. You you want to help with something please use [GitHub issue tracker](https://github.com/tomas789/kitti2bag/issues). 125 | 126 | ## Related works 127 | 128 | * [pykitti](https://github.com/utiasSTARS/pykitti) is very simple library for dealing with KITTI dataset in python. 129 | * [kitti_player](https://github.com/tomas789/kitti_player) allows to play dataset directly. No bag file needed. I found difficult to get it work. Some bug fixed can be found in [my fork of kitti_player](https://github.com/tomas789/kitti_player) but still not good enough. 130 | -------------------------------------------------------------------------------- /docker_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros environment 5 | source "/opt/ros/$ROS_DISTRO/setup.bash" 6 | exec kitti2bag "$@" 7 | -------------------------------------------------------------------------------- /kitti2bag/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomas789/kitti2bag/bf0d46c49a77f5d5500621934ccd617d18cf776b/kitti2bag/__init__.py -------------------------------------------------------------------------------- /kitti2bag/__main__.py: -------------------------------------------------------------------------------- 1 | from .kitti2bag import run_kitti2bag 2 | 3 | 4 | def main(): 5 | run_kitti2bag() 6 | 7 | 8 | if __name__ == '__main__': 9 | main() 10 | 11 | -------------------------------------------------------------------------------- /kitti2bag/kitti2bag.py: -------------------------------------------------------------------------------- 1 | #!env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import sys 5 | 6 | try: 7 | import pykitti 8 | except ImportError as e: 9 | print('Could not load module \'pykitti\'. Please run `pip install pykitti`') 10 | sys.exit(1) 11 | 12 | import tf 13 | import os 14 | import cv2 15 | import rospy 16 | import rosbag 17 | import progressbar 18 | from tf2_msgs.msg import TFMessage 19 | from datetime import datetime 20 | from std_msgs.msg import Header 21 | from sensor_msgs.msg import CameraInfo, Imu, PointField, NavSatFix 22 | import sensor_msgs.point_cloud2 as pcl2 23 | from geometry_msgs.msg import TransformStamped, TwistStamped, Transform 24 | from cv_bridge import CvBridge 25 | import numpy as np 26 | import argparse 27 | 28 | def save_imu_data(bag, kitti, imu_frame_id, topic): 29 | print("Exporting IMU") 30 | for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): 31 | q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw) 32 | imu = Imu() 33 | imu.header.frame_id = imu_frame_id 34 | imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) 35 | imu.orientation.x = q[0] 36 | imu.orientation.y = q[1] 37 | imu.orientation.z = q[2] 38 | imu.orientation.w = q[3] 39 | imu.linear_acceleration.x = oxts.packet.af 40 | imu.linear_acceleration.y = oxts.packet.al 41 | imu.linear_acceleration.z = oxts.packet.au 42 | imu.angular_velocity.x = oxts.packet.wf 43 | imu.angular_velocity.y = oxts.packet.wl 44 | imu.angular_velocity.z = oxts.packet.wu 45 | bag.write(topic, imu, t=imu.header.stamp) 46 | 47 | 48 | def save_dynamic_tf(bag, kitti, kitti_type, initial_time): 49 | print("Exporting time dependent transformations") 50 | if kitti_type.find("raw") != -1: 51 | for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): 52 | tf_oxts_msg = TFMessage() 53 | tf_oxts_transform = TransformStamped() 54 | tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) 55 | tf_oxts_transform.header.frame_id = 'world' 56 | tf_oxts_transform.child_frame_id = 'base_link' 57 | 58 | transform = (oxts.T_w_imu) 59 | t = transform[0:3, 3] 60 | q = tf.transformations.quaternion_from_matrix(transform) 61 | oxts_tf = Transform() 62 | 63 | oxts_tf.translation.x = t[0] 64 | oxts_tf.translation.y = t[1] 65 | oxts_tf.translation.z = t[2] 66 | 67 | oxts_tf.rotation.x = q[0] 68 | oxts_tf.rotation.y = q[1] 69 | oxts_tf.rotation.z = q[2] 70 | oxts_tf.rotation.w = q[3] 71 | 72 | tf_oxts_transform.transform = oxts_tf 73 | tf_oxts_msg.transforms.append(tf_oxts_transform) 74 | 75 | bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp) 76 | 77 | elif kitti_type.find("odom") != -1: 78 | timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) 79 | for timestamp, tf_matrix in zip(timestamps, kitti.T_w_cam0): 80 | tf_msg = TFMessage() 81 | tf_stamped = TransformStamped() 82 | tf_stamped.header.stamp = rospy.Time.from_sec(timestamp) 83 | tf_stamped.header.frame_id = 'world' 84 | tf_stamped.child_frame_id = 'camera_left' 85 | 86 | t = tf_matrix[0:3, 3] 87 | q = tf.transformations.quaternion_from_matrix(tf_matrix) 88 | transform = Transform() 89 | 90 | transform.translation.x = t[0] 91 | transform.translation.y = t[1] 92 | transform.translation.z = t[2] 93 | 94 | transform.rotation.x = q[0] 95 | transform.rotation.y = q[1] 96 | transform.rotation.z = q[2] 97 | transform.rotation.w = q[3] 98 | 99 | tf_stamped.transform = transform 100 | tf_msg.transforms.append(tf_stamped) 101 | 102 | bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp) 103 | 104 | 105 | def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time): 106 | print("Exporting camera {}".format(camera)) 107 | if kitti_type.find("raw") != -1: 108 | camera_pad = '{0:02d}'.format(camera) 109 | image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad)) 110 | image_path = os.path.join(image_dir, 'data') 111 | image_filenames = sorted(os.listdir(image_path)) 112 | with open(os.path.join(image_dir, 'timestamps.txt')) as f: 113 | image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines()) 114 | 115 | calib = CameraInfo() 116 | calib.header.frame_id = camera_frame_id 117 | calib.width, calib.height = tuple(util['S_rect_{}'.format(camera_pad)].tolist()) 118 | calib.distortion_model = 'plumb_bob' 119 | calib.K = util['K_{}'.format(camera_pad)] 120 | calib.R = util['R_rect_{}'.format(camera_pad)] 121 | calib.D = util['D_{}'.format(camera_pad)] 122 | calib.P = util['P_rect_{}'.format(camera_pad)] 123 | 124 | elif kitti_type.find("odom") != -1: 125 | camera_pad = '{0:01d}'.format(camera) 126 | image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad)) 127 | image_filenames = sorted(os.listdir(image_path)) 128 | image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) 129 | 130 | calib = CameraInfo() 131 | calib.header.frame_id = camera_frame_id 132 | calib.P = util['P{}'.format(camera_pad)] 133 | 134 | iterable = zip(image_datetimes, image_filenames) 135 | bar = progressbar.ProgressBar() 136 | for dt, filename in bar(iterable): 137 | image_filename = os.path.join(image_path, filename) 138 | cv_image = cv2.imread(image_filename) 139 | calib.height, calib.width = cv_image.shape[:2] 140 | if camera in (0, 1): 141 | cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 142 | encoding = "mono8" if camera in (0, 1) else "bgr8" 143 | image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) 144 | image_message.header.frame_id = camera_frame_id 145 | if kitti_type.find("raw") != -1: 146 | image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f"))) 147 | topic_ext = "/image_raw" 148 | elif kitti_type.find("odom") != -1: 149 | image_message.header.stamp = rospy.Time.from_sec(dt) 150 | topic_ext = "/image_rect" 151 | calib.header.stamp = image_message.header.stamp 152 | bag.write(topic + topic_ext, image_message, t = image_message.header.stamp) 153 | bag.write(topic + '/camera_info', calib, t = calib.header.stamp) 154 | 155 | def save_velo_data(bag, kitti, velo_frame_id, topic): 156 | print("Exporting velodyne data") 157 | velo_path = os.path.join(kitti.data_path, 'velodyne_points') 158 | velo_data_dir = os.path.join(velo_path, 'data') 159 | velo_filenames = sorted(os.listdir(velo_data_dir)) 160 | with open(os.path.join(velo_path, 'timestamps.txt')) as f: 161 | lines = f.readlines() 162 | velo_datetimes = [] 163 | for line in lines: 164 | if len(line) == 1: 165 | continue 166 | dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f') 167 | velo_datetimes.append(dt) 168 | 169 | iterable = zip(velo_datetimes, velo_filenames) 170 | bar = progressbar.ProgressBar() 171 | for dt, filename in bar(iterable): 172 | if dt is None: 173 | continue 174 | 175 | velo_filename = os.path.join(velo_data_dir, filename) 176 | 177 | # read binary data 178 | scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4) 179 | 180 | # create header 181 | header = Header() 182 | header.frame_id = velo_frame_id 183 | header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f"))) 184 | 185 | # fill pcl msg 186 | fields = [PointField('x', 0, PointField.FLOAT32, 1), 187 | PointField('y', 4, PointField.FLOAT32, 1), 188 | PointField('z', 8, PointField.FLOAT32, 1), 189 | PointField('i', 12, PointField.FLOAT32, 1)] 190 | pcl_msg = pcl2.create_cloud(header, fields, scan) 191 | 192 | bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp) 193 | 194 | 195 | def get_static_transform(from_frame_id, to_frame_id, transform): 196 | t = transform[0:3, 3] 197 | q = tf.transformations.quaternion_from_matrix(transform) 198 | tf_msg = TransformStamped() 199 | tf_msg.header.frame_id = from_frame_id 200 | tf_msg.child_frame_id = to_frame_id 201 | tf_msg.transform.translation.x = float(t[0]) 202 | tf_msg.transform.translation.y = float(t[1]) 203 | tf_msg.transform.translation.z = float(t[2]) 204 | tf_msg.transform.rotation.x = float(q[0]) 205 | tf_msg.transform.rotation.y = float(q[1]) 206 | tf_msg.transform.rotation.z = float(q[2]) 207 | tf_msg.transform.rotation.w = float(q[3]) 208 | return tf_msg 209 | 210 | 211 | def inv(transform): 212 | "Invert rigid body transformation matrix" 213 | R = transform[0:3, 0:3] 214 | t = transform[0:3, 3] 215 | t_inv = -1 * R.T.dot(t) 216 | transform_inv = np.eye(4) 217 | transform_inv[0:3, 0:3] = R.T 218 | transform_inv[0:3, 3] = t_inv 219 | return transform_inv 220 | 221 | 222 | def save_static_transforms(bag, transforms, timestamps): 223 | print("Exporting static transformations") 224 | tfm = TFMessage() 225 | for transform in transforms: 226 | t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2]) 227 | tfm.transforms.append(t) 228 | for timestamp in timestamps: 229 | time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) 230 | for i in range(len(tfm.transforms)): 231 | tfm.transforms[i].header.stamp = time 232 | bag.write('/tf_static', tfm, t=time) 233 | 234 | 235 | def save_gps_fix_data(bag, kitti, gps_frame_id, topic): 236 | for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): 237 | navsatfix_msg = NavSatFix() 238 | navsatfix_msg.header.frame_id = gps_frame_id 239 | navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) 240 | navsatfix_msg.latitude = oxts.packet.lat 241 | navsatfix_msg.longitude = oxts.packet.lon 242 | navsatfix_msg.altitude = oxts.packet.alt 243 | navsatfix_msg.status.service = 1 244 | bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp) 245 | 246 | 247 | def save_gps_vel_data(bag, kitti, gps_frame_id, topic): 248 | for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): 249 | twist_msg = TwistStamped() 250 | twist_msg.header.frame_id = gps_frame_id 251 | twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) 252 | twist_msg.twist.linear.x = oxts.packet.vf 253 | twist_msg.twist.linear.y = oxts.packet.vl 254 | twist_msg.twist.linear.z = oxts.packet.vu 255 | twist_msg.twist.angular.x = oxts.packet.wf 256 | twist_msg.twist.angular.y = oxts.packet.wl 257 | twist_msg.twist.angular.z = oxts.packet.wu 258 | bag.write(topic, twist_msg, t=twist_msg.header.stamp) 259 | 260 | 261 | def run_kitti2bag(): 262 | parser = argparse.ArgumentParser(description = "Convert KITTI dataset to ROS bag file the easy way!") 263 | # Accepted argument values 264 | kitti_types = ["raw_synced", "odom_color", "odom_gray"] 265 | odometry_sequences = [] 266 | for s in range(22): 267 | odometry_sequences.append(str(s).zfill(2)) 268 | 269 | parser.add_argument("kitti_type", choices = kitti_types, help = "KITTI dataset type") 270 | parser.add_argument("dir", nargs = "?", default = os.getcwd(), help = "base directory of the dataset, if no directory passed the deafult is current working directory") 271 | parser.add_argument("-t", "--date", help = "date of the raw dataset (i.e. 2011_09_26), option is only for RAW datasets.") 272 | parser.add_argument("-r", "--drive", help = "drive number of the raw dataset (i.e. 0001), option is only for RAW datasets.") 273 | parser.add_argument("-s", "--sequence", choices = odometry_sequences,help = "sequence of the odometry dataset (between 00 - 21), option is only for ODOMETRY datasets.") 274 | args = parser.parse_args() 275 | 276 | bridge = CvBridge() 277 | compression = rosbag.Compression.NONE 278 | # compression = rosbag.Compression.BZ2 279 | # compression = rosbag.Compression.LZ4 280 | 281 | # CAMERAS 282 | cameras = [ 283 | (0, 'camera_gray_left', '/kitti/camera_gray_left'), 284 | (1, 'camera_gray_right', '/kitti/camera_gray_right'), 285 | (2, 'camera_color_left', '/kitti/camera_color_left'), 286 | (3, 'camera_color_right', '/kitti/camera_color_right') 287 | ] 288 | 289 | if args.kitti_type.find("raw") != -1: 290 | 291 | if args.date == None: 292 | print("Date option is not given. It is mandatory for raw dataset.") 293 | print("Usage for raw dataset: kitti2bag raw_synced [dir] -t -r ") 294 | sys.exit(1) 295 | elif args.drive == None: 296 | print("Drive option is not given. It is mandatory for raw dataset.") 297 | print("Usage for raw dataset: kitti2bag raw_synced [dir] -t -r ") 298 | sys.exit(1) 299 | 300 | bag = rosbag.Bag("kitti_{}_drive_{}_{}.bag".format(args.date, args.drive, args.kitti_type[4:]), 'w', compression=compression) 301 | kitti = pykitti.raw(args.dir, args.date, args.drive) 302 | if not os.path.exists(kitti.data_path): 303 | print('Path {} does not exists. Exiting.'.format(kitti.data_path)) 304 | sys.exit(1) 305 | 306 | if len(kitti.timestamps) == 0: 307 | print('Dataset is empty? Exiting.') 308 | sys.exit(1) 309 | 310 | try: 311 | # IMU 312 | imu_frame_id = 'imu_link' 313 | imu_topic = '/kitti/oxts/imu' 314 | gps_fix_topic = '/kitti/oxts/gps/fix' 315 | gps_vel_topic = '/kitti/oxts/gps/vel' 316 | velo_frame_id = 'velo_link' 317 | velo_topic = '/kitti/velo' 318 | 319 | T_base_link_to_imu = np.eye(4, 4) 320 | T_base_link_to_imu[0:3, 3] = [-2.71/2.0-0.05, 0.32, 0.93] 321 | 322 | # tf_static 323 | transforms = [ 324 | ('base_link', imu_frame_id, T_base_link_to_imu), 325 | (imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)), 326 | (imu_frame_id, cameras[0][1], inv(kitti.calib.T_cam0_imu)), 327 | (imu_frame_id, cameras[1][1], inv(kitti.calib.T_cam1_imu)), 328 | (imu_frame_id, cameras[2][1], inv(kitti.calib.T_cam2_imu)), 329 | (imu_frame_id, cameras[3][1], inv(kitti.calib.T_cam3_imu)) 330 | ] 331 | 332 | util = pykitti.utils.read_calib_file(os.path.join(kitti.calib_path, 'calib_cam_to_cam.txt')) 333 | 334 | # Export 335 | save_static_transforms(bag, transforms, kitti.timestamps) 336 | save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=None) 337 | save_imu_data(bag, kitti, imu_frame_id, imu_topic) 338 | save_gps_fix_data(bag, kitti, imu_frame_id, gps_fix_topic) 339 | save_gps_vel_data(bag, kitti, imu_frame_id, gps_vel_topic) 340 | for camera in cameras: 341 | save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=None) 342 | save_velo_data(bag, kitti, velo_frame_id, velo_topic) 343 | 344 | finally: 345 | print("## OVERVIEW ##") 346 | print(bag) 347 | bag.close() 348 | 349 | elif args.kitti_type.find("odom") != -1: 350 | 351 | if args.sequence == None: 352 | print("Sequence option is not given. It is mandatory for odometry dataset.") 353 | print("Usage for odometry dataset: kitti2bag {odom_color, odom_gray} [dir] -s ") 354 | sys.exit(1) 355 | 356 | bag = rosbag.Bag("kitti_data_odometry_{}_sequence_{}.bag".format(args.kitti_type[5:],args.sequence), 'w', compression=compression) 357 | 358 | kitti = pykitti.odometry(args.dir, args.sequence) 359 | if not os.path.exists(kitti.sequence_path): 360 | print('Path {} does not exists. Exiting.'.format(kitti.sequence_path)) 361 | sys.exit(1) 362 | 363 | kitti.load_calib() 364 | kitti.load_timestamps() 365 | 366 | if len(kitti.timestamps) == 0: 367 | print('Dataset is empty? Exiting.') 368 | sys.exit(1) 369 | 370 | if args.sequence in odometry_sequences[:11]: 371 | print("Odometry dataset sequence {} has ground truth information (poses).".format(args.sequence)) 372 | kitti.load_poses() 373 | 374 | try: 375 | util = pykitti.utils.read_calib_file(os.path.join(args.dir,'sequences',args.sequence, 'calib.txt')) 376 | current_epoch = (datetime.utcnow() - datetime(1970, 1, 1)).total_seconds() 377 | # Export 378 | if args.kitti_type.find("gray") != -1: 379 | used_cameras = cameras[:2] 380 | elif args.kitti_type.find("color") != -1: 381 | used_cameras = cameras[-2:] 382 | 383 | save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=current_epoch) 384 | for camera in used_cameras: 385 | save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=current_epoch) 386 | 387 | finally: 388 | print("## OVERVIEW ##") 389 | print(bag) 390 | bag.close() 391 | 392 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file = README.md 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | 5 | setup( 6 | name='kitti2bag', 7 | version='1.5', 8 | description='Convert KITTI dataset to ROS bag file the easy way!', 9 | author='Tomas Krejci', 10 | author_email='tomas@krej.ci', 11 | url='https://github.com/tomas789/kitti2bag/', 12 | download_url = 'https://github.com/tomas789/kitti2bag/archive/1.5.zip', 13 | keywords = ['dataset', 'ros', 'rosbag', 'kitti'], 14 | entry_points = { 15 | 'console_scripts': ['kitti2bag=kitti2bag.__main__:main'], 16 | }, 17 | install_requires=['pykitti', 'progressbar2'] 18 | ) 19 | -------------------------------------------------------------------------------- /upload.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | pandoc --from=markdown --to=rst --output=README README.md 3 | python setup.py sdist upload 4 | --------------------------------------------------------------------------------