Mercator scale needs to be the same when doing (lat,lon,alt)->(x,y,z)

README.md

@@ -37,8 +37,8 @@ basedir = '/your/dataset/dir'
 date = '2011_09_26'
 drive = '0019'
 
-# The range argument is optional - default is None, which loads the whole dataset.
-# Calibration and timestamp data are loaded automatically. 
+# 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 generators.
 data = pykitti.raw(basedir, date, drive, frames=range(0, 50, 5))
 
@@ -56,7 +56,8 @@ point_cam0 = data.calib.T_cam0_velo.dot(point_velo)
 point_imu = np.array([0,0,0,1])
 point_w = [o.T_w_imu.dot(point_imu) for o in data.oxts]
 
-cam2_image = next(data.rgb).left
+cam0_image = next(data.cam0)
+cam2_image, cam3_image = next(data.rgb)
 ```
 ### 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:
@@ -69,3 +70,4 @@ Note: This package does not actually require that OpenCV be installed on your sy
 
 ## References
 [1] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, "Vision meets robotics: The KITTI dataset," Int. J. Robot. Research (IJRR), vol. 32, no. 11, pp. 1231–1237, Sep. 2013. [http://www.cvlibs.net/datasets/kitti/](http://www.cvlibs.net/datasets/kitti/)
+`

pykitti/odometry.py

@@ -59,7 +59,7 @@ def cam0(self):
         impath = os.path.join(self.sequence_path, 'image_0', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -71,7 +71,7 @@ def cam1(self):
         impath = os.path.join(self.sequence_path, 'image_1', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -83,7 +83,7 @@ def cam2(self):
         impath = os.path.join(self.sequence_path, 'image_2', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -95,7 +95,7 @@ def cam3(self):
         impath = os.path.join(self.sequence_path, 'image_3', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -121,7 +121,7 @@ def velo(self):
         velo_files = sorted(glob.glob(velo_path))
 
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             velo_files = [velo_files[i] for i in self.frames]
 
         # Return a generator yielding Velodyne scans.
@@ -191,5 +191,5 @@ def _load_timestamps(self):
                 self.timestamps.append(t)
 
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             self.timestamps = [self.timestamps[i] for i in self.frames]

pykitti/raw.py

@@ -43,7 +43,7 @@ def oxts(self):
         oxts_files = sorted(glob.glob(oxts_path))
 
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             oxts_files = [oxts_files[i] for i in self.frames]
 
         # Return a generator yielding OXTS packets and poses
@@ -55,7 +55,7 @@ def cam0(self):
         impath = os.path.join(self.data_path, 'image_00', 'data', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -67,7 +67,7 @@ def cam1(self):
         impath = os.path.join(self.data_path, 'image_01', 'data', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -79,7 +79,7 @@ def cam2(self):
         impath = os.path.join(self.data_path, 'image_02', 'data', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -91,7 +91,7 @@ def cam3(self):
         impath = os.path.join(self.data_path, 'image_03', 'data', '*.png')
         imfiles = sorted(glob.glob(impath))
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             imfiles = [imfiles[i] for i in self.frames]
 
         # Return a generator yielding the images
@@ -118,7 +118,7 @@ def velo(self):
         velo_files = sorted(glob.glob(velo_path))
 
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             velo_files = [velo_files[i] for i in self.frames]
 
         # Return a generator yielding Velodyne scans.
@@ -226,5 +226,5 @@ def _load_timestamps(self):
                 self.timestamps.append(t)
 
         # Subselect the chosen range of frames, if any
-        if self.frames:
+        if self.frames is not None:
             self.timestamps = [self.timestamps[i] for i in self.frames]

pykitti/utils.py

@@ -60,16 +60,11 @@ def read_calib_file(filepath):
     return data
 
 
-def pose_from_oxts_packet(packet):
-    """Helper method to compute a SE(3) pose matrix from an OXTS packet."""
+def pose_from_oxts_packet(packet, scale):
+    """Helper method to compute a SE(3) pose matrix from an OXTS packet.
+    """
     er = 6378137.  # earth radius (approx.) in meters
 
-    # compute scale from lat value
-    scale = np.cos(packet.lat * np.pi / 180.)
-
-    t_0 = []    # initial position
-    poses = []  # list of poses computed from oxts
-
     # Use a Mercator projection to get the translation vector
     tx = scale * packet.lon * np.pi * er / 180.
     ty = scale * er * \
@@ -84,11 +79,15 @@ def pose_from_oxts_packet(packet):
     R = Rz.dot(Ry.dot(Rx))
 
     # Combine the translation and rotation into a homogeneous transform
-    return transform_from_rot_trans(R, t)
+    return R, t
 
 
 def get_oxts_packets_and_poses(oxts_files):
-    """Generator to read OXTS ground truth data."""
+    """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, ' +
@@ -103,6 +102,11 @@ def get_oxts_packets_and_poses(oxts_files):
     # 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
+
     for filename in oxts_files:
         with open(filename, 'r') as f:
             for line in f.readlines():
@@ -112,7 +116,16 @@ def get_oxts_packets_and_poses(oxts_files):
                 line[-5:] = [int(float(x)) for x in line[-5:]]
 
                 packet = OxtsPacket(*line)
-                T_w_imu = pose_from_oxts_packet(packet)
+
+                if scale is None:
+                    scale = np.cos(packet.lat * np.pi / 180.)
+
+                R, t = pose_from_oxts_packet(packet, scale)
+
+                if origin is None:
+                    origin = t
+
+                T_w_imu = transform_from_rot_trans(R, t - origin)
 
                 yield OxtsData(packet, T_w_imu)
 

setup.py

@@ -2,12 +2,12 @@
 
 setup(
     name='pykitti',
-    version='0.1.2',
+    version='0.2.0',
     description='A minimal set of tools for working with the KITTI dataset in Python',
     author='Lee Clement',
     author_email='[email protected]',
     url='https://github.com/utiasSTARS/pykitti',
-    download_url='https://github.com/utiasSTARS/pykitti/tarball/0.1.2',
+    download_url='https://github.com/utiasSTARS/pykitti/tarball/0.2.0',
     license='MIT',
     packages=['pykitti'],
     install_requires=['numpy', 'matplotlib']