diff --git a/README.md b/README.md index e19b4e0..f38264b 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,9 @@ Homogeneous coordinate transformations are provided as 4x4 `numpy.array` objects Pinhole camera intrinsics for camera `N` are provided as 3x3 `numpy.array` objects and are denoted as `K_camN`. Stereo pair baselines are given in meters as `b_gray` for the monochrome stereo pair (`cam0` and `cam1`), and `b_rgb` for the color stereo pair (`cam2` and `cam3`). ## Example -More detailed examples can be found in the `demos` directory, but the general idea is to specify what dataset you want to load, then access the parts you need and do something with them. +More detailed examples can be found in the `demos` directory, but the general idea is to specify what dataset you want to load, then access the parts you need and do something with them. + +Camera and velodyne data are available via generators for easy sequential access (e.g., for visual odometry), and by indexed getter methods for random access (e.g., for deep learning). Images are loaded as `PIL.Image` objects using Pillow. ```python import pykitti @@ -41,18 +43,22 @@ date = '2011_09_26' drive = '0019' # The 'frames' argument is optional - default: None, which loads the whole dataset. -# Calibration and timestamp data are read automatically. -# Other sensor data (cameras, IMU, Velodyne) are available via properties -# that create generators when accessed. +# Calibration, timestamps, and IMU data are read automatically. +# Camera and velodyne data are available via properties that create generators +# when accessed, or through getter methods that provide random access. data = pykitti.raw(basedir, date, drive, frames=range(0, 50, 5)) -# dataset.calib: Calibration data are accessible as a named tuple -# dataset.timestamps: Timestamps are parsed into a list of datetime objects -# dataset.oxts: Returns a generator that loads OXTS packets as named tuples -# dataset.camN: Returns a generator that loads individual images from camera N -# dataset.gray: Returns a generator that loads monochrome stereo pairs (cam0, cam1) -# dataset.rgb: Returns a generator that loads RGB stereo pairs (cam2, cam3) -# dataset.velo: Returns a generator that loads velodyne scans as [x,y,z,reflectance] +# dataset.calib: Calibration data are accessible as a named tuple +# dataset.timestamps: Timestamps are parsed into a list of datetime objects +# dataset.oxts: List of OXTS packets and 6-dof poses as named tuples +# dataset.camN: Returns a generator that loads individual images from camera N +# dataset.get_camN(idx): Returns the image from camera N at idx +# dataset.gray: Returns a generator that loads monochrome stereo pairs (cam0, cam1) +# dataset.get_gray(idx): Returns the monochrome stereo pair at idx +# dataset.rgb: Returns a generator that loads RGB stereo pairs (cam2, cam3) +# dataset.get_rgb(idx): Returns the RGB stereo pair at idx +# dataset.velo: Returns a generator that loads velodyne scans as [x,y,z,reflectance] +# dataset.get_velo(idx): Returns the velodyne scan at idx point_velo = np.array([0,0,0,1]) point_cam0 = data.calib.T_cam0_velo.dot(point_velo) @@ -61,16 +67,17 @@ point_imu = np.array([0,0,0,1]) point_w = [o.T_w_imu.dot(point_imu) for o in data.oxts] for cam0_image in data.cam0: + # do something pass -rgb_iterator = data.rgb # Assign the generator so it doesn't -cam2_image, cam3_image = next(rgb_iterator) +cam2_image, cam3_image = data.get_rgb(3) ``` ### OpenCV -Image data can be automatically converted to an OpenCV-friendly format (i.e., `uint8` with `BGR` color channel ordering) simply by specifying an additional parameter in the constructor: +PIL Image data can be converted to an OpenCV-friendly format using numpy and `cv2.cvtColor`: ```python -data = pykitti.raw(basedir, date, drive, frames=range(0, 50, 5), imformat='cv2') +img_np = np.array(img) +img_cv2 = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR) ``` Note: This package does not actually require that OpenCV be installed on your system, except to run `demo_raw_cv2.py`. diff --git a/demos/demo_odometry.py b/demos/demo_odometry.py index d809168..75ed5d0 100644 --- a/demos/demo_odometry.py +++ b/demos/demo_odometry.py @@ -13,29 +13,27 @@ basedir = '/Users/leeclement/Desktop/KITTI/odometry/dataset' # Specify the dataset to load -sequence = '01' +sequence = '04' # Load the data. Optionally, specify the frame range to load. -# Passing imformat='cv2' will convert images to uint8 and BGR for -# easy use with OpenCV. # dataset = pykitti.odometry(basedir, sequence) dataset = pykitti.odometry(basedir, sequence, frames=range(0, 20, 5)) # dataset.calib: Calibration data are accessible as a named tuple # dataset.timestamps: Timestamps are parsed into a list of timedelta objects -# dataset.poses: Generator to load ground truth poses T_w_cam0 +# dataset.poses: List of ground truth poses T_w_cam0 # dataset.camN: Generator to load individual images from camera N # dataset.gray: Generator to load monochrome stereo pairs (cam0, cam1) # dataset.rgb: Generator to load RGB stereo pairs (cam2, cam3) # dataset.velo: Generator to load velodyne scans as [x,y,z,reflectance] # Grab some data -second_pose = next(iter(itertools.islice(dataset.poses, 1, None))) +second_pose = dataset.poses[1] first_gray = next(iter(dataset.gray)) first_cam1 = next(iter(dataset.cam1)) -first_rgb = next(iter(dataset.rgb)) -first_cam2 = next(iter(dataset.cam2)) -third_velo = next(iter(itertools.islice(dataset.velo, 2, None))) +first_rgb = dataset.get_rgb(0) +first_cam2 = dataset.get_cam2(0) +third_velo = dataset.get_velo(2) # Display some of the data np.set_printoptions(precision=4, suppress=True) diff --git a/demos/demo_raw.py b/demos/demo_raw.py index ff8fe5b..dffccfd 100644 --- a/demos/demo_raw.py +++ b/demos/demo_raw.py @@ -14,29 +14,31 @@ # Specify the dataset to load date = '2011_09_30' -drive = '0016' +drive = '0034' # Load the data. Optionally, specify the frame range to load. -# Passing imformat='cv2' will convert images to uint8 and BGR for -# easy use with OpenCV. # dataset = pykitti.raw(basedir, date, drive) dataset = pykitti.raw(basedir, date, drive, frames=range(0, 20, 5)) -# dataset.calib: Calibration data are accessible as a named tuple -# dataset.timestamps: Timestamps are parsed into a list of datetime objects -# dataset.oxts: Generator to load OXTS packets as named tuples -# dataset.camN: Generator to load individual images from camera N -# dataset.gray: Generator to load monochrome stereo pairs (cam0, cam1) -# dataset.rgb: Generator to load RGB stereo pairs (cam2, cam3) -# dataset.velo: Generator to load velodyne scans as [x,y,z,reflectance] +# dataset.calib: Calibration data are accessible as a named tuple +# dataset.timestamps: Timestamps are parsed into a list of datetime objects +# dataset.oxts: List of OXTS packets and 6-dof poses as named tuples +# dataset.camN: Returns a generator that loads individual images from camera N +# dataset.get_camN(idx): Returns the image from camera N at idx +# dataset.gray: Returns a generator that loads monochrome stereo pairs (cam0, cam1) +# dataset.get_gray(idx): Returns the monochrome stereo pair at idx +# dataset.rgb: Returns a generator that loads RGB stereo pairs (cam2, cam3) +# dataset.get_rgb(idx): Returns the RGB stereo pair at idx +# dataset.velo: Returns a generator that loads velodyne scans as [x,y,z,reflectance] +# dataset.get_velo(idx): Returns the velodyne scan at idx # Grab some data -second_pose = next(iter(itertools.islice(dataset.oxts, 1, None))).T_w_imu +second_pose = dataset.oxts[1].T_w_imu first_gray = next(iter(dataset.gray)) first_cam1 = next(iter(dataset.cam1)) -first_rgb = next(iter(dataset.rgb)) -first_cam2 = next(iter(dataset.cam2)) -third_velo = next(iter(itertools.islice(dataset.velo, 2, None))) +first_rgb = dataset.get_rgb(0) +first_cam2 = dataset.get_cam2(0) +third_velo = dataset.get_velo(2) # Display some of the data np.set_printoptions(precision=4, suppress=True) diff --git a/demos/demo_raw_cv2.py b/demos/demo_raw_cv2.py index 1c32722..917e650 100644 --- a/demos/demo_raw_cv2.py +++ b/demos/demo_raw_cv2.py @@ -1,5 +1,6 @@ """Example of pykitti.raw usage with OpenCV.""" import cv2 +import numpy as np import matplotlib.pyplot as plt import pykitti @@ -12,32 +13,34 @@ # Specify the dataset to load date = '2011_09_30' -drive = '0016' +drive = '0034' # Load the data. Optionally, specify the frame range to load. -# Passing imformat='cv2' will convert images to uint8 and BGR for -# easy use with OpenCV. dataset = pykitti.raw(basedir, date, drive, - frames=range(0, 20, 5), imformat='cv2') - -# dataset.calib: Calibration data are accessible as a named tuple -# dataset.timestamps: Timestamps are parsed into a list of datetime objects -# dataset.oxts: Generator to load OXTS packets as named tuples -# dataset.camN: Generator to load individual images from camera N -# dataset.gray: Generator to load monochrome stereo pairs (cam0, cam1) -# dataset.rgb: Generator to load RGB stereo pairs (cam2, cam3) -# dataset.velo: Generator to load velodyne scans as [x,y,z,reflectance] + frames=range(0, 20, 5)) + +# dataset.calib: Calibration data are accessible as a named tuple +# dataset.timestamps: Timestamps are parsed into a list of datetime objects +# dataset.oxts: List of OXTS packets and 6-dof poses as named tuples +# dataset.camN: Returns a generator that loads individual images from camera N +# dataset.get_camN(idx): Returns the image from camera N at idx +# dataset.gray: Returns a generator that loads monochrome stereo pairs (cam0, cam1) +# dataset.get_gray(idx): Returns the monochrome stereo pair at idx +# dataset.rgb: Returns a generator that loads RGB stereo pairs (cam2, cam3) +# dataset.get_rgb(idx): Returns the RGB stereo pair at idx +# dataset.velo: Returns a generator that loads velodyne scans as [x,y,z,reflectance] +# dataset.get_velo(idx): Returns the velodyne scan at idx # Grab some data -first_gray = next(iter(dataset.gray)) -first_rgb = next(iter(dataset.rgb)) +first_gray = dataset.get_gray(0) +first_rgb = dataset.get_rgb(0) # Do some stereo processing stereo = cv2.StereoBM_create() -disp_gray = stereo.compute(first_gray[0], first_gray[1]) +disp_gray = stereo.compute(np.array(first_gray[0]), np.array(first_gray[1])) disp_rgb = stereo.compute( - cv2.cvtColor(first_rgb[0], cv2.COLOR_BGR2GRAY), - cv2.cvtColor(first_rgb[1], cv2.COLOR_BGR2GRAY)) + cv2.cvtColor(np.array(first_rgb[0]), cv2.COLOR_RGB2GRAY), + cv2.cvtColor(np.array(first_rgb[1]), cv2.COLOR_RGB2GRAY)) # Display some data f, ax = plt.subplots(2, 2, figsize=(15, 5)) @@ -47,7 +50,7 @@ ax[0, 1].imshow(disp_gray, cmap='viridis') ax[0, 1].set_title('Gray Stereo Disparity') -ax[1, 0].imshow(cv2.cvtColor(first_rgb[0], cv2.COLOR_BGR2RGB)) +ax[1, 0].imshow(first_rgb[0]) ax[1, 0].set_title('Left RGB Image (cam2)') ax[1, 1].imshow(disp_rgb, cmap='viridis') diff --git a/pykitti/odometry.py b/pykitti/odometry.py index a899565..030a623 100644 --- a/pykitti/odometry.py +++ b/pykitti/odometry.py @@ -23,94 +23,56 @@ def __init__(self, base_path, sequence, **kwargs): self.pose_path = os.path.join(base_path, 'poses') self.frames = kwargs.get('frames', None) - # Setting imformat='cv2' will convert the images to uint8 and BGR for - # easy use with OpenCV. - self.imformat = kwargs.get('imformat', None) - # Default image file extension is 'png' self.imtype = kwargs.get('imtype', 'png') + # Find all the data files + self._get_file_lists() + # Pre-load data that isn't returned as a generator self._load_calib() self._load_timestamps() + self._load_poses() def __len__(self): """Return the number of frames loaded.""" return len(self.timestamps) - @property - def poses(self): - """Generator to load ground truth poses (T_w_cam0) from file.""" - pose_file = os.path.join(self.pose_path, self.sequence + '.txt') - - # Read and parse the poses - try: - with open(pose_file, 'r') as f: - lines = f.readlines() - if self.frames is not None: - lines = [lines[i] for i in self.frames] - - for line in lines: - T_w_cam0 = np.fromstring(line, dtype=float, sep=' ') - T_w_cam0 = T_w_cam0.reshape(3, 4) - T_w_cam0 = np.vstack((T_w_cam0, [0, 0, 0, 1])) - yield T_w_cam0 - - except FileNotFoundError: - print('Ground truth poses are not avaialble for sequence ' + - self.sequence + '.') - @property def cam0(self): """Generator to read image files for cam0 (monochrome left).""" - impath = os.path.join(self.sequence_path, 'image_0', - '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam0_files, mode='L') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam0(self, idx): + """Read image file for cam0 (monochrome left) at the specified index.""" + return utils.load_image(self.cam0_files[idx], mode='L') @property def cam1(self): """Generator to read image files for cam1 (monochrome right).""" - impath = os.path.join(self.sequence_path, 'image_1', - '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam1_files, mode='L') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam1(self, idx): + """Read image file for cam1 (monochrome right) at the specified index.""" + return utils.load_image(self.cam1_files[idx], mode='L') @property def cam2(self): """Generator to read image files for cam2 (RGB left).""" - impath = os.path.join(self.sequence_path, 'image_2', - '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam2_files, mode='RGB') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam2(self, idx): + """Read image file for cam2 (RGB left) at the specified index.""" + return utils.load_image(self.cam2_files[idx], mode='RGB') @property def cam3(self): """Generator to read image files for cam0 (RGB right).""" - impath = os.path.join(self.sequence_path, 'image_3', - '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam3_files, mode='RGB') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam3(self, idx): + """Read image file for cam3 (RGB right) at the specified index.""" + return utils.load_image(self.cam3_files[idx], mode='RGB') @property def gray(self): @@ -118,26 +80,61 @@ def gray(self): """ return zip(self.cam0, self.cam1) + def get_gray(self, idx): + """Read monochrome stereo pair at the specified index.""" + return (self.get_cam0(idx), self.get_cam1(idx)) + @property def rgb(self): """Generator to read RGB stereo pairs from file. """ return zip(self.cam2, self.cam3) + def get_rgb(self, idx): + """Read RGB stereo pair at the specified index.""" + return (self.get_cam2(idx), self.get_cam3(idx)) + @property def velo(self): """Generator to read velodyne [x,y,z,reflectance] scan data from binary files.""" - # Find all the Velodyne files - velo_path = os.path.join(self.sequence_path, 'velodyne', '*.bin') - velo_files = sorted(glob.glob(velo_path)) + # Return a generator yielding Velodyne scans. + # Each scan is a Nx4 array of [x,y,z,reflectance] + return utils.yield_velo_scans(self.velo_files) + + def get_velo(self, idx): + """Read velodyne [x,y,z,reflectance] scan at the specified index.""" + return utils.load_velo_scan(self.velo_files[idx]) + + def _get_file_lists(self): + """Find and list data files for each sensor.""" + self.cam0_files = sorted(glob.glob( + os.path.join(self.sequence_path, 'image_0', + '*.{}'.format(self.imtype)))) + self.cam1_files = sorted(glob.glob( + os.path.join(self.sequence_path, 'image_1', + '*.{}'.format(self.imtype)))) + self.cam2_files = sorted(glob.glob( + os.path.join(self.sequence_path, 'image_2', + '*.{}'.format(self.imtype)))) + self.cam3_files = sorted(glob.glob( + os.path.join(self.sequence_path, 'image_3', + '*.{}'.format(self.imtype)))) + self.velo_files = sorted(glob.glob( + os.path.join(self.sequence_path, 'velodyne', + '*.bin'))) # Subselect the chosen range of frames, if any if self.frames is not None: - velo_files = [velo_files[i] for i in self.frames] - - # Return a generator yielding Velodyne scans. - # Each scan is a Nx4 array of [x,y,z,reflectance] - return utils.get_velo_scans(velo_files) + self.cam0_files = utils.subselect_files( + self.cam0_files, self.frames) + self.cam1_files = utils.subselect_files( + self.cam1_files, self.frames) + self.cam2_files = utils.subselect_files( + self.cam2_files, self.frames) + self.cam3_files = utils.subselect_files( + self.cam3_files, self.frames) + self.velo_files = utils.subselect_files( + self.velo_files, self.frames) def _load_calib(self): """Load and compute intrinsic and extrinsic calibration parameters.""" @@ -209,3 +206,27 @@ def _load_timestamps(self): # Subselect the chosen range of frames, if any if self.frames is not None: self.timestamps = [self.timestamps[i] for i in self.frames] + + def _load_poses(self): + """Load ground truth poses (T_w_cam0) from file.""" + pose_file = os.path.join(self.pose_path, self.sequence + '.txt') + + # Read and parse the poses + poses = [] + try: + with open(pose_file, 'r') as f: + lines = f.readlines() + if self.frames is not None: + lines = [lines[i] for i in self.frames] + + for line in lines: + T_w_cam0 = np.fromstring(line, dtype=float, sep=' ') + T_w_cam0 = T_w_cam0.reshape(3, 4) + T_w_cam0 = np.vstack((T_w_cam0, [0, 0, 0, 1])) + poses.append(T_w_cam0) + + except FileNotFoundError: + print('Ground truth poses are not avaialble for sequence ' + + self.sequence + '.') + + self.poses = poses diff --git a/pykitti/raw.py b/pykitti/raw.py index c89d9d4..3a301bd 100644 --- a/pykitti/raw.py +++ b/pykitti/raw.py @@ -23,86 +23,56 @@ def __init__(self, base_path, date, drive, **kwargs): self.data_path = os.path.join(base_path, date, self.drive) self.frames = kwargs.get('frames', None) - # Setting imformat='cv2' will convert the images to uint8 and BGR for - # easy use with OpenCV. - self.imformat = kwargs.get('imformat', None) - # Default image file extension is '.png' self.imtype = kwargs.get('imtype', 'png') + # Find all the data files + self._get_file_lists() + # Pre-load data that isn't returned as a generator self._load_calib() self._load_timestamps() + self._load_oxts() def __len__(self): """Return the number of frames loaded.""" return len(self.timestamps) - @property - def oxts(self): - """Generator to read OXTS data from file.""" - # Find all the data files - oxts_path = os.path.join(self.data_path, 'oxts', 'data', '*.txt') - oxts_files = sorted(glob.glob(oxts_path)) - - # Subselect the chosen range of frames, if any - if self.frames is not None: - oxts_files = [oxts_files[i] for i in self.frames] - - # Return a generator yielding OXTS packets and poses - return utils.get_oxts_packets_and_poses(oxts_files) - @property def cam0(self): """Generator to read image files for cam0 (monochrome left).""" - impath = os.path.join(self.data_path, 'image_00', - 'data', '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam0_files, mode='L') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam0(self, idx): + """Read image file for cam0 (monochrome left) at the specified index.""" + return utils.load_image(self.cam0_files[idx], mode='L') @property def cam1(self): """Generator to read image files for cam1 (monochrome right).""" - impath = os.path.join(self.data_path, 'image_01', - 'data', '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam1_files, mode='L') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam1(self, idx): + """Read image file for cam1 (monochrome right) at the specified index.""" + return utils.load_image(self.cam1_files[idx], mode='L') @property def cam2(self): """Generator to read image files for cam2 (RGB left).""" - impath = os.path.join(self.data_path, 'image_02', - 'data', '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam2_files, mode='RGB') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam2(self, idx): + """Read image file for cam2 (RGB left) at the specified index.""" + return utils.load_image(self.cam2_files[idx], mode='RGB') @property def cam3(self): """Generator to read image files for cam0 (RGB right).""" - impath = os.path.join(self.data_path, 'image_03', - 'data', '*.{}'.format(self.imtype)) - imfiles = sorted(glob.glob(impath)) - # Subselect the chosen range of frames, if any - if self.frames is not None: - imfiles = [imfiles[i] for i in self.frames] + return utils.yield_images(self.cam3_files, mode='RGB') - # Return a generator yielding the images - return utils.get_images(imfiles, self.imformat) + def get_cam3(self, idx): + """Read image file for cam3 (RGB right) at the specified index.""" + return utils.load_image(self.cam3_files[idx], mode='RGB') @property def gray(self): @@ -110,27 +80,65 @@ def gray(self): """ return zip(self.cam0, self.cam1) + def get_gray(self, idx): + """Read monochrome stereo pair at the specified index.""" + return (self.get_cam0(idx), self.get_cam1(idx)) + @property def rgb(self): """Generator to read RGB stereo pairs from file. """ return zip(self.cam2, self.cam3) + def get_rgb(self, idx): + """Read RGB stereo pair at the specified index.""" + return (self.get_cam2(idx), self.get_cam3(idx)) + @property def velo(self): """Generator to read velodyne [x,y,z,reflectance] scan data from binary files.""" - # Find all the Velodyne files - velo_path = os.path.join( - self.data_path, 'velodyne_points', 'data', '*.bin') - velo_files = sorted(glob.glob(velo_path)) + # Return a generator yielding Velodyne scans. + # Each scan is a Nx4 array of [x,y,z,reflectance] + return utils.yield_velo_scans(self.velo_files) + + def get_velo(self, idx): + """Read velodyne [x,y,z,reflectance] scan at the specified index.""" + return utils.load_velo_scan(self.velo_files[idx]) + + def _get_file_lists(self): + """Find and list data files for each sensor.""" + self.oxts_files = sorted(glob.glob( + os.path.join(self.data_path, 'oxts', 'data', '*.txt'))) + self.cam0_files = sorted(glob.glob( + os.path.join(self.data_path, 'image_00', + 'data', '*.{}'.format(self.imtype)))) + self.cam1_files = sorted(glob.glob( + os.path.join(self.data_path, 'image_01', + 'data', '*.{}'.format(self.imtype)))) + self.cam2_files = sorted(glob.glob( + os.path.join(self.data_path, 'image_02', + 'data', '*.{}'.format(self.imtype)))) + self.cam3_files = sorted(glob.glob( + os.path.join(self.data_path, 'image_03', + 'data', '*.{}'.format(self.imtype)))) + self.velo_files = sorted(glob.glob( + os.path.join(self.data_path, 'velodyne_points', + 'data', '*.bin'))) # Subselect the chosen range of frames, if any if self.frames is not None: - velo_files = [velo_files[i] for i in self.frames] - - # Return a generator yielding Velodyne scans. - # Each scan is a Nx4 array of [x,y,z,reflectance] - return utils.get_velo_scans(velo_files) + self.oxts_files = utils.subselect_files( + self.oxts_files, self.frames) + self.cam0_files = utils.subselect_files( + self.cam0_files, self.frames) + self.cam1_files = utils.subselect_files( + self.cam1_files, self.frames) + self.cam2_files = utils.subselect_files( + self.cam2_files, self.frames) + self.cam3_files = utils.subselect_files( + self.cam3_files, self.frames) + self.velo_files = utils.subselect_files( + self.velo_files, self.frames) def _load_calib_rigid(self, filename): """Read a rigid transform calibration file as a numpy.array.""" @@ -252,3 +260,7 @@ def _load_timestamps(self): # Subselect the chosen range of frames, if any if self.frames is not None: self.timestamps = [self.timestamps[i] for i in self.frames] + + def _load_oxts(self): + """Load OXTS data from file.""" + self.oxts = utils.load_oxts_packets_and_poses(self.oxts_files) diff --git a/pykitti/utils.py b/pykitti/utils.py index e9a61ed..23d111b 100644 --- a/pykitti/utils.py +++ b/pykitti/utils.py @@ -2,12 +2,34 @@ from collections import namedtuple -import matplotlib.image as mpimg import numpy as np +from PIL import Image __author__ = "Lee Clement" __email__ = "lee.clement@robotics.utias.utoronto.ca" +# Per dataformat.txt +OxtsPacket = namedtuple('OxtsPacket', + 'lat, lon, alt, ' + + 'roll, pitch, yaw, ' + + 'vn, ve, vf, vl, vu, ' + + 'ax, ay, az, af, al, au, ' + + 'wx, wy, wz, wf, wl, wu, ' + + 'pos_accuracy, vel_accuracy, ' + + 'navstat, numsats, ' + + 'posmode, velmode, orimode') + +# Bundle into an easy-to-access structure +OxtsData = namedtuple('OxtsData', 'packet, T_w_imu') + + +def subselect_files(files, indices): + try: + files = [files[i] for i in indices] + except: + pass + return files + def rotx(t): """Rotation about the x-axis.""" @@ -82,31 +104,19 @@ def pose_from_oxts_packet(packet, scale): return R, t -def get_oxts_packets_and_poses(oxts_files): +def load_oxts_packets_and_poses(oxts_files): """Generator to read OXTS ground truth data. Poses are given in an East-North-Up coordinate system whose origin is the first GPS position. """ - # Per dataformat.txt - OxtsPacket = namedtuple('OxtsPacket', - 'lat, lon, alt, ' + - 'roll, pitch, yaw, ' + - 'vn, ve, vf, vl, vu, ' + - 'ax, ay, az, af, al, au, ' + - 'wx, wy, wz, wf, wl, wu, ' + - 'pos_accuracy, vel_accuracy, ' + - 'navstat, numsats, ' + - 'posmode, velmode, orimode') - - # Bundle into an easy-to-access structure - OxtsData = namedtuple('OxtsData', 'packet, T_w_imu') - # Scale for Mercator projection (from first lat value) scale = None # Origin of the global coordinate system (first GPS position) origin = None + oxts = [] + for filename in oxts_files: with open(filename, 'r') as f: for line in f.readlines(): @@ -127,27 +137,29 @@ def get_oxts_packets_and_poses(oxts_files): T_w_imu = transform_from_rot_trans(R, t - origin) - yield OxtsData(packet, T_w_imu) + oxts.append(OxtsData(packet, T_w_imu)) + + return oxts + + +def load_image(file, mode): + """Load an image from file.""" + return Image.open(file).convert(mode) -def get_images(imfiles, imformat): +def yield_images(imfiles, mode): """Generator to read image files.""" for file in imfiles: - # Convert to uint8 and BGR for OpenCV if requested - if imformat is 'cv2': - im = np.uint8(mpimg.imread(file) * 255) + yield load_image(file, mode) - # Convert RGB to BGR - if len(im.shape) > 2: - im = im[:, :, ::-1] - else: - im = mpimg.imread(file) - yield im +def load_velo_scan(file): + """Load and parse a velodyne binary file.""" + scan = np.fromfile(file, dtype=np.float32) + return scan.reshape((-1, 4)) -def get_velo_scans(velo_files): +def yield_velo_scans(velo_files): """Generator to parse velodyne binary files into arrays.""" - for filename in velo_files: - scan = np.fromfile(filename, dtype=np.float32) - yield scan.reshape((-1, 4)) + for file in velo_files: + yield load_velo_scan(file) diff --git a/setup.py b/setup.py index 7b171d0..58809d9 100644 --- a/setup.py +++ b/setup.py @@ -2,13 +2,13 @@ setup( name='pykitti', - version='0.2.4', + version='0.3.0', description='A minimal set of tools for working with the KITTI dataset in Python', author='Lee Clement', author_email='lee.clement@robotics.utias.utoronto.ca', url='https://github.com/utiasSTARS/pykitti', - download_url='https://github.com/utiasSTARS/pykitti/tarball/0.2.4', + download_url='https://github.com/utiasSTARS/pykitti/tarball/0.3.0', license='MIT', packages=['pykitti'], - install_requires=['numpy', 'matplotlib'] + install_requires=['numpy', 'matplotlib', 'Pillow'] )