support using images at a specific desired frequency

aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras

@@ -23,10 +23,10 @@ import signal
 
 np.set_printoptions(suppress=True)
 
-def initBagDataset(bagfile, topic, from_to):
+def initBagDataset(bagfile, topic, from_to, freq):
     print("\tDataset:          {0}".format(bagfile))
     print("\tTopic:            {0}".format(topic))
-    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to)
+    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to, bag_freq=freq)
     print("\tNumber of images: {0}".format(reader.numImages()))
     return reader
 
@@ -85,7 +85,8 @@ def parseArgs():
     groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data')
     groupSource.add_argument('--topics', nargs='+', dest='topics', help='The list of image topics', required=True)
     groupSource.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s]')
-
+    groupSource.add_argument('--bag-freq', metavar='bag_freq', type=float, help='Frequency to extract features at [hz]')
+
     groupTarget = parser.add_argument_group('Calibration target configuration')
     groupTarget.add_argument('--target', dest='targetYaml', help='Calibration target configuration as yaml file', required=True)
 
@@ -166,7 +167,7 @@ def main():
 
         if modelName in cameraModels:
             #open dataset 
-            dataset = initBagDataset(parsed.bagfile, topic, parsed.bag_from_to)
+            dataset = initBagDataset(parsed.bagfile, topic, parsed.bag_from_to, parsed.bag_freq)
 
             #create camera
             cameraModel = cameraModels[modelName]

aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera

@@ -69,6 +69,7 @@ def parseArgs():
     groupData = parser.add_argument_group('Dataset source')
     groupData.add_argument('--bag', dest='bagfile', nargs=1, help='Ros bag file containing image and imu data (rostopics specified in the yamls)', action=Once, required=True)
     groupData.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s]')
+    groupData.add_argument('--bag-freq', metavar='bag_freq', type=float, help='Frequency to extract features at [hz]')
     groupData.add_argument('--perform-synchronization',  action='store_true', dest='perform_synchronization', \
                           help='Perform a clock synchronization according to \'Clock synchronization algorithms for network measurements\' by Zhang et al. (2002).')
 

aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras

@@ -23,10 +23,10 @@ import signal
 
 np.set_printoptions(suppress=True)
 
-def __initBagDataset(bagfile, topic, from_to):
+def __initBagDataset(bagfile, topic, from_to, freq):
     print("\tDataset:          {0}".format(bagfile))
     print("\tTopic:            {0}".format(topic))
-    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to)
+    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to, bag_freq=freq)
     print("\tNumber of images: {0}".format(reader.numImages()))
     return reader
 
@@ -90,6 +90,7 @@ def __parseArgs():
     groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data.')
     groupSource.add_argument('--topic', dest='topic', help='The image topic.', required=True)
     groupSource.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s].')
+    groupSource.add_argument('--bag-freq', metavar='bag_freq', type=float, help='Frequency to extract features at [hz]')
 
     groupTarget = parser.add_argument_group('Calibration target configuration')
     groupTarget.add_argument('--target', dest='targetYaml', help='Calibration target configuration as yaml file.', required=True)
@@ -135,7 +136,7 @@ def main():
     ######
     ## load bagfile and extract targets:
     targetConfig = kc.CalibrationTargetParameters(parsed.targetYaml)
-    dataset = __initBagDataset(parsed.bagfile, parsed.topic, parsed.bag_from_to)
+    dataset = __initBagDataset(parsed.bagfile, parsed.topic, parsed.bag_from_to, parsed.bag_freq)
 
     #create camera
     cameraModel = cameraModels[parsed.model]

aslam_offline_calibration/kalibr/python/kalibr_common/ImageDatasetReader.py

@@ -31,7 +31,7 @@ def __next__(self):
 
 
 class BagImageDatasetReader(object):
-  def __init__(self, bagfile, imagetopic, bag_from_to=None, perform_synchronization=False):
+  def __init__(self, bagfile, imagetopic, bag_from_to=None, perform_synchronization=False, bag_freq=None):
     self.bagfile = bagfile
     self.topic = imagetopic
     self.perform_synchronization = perform_synchronization
@@ -60,6 +60,10 @@ def __init__(self, bagfile, imagetopic, bag_from_to=None, perform_synchronizatio
     if bag_from_to:
       self.indices = self.truncateIndicesFromTime(self.indices, bag_from_to)
 
+    # go through and remove indices not at the correct frequency
+    if bag_freq:
+      self.indices = self.truncateIndicesFromFreq(self.indices, bag_freq)
+
   # sort the ros messegaes by the header time not message time
   def sortByTime(self, indices):
     self.timestamp_corrector = sm.DoubleTimestampCorrector()
@@ -102,7 +106,30 @@ def truncateIndicesFromTime(self, indices, bag_from_to):
       if timestamp >= (bagstart + bag_from_to[0]) and timestamp <= (bagstart + bag_from_to[1]):
         valid_indices.append(idx)
     sm.logWarn(
-        "BagImageDatasetReader: truncated {0} / {1} images.".format(len(indices) - len(valid_indices), len(indices)))
+        "BagImageDatasetReader: truncated {0} / {1} images (from-to).".format(len(indices) - len(valid_indices), len(indices)))
+    return valid_indices
+
+  def truncateIndicesFromFreq(self, indices, freq):
+
+    # some value checking
+    if freq < 0.0:
+      raise RuntimeError("Frequency {0} Hz is smaller 0".format(freq))
+
+    # find the valid timestamps
+    timestamp_last = -1
+    valid_indices = []
+    for idx in self.indices:
+      topic, data, stamp = self.bag._read_message(self.index[idx].position)
+      timestamp = data.header.stamp.secs + data.header.stamp.nsecs / 1.0e9
+      if timestamp_last < 0.0:
+        timestamp_last = timestamp
+        valid_indices.append(idx)
+        continue
+      if (timestamp - timestamp_last) >= 1.0 / freq:
+        timestamp_last = timestamp
+        valid_indices.append(idx)
+    sm.logWarn(
+      "BagImageDatasetReader: truncated {0} / {1} images (frequency)".format(len(indices) - len(valid_indices), len(indices)))
     return valid_indices
 
   def __iter__(self):

aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py

@@ -18,11 +18,11 @@
 import scipy.optimize
 
 
-def initCameraBagDataset(bagfile, topic, from_to=None, perform_synchronization=False):
+def initCameraBagDataset(bagfile, topic, from_to, freq, perform_synchronization):
     print("Initializing camera rosbag dataset reader:")
     print("\tDataset:          {0}".format(bagfile))
     print("\tTopic:            {0}".format(topic))
-    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to, \
+    reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to, bag_freq=freq, \
                                       perform_synchronization=perform_synchronization)
     print("\tNumber of images: {0}".format(len(reader.index)))
     return reader
@@ -420,7 +420,7 @@ def __init__(self, chainConfig, targetConfig, parsed):
         for camNr in range(0, chainConfig.numCameras()):
             camConfig = chainConfig.getCameraParameters(camNr)
             dataset = initCameraBagDataset(parsed.bagfile[0], camConfig.getRosTopic(), \
-                                           parsed.bag_from_to, parsed.perform_synchronization)
+                                           parsed.bag_from_to, parsed.bag_freq, parsed.perform_synchronization)
 
             #create the camera
             self.camList.append( IccCamera( camConfig,