process_bags.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 Simple Script to Processes Bag files from RawSnapshot in Release 2.0
5 and writes the data to file for calibration
6 
7 Please direct any question to multisense@carnegierobotics.com or
8  http://support.carnegierobotics.com
9 '''
10 
11 import sys
12 import csv
13 import time
14 import os
15 import numpy as np
16 import string
17 import getopt
18 try:
19  import rospy
20  import rosbag
21 except:
22  raise Exception("Error importing ROS. Source the ROS environment" \
23  +" in the workspace where the multisense stack is located")
24 
25 class _BagProcessor():
26 
27  def __init__(self, bag_file):
28  self.bag_file = bag_file
29 
30  # Transforms to move points in the laser frame to the left
31  # camera optical frame
32  self.laserToSpindle = None
33  self.spindleToCamera = None
34 
35  # The matrix used to reproject disparity point into 3D. We will use this
36  # to perform the opposite operation and transform laser points into
37  # the camera frame
38  self.qMatrix = None
39  self.qMatrixInverse = None
40 
41  # The disparity image read from the bag file
42  self.disparityImage = None
43  self.disparityHeight = 0
44  self.disparityWidth = 0
45 
46  # This is the error in terms of pixels between the disparity value
47  # reported by the camera and the theoretical disparity value
48  # that the camera would need to have to perfectly match the laser data
49  self.errorImage = None
50 
51  # A debug image which is the laser data as a disparity image
52  self.laserDisparity = None
53  #end def
54 
55  #Wrapper method to processes the bag file
56  #Returns bag file name
57  def process(self, directory='.', rename_file = True,
58  namespace = 'multisense'):
59 
60  # Save out the stereo and camera images for debugging. Also store
61  # the disparity image to be used when computing the laser error
62  # metric
63  if not self.process_image(directory, namespace):
64  print "Could not find valid images in the bag file"
65  return None
66  #end if
67 
68  # Get the calibration for our camera
69  self.process_camera_yaml(directory, namespace)
70 
71  # Get the transforms to convert the laser data into the left
72  # camera optical frame, the same coordinate frame which the disparity image
73  # in in
74  self.process_laser_yaml(directory, namespace)
75 
76  # Process our laser data and convert each scan into disparity space
77  self.process_laser(directory, namespace)
78 
79  # Compute a error metric based on our processed stereo and laser
80  # data
81  average_error = self.compute_average_error()
82 
83  print "Average Pixel Error:", average_error, "pixels"
84 
85  if average_error < 2.:
86  print "Calibration is good"
87  else:
88  print "Calibration appears to be poor. Place the unit 1 meter from"\
89  " a texture-rich corner an rerun the check."
90  #end if
91 
92 
93  # Save a image which displays where the largest error are in our
94  # disparity image
95  self.compute_error_image(average_error, directory)
96 
97  return_value = self.bag_file
98  if rename_file:
99  return_value = self.rename_bag(directory)
100  # end if
101 
102  return return_value
103  #end def
104 
105  #Method to extract laser data from bag file and save into .csv
106  def process_laser(self, directory='.', namespace='multisense'):
107  bag = rosbag.Bag(self.bag_file)
108  with open(directory + '/' + 'lidarData.csv', 'wb') as laser_file:
109  topic_name = '/%s/calibration/raw_lidar_data' % namespace
110  laser_writer = csv.writer(laser_file, delimiter=',')
111  for topic, msg, t in bag.read_messages(topics=[topic_name]):
112  #Unbundle message
113  scan_count = msg.scan_count
114  time_start = float(msg.time_start.secs +
115  msg.time_start.nsecs * 1e-9)
116  time_end = float(msg.time_end.secs + msg.time_end.nsecs * 1e-9)
117  angle_start = msg.angle_start
118  angle_end = msg.angle_end
119  dist = msg.distance
120  reflect = msg.intensity
121 
122  #Expected Format: scan_count, time_start, time_end, angle_start,
123  #angle_end, , range, , intensity, ,len(range)
124  row = [scan_count, time_start, time_end,
125  angle_start, angle_end, " "] + list(dist) + [" "] \
126  + list(reflect) + [" "] + [len(list(dist))]
127 
128  laser_writer.writerow(row)
129 
130  # Transform our laser data into the camera frame
131  self.compute_laser_error(msg)
132 
133  #end for
134 
135  laser_file.close()
136 
137  #end with
138  #end def
139 
140 
141  #Method to extract first instance of disparity and recitified images from
142  #bag
143  def process_image(self, directory='.', namespace='multisense'):
144  topic_name = '/%s/calibration/raw_cam_data' % namespace
145  bag = rosbag.Bag(self.bag_file)
146 
147  #Attempt to find both disparity and rectified images before quitting
148  found_rectified_image = False
149  found_disparity_image = False
150 
151  for topic, msg, t in bag.read_messages(topics=[topic_name]):
152  width = msg.width
153  height = msg.height
154 
155  #Write to .pgm file stereo_left_0000.pgm
156  if not found_rectified_image and len(list(msg.gray_scale_image)) != 0:
157  self.write_pgm(np.array(list(msg.gray_scale_image)),
158  directory + '/' + "stereo_left_0000.pgm",
159  width, height, 8)
160  found_rectified_image = True
161  #end if
162 
163  if not found_disparity_image and len(list(msg.disparity_image)) != 0:
164  self.disparityImage = np.array(list(msg.disparity_image), dtype=np.uint16)
165  self.errorImage = np.zeros(len(list(msg.disparity_image)), dtype=np.float64) - 1
166  self.laserDisparity = np.zeros(len(list(msg.disparity_image)), dtype=np.uint16)
167  self.disparityHeight = height
168  self.disparityWidth = width
169  self.write_pgm(self.disparityImage,
170  directory + '/' + "disparity_0000.pgm",
171  width, height, 16)
172  found_disparity_image = True
173  #end if
174 
175 
176  #Quit once disparity and rectified images have been found
177  if found_disparity_image and found_rectified_image:
178  return True
179  #end if
180  #end for
181 
182  return False
183  #end def
184 
185  #Method to write an image to a .pgm file
186  def write_pgm(self, data, name, width, height, bits):
187  image = open(name, 'wb')
188 
189  #Create .pgm file header
190  pgm_header = 'P5' + '\n' + str(width) + ' ' \
191  + str(height) + '\n' + str(2**bits - 1) + '\n'
192 
193  #Data needs to be big endian not little endian for 16bit images
194  if bits == 16:
195  data = data.byteswap()
196  #end if
197 
198  image.write(pgm_header)
199  data.tofile(image)
200  image.close()
201  #end def
202 
203  #Extract image intrinsics from RawCamConfig.msg
204  def process_camera_yaml(self, directory='.', namespace='multisense'):
205  topic_name = '/%s/calibration/raw_cam_config' % namespace
206  bag = rosbag.Bag(self.bag_file)
207  for topic, msg, t in bag.read_messages(topics=[topic_name]):
208 
209  fx = msg.fx
210  fy = msg.fy
211  cx = msg.cx
212  cy = msg.cy
213  tx = msg.tx
214 
215  #Follow expected format of YAML file
216  p1 = "[ %.17e, 0., %d, 0., 0., \n" % (fx, cx) \
217  + " %.17e, %d, 0., 0., 0., 1., 0.]" % (fy, cy) \
218 
219  p2 = "[ %.17e, 0., %d, %.17e, 0., \n" % (fx, cx, tx*fx) \
220  + " %.17e, %d, 0., 0., 0., 1., 0.]" % (fy, cy) \
221 
222  self.write_camera_yaml(directory + "/extrinsics_0p5mp.yml", p1, p2)
223 
224 
225 
226  self.qMatrix = np.matrix([[fy * tx, 0 , 0, -fy * cx * tx],
227  [0, fx * tx, 0, -fx * cy * tx],
228  [0, 0, 0, fx * fy * tx],
229  [0, 0, -fy, 0]])
230 
231  self.qMatrixInverse = np.linalg.inv(self.qMatrix)
232 
233  break
234  #end for
235  #end def
236 
237 
238  #Writes P1 and P2 in openCV format to be used internally.
239  #P1 and P2 represent the camera intrinsics
240  def write_camera_yaml(self, name, p1, p2):
241  yaml = open(name, 'wb')
242 
243  yaml_header = "%YAML:1.0\n"
244  yaml.write(yaml_header)
245 
246  p1_header = "P1: !!opencv-matrix\n" \
247  + " rows: 3\n" \
248  + " cols: 4\n" \
249  + " dt: d\n"
250 
251 
252  p2_header = "P2: !!opencv-matrix\n" \
253  + " rows: 3\n" \
254  + " cols: 4\n" \
255  + " dt: d\n"
256 
257  yaml.write(p1_header)
258  yaml.write(" data: " + p1 + '\n\n')
259  yaml.write(p2_header)
260  yaml.write(" data: " + p2 + '\n')
261  yaml.close()
262 
263  #Extract Laser To Spindle and Camera To Spindle Extrinsics from
264  #RawLidarCal.msg
265  def process_laser_yaml(self, directory=".", namespace='multisense'):
266  topic_name = '/%s/calibration/raw_lidar_cal' % namespace
267  bag = rosbag.Bag(self.bag_file)
268  for topic, msg, t in bag.read_messages(topics=[topic_name]):
269 
270  #Laser to spindle
271  lts = list(msg.laserToSpindle)
272  #Camera to spindle
273  cts = list(msg.cameraToSpindleFixed)
274 
275  #Mimics existing OpenCV format
276  laser_t_spind = "[ %.17e, %.17e,\n" % (lts[0], lts[1]) \
277  +" %.17e, %.17e,\n" % (lts[2], lts[3]) \
278  +" %.17e, %.17e,\n" % (lts[4], lts[5]) \
279  +" %.17e, %.17e,\n" % (lts[6], lts[7]) \
280  +" %.17e, %.17e,\n" % (lts[8], lts[9]) \
281  +" %.17e, 0., 0., 0., 0., 1. ]" % (lts[10])
282 
283 
284  camera_t_spind = "[ %.17e, %.17e,\n" % (cts[0], cts[1]) \
285  +" %.17e, %.17e,\n" % (cts[2], cts[3]) \
286  +" %.17e, %.17e,\n" % (cts[4], cts[5]) \
287  +" %.17e, %.17e,\n" % (cts[6], cts[7]) \
288  +" %.17e, %.17e,\n" % (cts[8], cts[9]) \
289  +" %.17e, %.17e, 0., 0., 0., 1. ]" % (cts[10], cts[11])
290 
291  self.write_laser_yaml(directory + "/laser_cal.yml",
292  laser_t_spind, camera_t_spind)
293 
294  self.laserToSpindle = np.matrix([[lts[0], lts[1], lts[2], lts[3]],
295  [lts[4], lts[5], lts[6], lts[7]],
296  [lts[8], lts[9], lts[10], lts[11]],
297  [lts[12], lts[13], lts[14], lts[15]]])
298 
299  self.spindleToCamera = np.matrix([[cts[0], cts[1], cts[2], cts[3]],
300  [cts[4], cts[5], cts[6], cts[7]],
301  [cts[8], cts[9], cts[10], cts[11]],
302  [cts[12], cts[13], cts[14], cts[15]]])
303 
304 
305  break
306  #end for
307  #end def
308 
309  #Write laser calibration in expected OpenCV format
310  def write_laser_yaml(self, name, laser_t_spind, cam_t_spind):
311  yaml = open(name, 'wb')
312  yaml_header = "%YAML:1.0\n"
313  yaml.write(yaml_header)
314 
315  laser_t_spind_h = "laser_T_spindle: !!opencv-matrix\n" \
316  +" rows: 4\n" \
317  +" cols: 4\n" \
318  +" dt: d\n"
319 
320  cam_t_spind_h = "camera_T_spindle_fixed: !!opencv-matrix\n" \
321  +" rows: 4\n" \
322  +" cols: 4\n" \
323  +" dt: d\n"
324 
325  yaml.write(laser_t_spind_h)
326  yaml.write(" data: " + laser_t_spind + '\n')
327  yaml.write(cam_t_spind_h)
328  yaml.write(" data: " + cam_t_spind + '\n')
329  yaml.close()
330  #end def
331 
332  #Writes out sensor information appends SN to bagfile name
333  #Returns new bag file name
334  def rename_bag(self, directory=".", namespace='multisense'):
335  topic_name = '/%s/calibration/device_info' % namespace
336  bag = rosbag.Bag(self.bag_file)
337  for topic, msg, t in bag.read_messages(topics=[topic_name]):
338 
339  #Extract all the digits in the serial number. The format of the
340  #serial number varies from unit to unit (SNXXX, SL XXXX, SN SLXXXX)
341  sn = msg.serialNumber.strip()
342  exclude_characters = string.letters + string.punctuation + " "
343  sn = sn.strip(exclude_characters)
344 
345  #If for whatever reason there are characters still in our serial
346  #number (XXXvX) just append SN to the front
347  try:
348  sn = "SN%04d" % int(sn)
349  except:
350  sn = "SN" + sn
351 
352  info = open(os.path.join(directory, "dev_info.txt"), 'wb')
353 
354  info.write(str(msg))
355  info.close
356 
357 
358  bag = os.path.basename(self.bag_file)
359  path = os.path.dirname(self.bag_file)
360 
361  fname = os.path.join(path, os.path.splitext(bag)[0]\
362  + "_SL_%s_calCheck.bag" % sn)
363 
364 
365  os.rename(self.bag_file, fname)
366  self.bag_file = fname
367  return fname
368  #end for
369  #end def
370 
371  # Compute the average of all our errors
373  average = 0.
374  valid_elements = 0
375 
376  for item in self.errorImage:
377  # A value of -1 means the cell has not been initialized
378  if item != -1:
379  average += item
380  valid_elements += 1
381  #end if
382  #end for
383 
384  average /= valid_elements
385 
386  return average
387 
388  #end def
389 
390  # Convert our errors into a image to see where the calibration is poor
391  def compute_error_image(self, average_error, directory='.'):
392  min_error = 0
393  max_error = 3 * average_error
394 
395  eight_bit_error = np.zeros(len(self.errorImage), dtype=np.uint8)
396 
397  # Iterate through our error image and convert it to a 8 bit image
398  # using the average_error error as our max error
399  for i, item in enumerate(self.errorImage):
400  # A value of -1 means the cell has not been initialized
401  if item != -1:
402  eight_bit_error[i] = min((item - min_error) / (max_error - min_error) * 255, 255)
403  #end if
404  #end for
405 
406  self.write_pgm(eight_bit_error, os.path.join(directory,"errorImage.pgm"),
407  self.disparityWidth, self.disparityHeight, 8)
408 
409  if self.laserDisparity != None:
410  self.write_pgm(self.laserDisparity, os.path.join(directory,"laserDisparity.pgm"),
411  self.disparityWidth, self.disparityHeight, 16)
412  #end if
413 
414  #end def
415 
416 
417  # Compute the error or an individual scan message
418  def compute_laser_error(self, laser_msg):
419  # Transform a laser scan so it is in the camera frame
420  laser_scan = self.transform_laser_scan(laser_msg)
421 
422  # Make sure our laser_scan point are not None. I.e. we got valid
423  # laser data from the bag file
424  if laser_scan == None:
425  return
426  #end if
427 
428  # Convert our laser scan into the disparity space of the camera
429  disparity_scan = self.qMatrixInverse * laser_scan
430 
431  for i in range(disparity_scan.shape[1]):
432 
433 
434  # Scale our projected vector
435  u = round(disparity_scan[0,i]/disparity_scan[3,i])
436  v = round(disparity_scan[1,i]/disparity_scan[3,i])
437  d = disparity_scan[2,i]/disparity_scan[3,i]
438 
439  # Make sure the point projects into our image
440  if (u < 0 or u >= self.disparityWidth or
441  v < 0 or v >= self.disparityHeight):
442  continue
443  #end if
444 
445  index = u + self.disparityWidth * v
446 
447  # Save our computed disparity value in a image. We will store
448  # the value in 1/6 of a pixel
449  self.laserDisparity[index] = d * 16.
450 
451 
452  # Get the actual disparity. This is a value which is in 1/16 of a pixel.
453  # Here we will convert it to pixels
454  actual_disparity = self.disparityImage[index]/16.
455 
456  # If our actual disparity is 0 it means our value is not valid. Here
457  # we will just skip the point
458  if actual_disparity == 0:
459  continue
460  #end if
461 
462 
463  # A value of -1 means we dont have a disparity error value for the pixel.
464  # Otherwise we will select the minimum error
465  if self.errorImage[index] == -1:
466  self.errorImage[index] = abs(d - actual_disparity)
467  else:
468  self.errorImage[index] = min(self.errorImage[index], abs(d - actual_disparity))
469  #end if
470 
471  #end for
472  #end def
473 
474  # Transform a single laser scan message into the camera optical frame
475  def transform_laser_scan(self, laser_msg):
476 
477  # Angles are in micro radians. Also we need to handle rollover of the angle
478  angle_start = self.normalize_angle(laser_msg.angle_start * 1e-6)
479  angle_end = self.normalize_angle(laser_msg.angle_end * 1e-6)
480  angle_range = self.normalize_angle(angle_end - angle_start)
481 
482  # Hokuyo UTM30-LX-EW scanners range from -135 to 135 degrees
483  start_mirror = -135.0 * np.pi /180.
484  end_mirror = 135.0 * np.pi/180.
485 
486  laserPoints = None
487 
488  # Iterate through each laser point and compute the spindle transform
489  for i, d in enumerate(laser_msg.distance):
490 
491  percentage = i / float(len(laser_msg.distance))
492 
493  mirror_angle = start_mirror + percentage * (end_mirror - start_mirror)
494 
495  # Data from the Hokuyo is in millimeters. Convert it to meters
496  distance = 1e-3 * d
497 
498  # The expected laser data coordinate frame has the laser data in the x-z plane
499  point = np.matrix([[ distance * np.sin(mirror_angle)],
500  [ 0.],
501  [ distance * np.cos(mirror_angle)],
502  [1.]])
503 
504  # Determine the angle of our spindle for this point. We will
505  # perform a simple linear interpolation between the start and
506  # end spindle angles
507  spindle_angle = angle_start + percentage * angle_range
508 
509  # The spindle rotates about the z axis of the laser scan data
510  spindle_transform = np.matrix([[np.cos(spindle_angle), -np.sin(spindle_angle), 0., 0.],
511  [np.sin(spindle_angle), np.cos(spindle_angle), 0., 0.],
512  [0., 0., 1., 0.],
513  [0., 0., 0., 1.]])
514 
515  # Transform our spindle into the camera coordinate frame
516  transformed_point = self.spindleToCamera * spindle_transform * self.laserToSpindle * point;
517 
518  # Skip points whose Z coordinates are too close to the camera.
519  # The minimum range of the MultiSense is 0.4 m
520  if transformed_point[2, 0] < 0.4:
521  continue
522  #end if
523 
524  # Add our transformed points to our ouput laser scan
525  if laserPoints == None:
526  laserPoints = transformed_point
527  else:
528  laserPoints = np.append(laserPoints, transformed_point, axis=1)
529  #end if
530 
531  #end for
532 
533  return laserPoints
534  #end def
535 
536  # Normalize angles so it is positive and bound within 0 and 2pi
537  def normalize_angle(self, angle):
538  # first make sure our angle is positive
539  positive_angle = ((angle % (2 * np.pi)) + (2 * np.pi)) % (2 * np.pi)
540 
541  # Bound our angle to 2 pi
542  if positive_angle > (2 * np.pi):
543  return positive_angle - (2 * np.pi)
544  #end if
545 
546  return positive_angle;
547  #end def
548 
549 
550 
551 def usage(argv):
552  print "\nUsage: %s -b <bagfile> [Options]" % argv
553  print "Where [Options] are"
554  print " -o <output_directory>\t Specifies theoutput directory to save"\
555  +" the output debug files. Default: ."
556  print " -n <namespace>\t\t The namespace the Multisense topics are in. "\
557  + "Default: multisense"
558  print " -r\t\t\t\t Rename the input bag file to send to CRL"
559  print " -h\t\t\t\t Print the usage"
560 # end def
561 
562 if __name__ == '__main__':
563  # Set up default arguments
564  bag_file_name = None
565  output_directory_name = time.strftime('%Y-%m-%d_%H-%M-%S_process_bags')
566  namespace = 'multisense'
567  rename_file = False
568 
569  try:
570  opts, args = getopt.getopt(sys.argv[1:], "b:o:n:rh")
571  except getopt.GetoptError as err:
572  print str(err)
573  usage(sys.argv[0])
574  sys.exit(2)
575  for o, a in opts:
576  if o == "-h":
577  usage(sys.argv[0])
578  sys.exit()
579  elif o == "-r":
580  rename_file = True
581  elif o == "-b":
582  bag_file_name = str(a)
583  elif o == "-o":
584  output_directory_name = str(a)
585  else:
586  usage(sys.argv[0])
587  sys.exit()
588  #end if
589  #end for
590 
591  if None == bag_file_name:
592  print "Error no bag file specified"
593  usage(sys.argv[0])
594  sys.exit()
595  #end if
596 
597  # Prepare to run _BagProcessor.
598  if not os.path.exists(output_directory_name):
599  os.makedirs(output_directory_name)
600  # end if
601 
602  if not os.path.isdir(output_directory_name):
603  raise IOError('%s is not a directory' % output_directory_name)
604  # end if
605 
606  # Do the processsing.
607  bagProcessor = _BagProcessor(bag_file_name)
608  bagProcessor.process(output_directory_name, rename_file, namespace)
609  sys.exit(0)
610 # end if
def write_pgm(self, data, name, width, height, bits)
def process_laser_yaml(self, directory=".", namespace='multisense')
def usage(argv)
def transform_laser_scan(self, laser_msg)
def write_laser_yaml(self, name, laser_t_spind, cam_t_spind)
def process_camera_yaml(self, directory='.', namespace='multisense')
def __init__(self, bag_file)
Definition: process_bags.py:27
def process_image(self, directory='.', namespace='multisense')
def rename_bag(self, directory=".", namespace='multisense')
def normalize_angle(self, angle)
def write_camera_yaml(self, name, p1, p2)
def process_laser(self, directory='.', namespace='multisense')
def compute_laser_error(self, laser_msg)
def compute_error_image(self, average_error, directory='.')
def process(self, directory='.', rename_file=True, namespace='multisense')
Definition: process_bags.py:58


multisense_cal_check
Author(s):
autogenerated on Sat Apr 6 2019 02:16:58