process_bags.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 Simple Script to Processes Bag files from RawSnapshot in Release 2.0
00005 and writes the data to file for calibration
00006 
00007 Please direct any question to multisense@carnegierobotics.com or
00008     http://support.carnegierobotics.com
00009 '''
00010 
00011 import sys
00012 import csv
00013 import time
00014 import os
00015 import numpy as np
00016 import string
00017 import getopt
00018 try:
00019     import rospy
00020     import rosbag
00021 except:
00022     raise Exception("Error importing ROS. Source the ROS environment" \
00023                    +" in the workspace where the multisense stack is located")
00024 
00025 class _BagProcessor():
00026 
00027     def __init__(self, bag_file):
00028         self.bag_file = bag_file
00029 
00030         # Transforms to move points in the laser frame to the left
00031         # camera optical frame
00032         self.laserToSpindle = None
00033         self.spindleToCamera = None
00034 
00035         # The matrix used to reproject disparity point into 3D. We will use this
00036         # to perform the opposite operation and transform laser points into
00037         # the camera frame
00038         self.qMatrix = None
00039         self.qMatrixInverse = None
00040 
00041         # The disparity image read from the bag file
00042         self.disparityImage = None
00043         self.disparityHeight = 0
00044         self.disparityWidth = 0
00045 
00046         # This is the error in terms of pixels between the disparity value
00047         # reported by the camera and the theoretical disparity value
00048         # that the camera would need to have to perfectly match the laser data
00049         self.errorImage = None
00050 
00051         # A debug image which is the laser data as a disparity image
00052         self.laserDisparity = None
00053     #end def
00054 
00055     #Wrapper method to processes the bag file
00056     #Returns bag file name
00057     def process(self, directory='.', rename_file = True,
00058                 namespace = 'multisense'):
00059 
00060         # Save out the stereo and camera images for debugging. Also store
00061         # the disparity image to be used when computing the laser error
00062         # metric
00063         if not self.process_image(directory, namespace):
00064             print "Could not find valid images in the bag file"
00065             return None
00066         #end if
00067 
00068         # Get the calibration for our camera
00069         self.process_camera_yaml(directory, namespace)
00070 
00071         # Get the transforms to convert the laser data into the left
00072         # camera optical frame, the same coordinate frame which the disparity image
00073         # in in
00074         self.process_laser_yaml(directory, namespace)
00075 
00076         # Process our laser data and convert each scan into disparity space
00077         self.process_laser(directory, namespace)
00078 
00079         # Compute a error metric based on our processed stereo and laser
00080         # data
00081         average_error = self.compute_average_error()
00082 
00083         print "Average Pixel Error:", average_error, "pixels"
00084 
00085         if average_error < 2.:
00086             print "Calibration is good"
00087         else:
00088             print "Calibration appears to be poor. Place the unit 1 meter from"\
00089                   " a texture-rich corner an rerun the check."
00090         #end if
00091 
00092 
00093         # Save a image which displays where the largest error are in our
00094         # disparity image
00095         self.compute_error_image(average_error, directory)
00096 
00097         return_value = self.bag_file
00098         if rename_file:
00099             return_value = self.rename_bag(directory)
00100         # end if
00101 
00102         return return_value
00103     #end def
00104 
00105     #Method to extract laser data from bag file and save into .csv
00106     def process_laser(self, directory='.', namespace='multisense'):
00107         bag = rosbag.Bag(self.bag_file)
00108         with open(directory + '/' + 'lidarData.csv', 'wb') as laser_file:
00109             topic_name = '/%s/calibration/raw_lidar_data' % namespace
00110             laser_writer = csv.writer(laser_file, delimiter=',')
00111             for topic, msg, t in bag.read_messages(topics=[topic_name]):
00112                 #Unbundle message
00113                 scan_count = msg.scan_count
00114                 time_start = float(msg.time_start.secs +
00115                                         msg.time_start.nsecs * 1e-9)
00116                 time_end = float(msg.time_end.secs + msg.time_end.nsecs * 1e-9)
00117                 angle_start = msg.angle_start
00118                 angle_end = msg.angle_end
00119                 dist = msg.distance
00120                 reflect = msg.intensity
00121 
00122                 #Expected Format: scan_count, time_start, time_end, angle_start,
00123                 #angle_end, , range, , intensity, ,len(range)
00124                 row = [scan_count, time_start, time_end,
00125                        angle_start, angle_end, " "] + list(dist) + [" "] \
00126                        + list(reflect) + [" "] + [len(list(dist))]
00127 
00128                 laser_writer.writerow(row)
00129 
00130                 # Transform our laser data into the camera frame
00131                 self.compute_laser_error(msg)
00132 
00133             #end for
00134 
00135         laser_file.close()
00136 
00137         #end with
00138     #end def
00139 
00140 
00141     #Method to extract first instance of disparity and recitified images from
00142     #bag
00143     def process_image(self, directory='.', namespace='multisense'):
00144         topic_name = '/%s/calibration/raw_cam_data' % namespace
00145         bag = rosbag.Bag(self.bag_file)
00146 
00147         #Attempt to find both disparity and rectified images before quitting
00148         found_rectified_image = False
00149         found_disparity_image = False
00150 
00151         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00152             width = msg.width
00153             height = msg.height
00154 
00155             #Write to .pgm file stereo_left_0000.pgm
00156             if not found_rectified_image and len(list(msg.gray_scale_image)) != 0:
00157                 self.write_pgm(np.array(list(msg.gray_scale_image)),
00158                                 directory + '/' + "stereo_left_0000.pgm",
00159                                 width, height, 8)
00160                 found_rectified_image = True
00161             #end if
00162 
00163             if not found_disparity_image and len(list(msg.disparity_image)) != 0:
00164                 self.disparityImage = np.array(list(msg.disparity_image), dtype=np.uint16)
00165                 self.errorImage = np.zeros(len(list(msg.disparity_image)), dtype=np.float64) - 1
00166                 self.laserDisparity = np.zeros(len(list(msg.disparity_image)), dtype=np.uint16)
00167                 self.disparityHeight = height
00168                 self.disparityWidth = width
00169                 self.write_pgm(self.disparityImage,
00170                                 directory + '/' + "disparity_0000.pgm",
00171                                 width, height, 16)
00172                 found_disparity_image = True
00173             #end if
00174 
00175 
00176             #Quit once disparity and rectified images have been found
00177             if found_disparity_image and found_rectified_image:
00178                 return True
00179             #end if
00180         #end for
00181 
00182         return False
00183     #end def
00184 
00185     #Method to write an image to a .pgm file
00186     def write_pgm(self, data, name, width, height, bits):
00187         image = open(name, 'wb')
00188 
00189         #Create .pgm file header
00190         pgm_header = 'P5' + '\n' + str(width) + ' ' \
00191                      + str(height) + '\n' + str(2**bits - 1) + '\n'
00192 
00193         #Data needs to be big endian not little endian for 16bit images
00194         if bits == 16:
00195             data = data.byteswap()
00196         #end if
00197 
00198         image.write(pgm_header)
00199         data.tofile(image)
00200         image.close()
00201     #end def
00202 
00203     #Extract image intrinsics from RawCamConfig.msg
00204     def process_camera_yaml(self, directory='.', namespace='multisense'):
00205         topic_name = '/%s/calibration/raw_cam_config' % namespace
00206         bag = rosbag.Bag(self.bag_file)
00207         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00208 
00209             fx = msg.fx
00210             fy = msg.fy
00211             cx = msg.cx
00212             cy = msg.cy
00213             tx = msg.tx
00214 
00215             #Follow expected format of YAML file
00216             p1 =  "[ %.17e, 0., %d, 0., 0., \n" % (fx, cx) \
00217                  + "       %.17e, %d, 0., 0., 0., 1., 0.]" % (fy, cy) \
00218 
00219             p2 =  "[ %.17e, 0., %d, %.17e, 0., \n" % (fx, cx, tx*fx) \
00220                  + "       %.17e, %d, 0., 0., 0., 1., 0.]" % (fy, cy) \
00221 
00222             self.write_camera_yaml(directory + "/extrinsics_0p5mp.yml", p1, p2)
00223 
00224 
00225 
00226             self.qMatrix = np.matrix([[fy * tx, 0 , 0, -fy * cx * tx],
00227                                       [0, fx * tx, 0, -fx * cy * tx],
00228                                       [0, 0, 0, fx * fy * tx],
00229                                       [0, 0, -fy, 0]])
00230 
00231             self.qMatrixInverse = np.linalg.inv(self.qMatrix)
00232 
00233             break
00234         #end for
00235     #end def
00236 
00237 
00238     #Writes P1 and P2 in openCV format to be used internally.
00239     #P1 and P2 represent the camera intrinsics
00240     def write_camera_yaml(self, name, p1, p2):
00241         yaml = open(name, 'wb')
00242 
00243         yaml_header = "%YAML:1.0\n"
00244         yaml.write(yaml_header)
00245 
00246         p1_header = "P1: !!opencv-matrix\n" \
00247                      + "   rows: 3\n" \
00248                      + "   cols: 4\n" \
00249                      + "   dt: d\n"
00250 
00251 
00252         p2_header = "P2: !!opencv-matrix\n" \
00253                      + "   rows: 3\n" \
00254                      + "   cols: 4\n" \
00255                      + "   dt: d\n"
00256 
00257         yaml.write(p1_header)
00258         yaml.write("   data: " + p1 + '\n\n')
00259         yaml.write(p2_header)
00260         yaml.write("   data: " + p2 + '\n')
00261         yaml.close()
00262 
00263     #Extract Laser To Spindle and Camera To Spindle Extrinsics from
00264     #RawLidarCal.msg
00265     def process_laser_yaml(self, directory=".", namespace='multisense'):
00266         topic_name = '/%s/calibration/raw_lidar_cal' % namespace
00267         bag = rosbag.Bag(self.bag_file)
00268         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00269 
00270             #Laser to spindle
00271             lts = list(msg.laserToSpindle)
00272             #Camera to spindle
00273             cts = list(msg.cameraToSpindleFixed)
00274 
00275             #Mimics existing OpenCV format
00276             laser_t_spind = "[ %.17e, %.17e,\n" % (lts[0], lts[1]) \
00277                             +"      %.17e, %.17e,\n" % (lts[2], lts[3]) \
00278                             +"      %.17e, %.17e,\n" % (lts[4], lts[5]) \
00279                             +"      %.17e, %.17e,\n" % (lts[6], lts[7]) \
00280                             +"      %.17e, %.17e,\n" % (lts[8], lts[9]) \
00281                             +"      %.17e, 0., 0., 0., 0., 1. ]" % (lts[10])
00282 
00283 
00284             camera_t_spind = "[ %.17e, %.17e,\n" % (cts[0], cts[1]) \
00285                              +"      %.17e, %.17e,\n" % (cts[2], cts[3]) \
00286                              +"      %.17e, %.17e,\n" % (cts[4], cts[5]) \
00287                              +"      %.17e, %.17e,\n" % (cts[6], cts[7]) \
00288                              +"      %.17e, %.17e,\n" % (cts[8], cts[9]) \
00289                              +"      %.17e, %.17e, 0., 0., 0., 1. ]" % (cts[10], cts[11])
00290 
00291             self.write_laser_yaml(directory + "/laser_cal.yml",
00292                                   laser_t_spind, camera_t_spind)
00293 
00294             self.laserToSpindle = np.matrix([[lts[0], lts[1], lts[2], lts[3]],
00295                                              [lts[4], lts[5], lts[6], lts[7]],
00296                                              [lts[8], lts[9], lts[10], lts[11]],
00297                                              [lts[12], lts[13], lts[14], lts[15]]])
00298 
00299             self.spindleToCamera = np.matrix([[cts[0], cts[1], cts[2], cts[3]],
00300                                              [cts[4], cts[5], cts[6], cts[7]],
00301                                              [cts[8], cts[9], cts[10], cts[11]],
00302                                              [cts[12], cts[13], cts[14], cts[15]]])
00303 
00304 
00305             break
00306         #end for
00307     #end def
00308 
00309     #Write laser calibration in expected OpenCV format
00310     def write_laser_yaml(self, name, laser_t_spind, cam_t_spind):
00311         yaml = open(name, 'wb')
00312         yaml_header = "%YAML:1.0\n"
00313         yaml.write(yaml_header)
00314 
00315         laser_t_spind_h = "laser_T_spindle: !!opencv-matrix\n" \
00316                           +"   rows: 4\n" \
00317                           +"   cols: 4\n" \
00318                           +"   dt: d\n"
00319 
00320         cam_t_spind_h = "camera_T_spindle_fixed: !!opencv-matrix\n" \
00321                         +"   rows: 4\n" \
00322                         +"   cols: 4\n" \
00323                         +"   dt: d\n"
00324 
00325         yaml.write(laser_t_spind_h)
00326         yaml.write("   data: " + laser_t_spind + '\n')
00327         yaml.write(cam_t_spind_h)
00328         yaml.write("   data: " + cam_t_spind + '\n')
00329         yaml.close()
00330     #end def
00331 
00332     #Writes out sensor information appends SN to bagfile name
00333     #Returns new bag file name
00334     def rename_bag(self, directory=".", namespace='multisense'):
00335         topic_name = '/%s/calibration/device_info' % namespace
00336         bag = rosbag.Bag(self.bag_file)
00337         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00338 
00339             #Extract all the digits in the serial number. The format of the
00340             #serial number varies from unit to unit (SNXXX, SL XXXX, SN SLXXXX)
00341             sn = msg.serialNumber.strip()
00342             exclude_characters = string.letters + string.punctuation + " "
00343             sn = sn.strip(exclude_characters)
00344 
00345             #If for whatever reason there are characters still in our serial
00346             #number (XXXvX) just append SN to the front
00347             try:
00348                 sn = "SN%04d" % int(sn)
00349             except:
00350                 sn = "SN" + sn
00351 
00352             info = open(os.path.join(directory, "dev_info.txt"), 'wb')
00353 
00354             info.write(str(msg))
00355             info.close
00356 
00357 
00358             bag = os.path.basename(self.bag_file)
00359             path = os.path.dirname(self.bag_file)
00360 
00361             fname = os.path.join(path,  os.path.splitext(bag)[0]\
00362                                + "_SL_%s_calCheck.bag" % sn)
00363 
00364 
00365             os.rename(self.bag_file, fname)
00366             self.bag_file = fname
00367             return fname
00368         #end for
00369     #end def
00370 
00371     # Compute the average of all our errors
00372     def compute_average_error(self):
00373         average = 0.
00374         valid_elements = 0
00375 
00376         for item in self.errorImage:
00377             # A value of -1 means the cell has not been initialized
00378             if item != -1:
00379                 average += item
00380                 valid_elements += 1
00381             #end if
00382         #end for
00383 
00384         average /= valid_elements
00385 
00386         return average
00387 
00388     #end def
00389 
00390     # Convert our errors into a image to see where the calibration is poor
00391     def compute_error_image(self, average_error, directory='.'):
00392         min_error = 0
00393         max_error = 3 * average_error
00394 
00395         eight_bit_error = np.zeros(len(self.errorImage), dtype=np.uint8)
00396 
00397         # Iterate through our error image and convert it to a 8 bit image
00398         # using the average_error error as our max error
00399         for i, item in enumerate(self.errorImage):
00400             # A value of -1 means the cell has not been initialized
00401             if item != -1:
00402                 eight_bit_error[i] = min((item - min_error) / (max_error - min_error) * 255, 255)
00403             #end if
00404         #end for
00405 
00406         self.write_pgm(eight_bit_error, os.path.join(directory,"errorImage.pgm"),
00407                        self.disparityWidth, self.disparityHeight, 8)
00408 
00409         if self.laserDisparity != None:
00410             self.write_pgm(self.laserDisparity, os.path.join(directory,"laserDisparity.pgm"),
00411                            self.disparityWidth, self.disparityHeight, 16)
00412         #end if
00413 
00414     #end def
00415 
00416 
00417     # Compute the error or an individual scan message
00418     def compute_laser_error(self, laser_msg):
00419         # Transform a laser scan so it is in the camera frame
00420         laser_scan = self.transform_laser_scan(laser_msg)
00421 
00422         # Make sure our laser_scan point are not None. I.e. we got valid
00423         # laser data from the bag file
00424         if laser_scan == None:
00425             return
00426         #end if
00427 
00428         # Convert our laser scan into the disparity space of the camera
00429         disparity_scan = self.qMatrixInverse * laser_scan
00430 
00431         for i in range(disparity_scan.shape[1]):
00432 
00433 
00434             # Scale our projected vector
00435             u = round(disparity_scan[0,i]/disparity_scan[3,i])
00436             v = round(disparity_scan[1,i]/disparity_scan[3,i])
00437             d = disparity_scan[2,i]/disparity_scan[3,i]
00438 
00439             # Make sure the point projects into our image
00440             if (u < 0 or u >= self.disparityWidth or
00441                 v < 0 or v >= self.disparityHeight):
00442                 continue
00443             #end if
00444 
00445             index = u + self.disparityWidth * v
00446 
00447             # Save our computed disparity value in a image. We will store
00448             # the value in 1/6 of a pixel
00449             self.laserDisparity[index] = d * 16.
00450 
00451 
00452             # Get the actual disparity. This is a value which is in 1/16 of a pixel.
00453             # Here we will convert it to pixels
00454             actual_disparity = self.disparityImage[index]/16.
00455 
00456             # If our actual disparity is 0 it means our value is not valid. Here
00457             # we will just skip the point
00458             if actual_disparity == 0:
00459                 continue
00460             #end if
00461 
00462 
00463             # A value of -1 means we dont have a disparity error value for the pixel.
00464             # Otherwise we will select the minimum error
00465             if self.errorImage[index] == -1:
00466                 self.errorImage[index] = abs(d - actual_disparity)
00467             else:
00468                 self.errorImage[index] = min(self.errorImage[index], abs(d - actual_disparity))
00469             #end if
00470 
00471         #end for
00472     #end def
00473 
00474     # Transform a single laser scan message into the camera optical frame
00475     def transform_laser_scan(self, laser_msg):
00476 
00477         # Angles are in micro radians. Also we need to handle rollover of the angle
00478         angle_start = self.normalize_angle(laser_msg.angle_start * 1e-6)
00479         angle_end = self.normalize_angle(laser_msg.angle_end * 1e-6)
00480         angle_range = self.normalize_angle(angle_end - angle_start)
00481 
00482         # Hokuyo UTM30-LX-EW scanners range from -135 to 135 degrees
00483         start_mirror = -135.0 * np.pi /180.
00484         end_mirror = 135.0 * np.pi/180.
00485 
00486         laserPoints = None
00487 
00488         # Iterate through each laser point and compute the spindle transform
00489         for i, d in enumerate(laser_msg.distance):
00490 
00491             percentage = i / float(len(laser_msg.distance))
00492 
00493             mirror_angle = start_mirror + percentage * (end_mirror - start_mirror)
00494 
00495             # Data from the Hokuyo is in millimeters. Convert it to meters
00496             distance = 1e-3 * d
00497 
00498             # The expected laser data coordinate frame has the laser data in the x-z plane
00499             point = np.matrix([[ distance * np.sin(mirror_angle)],
00500                                [ 0.],
00501                                [ distance * np.cos(mirror_angle)],
00502                                [1.]])
00503 
00504             # Determine the angle of our spindle for this point. We will
00505             # perform a simple linear interpolation between the start and
00506             # end spindle angles
00507             spindle_angle = angle_start + percentage * angle_range
00508 
00509             # The spindle rotates about the z axis of the laser scan data
00510             spindle_transform = np.matrix([[np.cos(spindle_angle), -np.sin(spindle_angle), 0., 0.],
00511                                            [np.sin(spindle_angle), np.cos(spindle_angle), 0., 0.],
00512                                            [0., 0., 1., 0.],
00513                                            [0., 0., 0., 1.]])
00514 
00515             # Transform our spindle into the camera coordinate frame
00516             transformed_point = self.spindleToCamera * spindle_transform * self.laserToSpindle * point;
00517 
00518             # Skip points whose Z coordinates are too close to the camera.
00519             # The minimum range of the MultiSense is 0.4 m
00520             if transformed_point[2, 0] < 0.4:
00521                 continue
00522             #end if
00523 
00524             # Add our transformed points to our ouput laser scan
00525             if laserPoints == None:
00526                 laserPoints = transformed_point
00527             else:
00528                 laserPoints = np.append(laserPoints, transformed_point, axis=1)
00529             #end if
00530 
00531         #end for
00532 
00533         return laserPoints
00534     #end def
00535 
00536     # Normalize angles so it is positive and bound within 0 and 2pi
00537     def normalize_angle(self, angle):
00538         # first make sure our angle is positive
00539         positive_angle = ((angle % (2 * np.pi)) + (2 * np.pi)) % (2 * np.pi)
00540 
00541         # Bound our angle to 2 pi
00542         if positive_angle > (2 * np.pi):
00543             return positive_angle - (2 * np.pi)
00544         #end if
00545 
00546         return positive_angle;
00547     #end def
00548 
00549 
00550 
00551 def usage(argv):
00552     print "\nUsage: %s -b <bagfile> [Options]" % argv
00553     print "Where [Options] are"
00554     print "     -o <output_directory>\t Specifies theoutput directory to save"\
00555                 +" the output debug files. Default: ."
00556     print "     -n <namespace>\t\t The namespace the Multisense topics are in. "\
00557                 + "Default: multisense"
00558     print "     -r\t\t\t\t Rename the input bag file to send to CRL"
00559     print "     -h\t\t\t\t Print the usage"
00560 # end def
00561 
00562 if __name__ == '__main__':
00563     # Set up default arguments
00564     bag_file_name = None
00565     output_directory_name = time.strftime('%Y-%m-%d_%H-%M-%S_process_bags')
00566     namespace = 'multisense'
00567     rename_file = False
00568 
00569     try:
00570         opts, args = getopt.getopt(sys.argv[1:], "b:o:n:rh")
00571     except getopt.GetoptError as err:
00572         print str(err)
00573         usage(sys.argv[0])
00574         sys.exit(2)
00575     for o, a in opts:
00576         if o == "-h":
00577             usage(sys.argv[0])
00578             sys.exit()
00579         elif o == "-r":
00580             rename_file = True
00581         elif o == "-b":
00582             bag_file_name = str(a)
00583         elif o == "-o":
00584             output_directory_name = str(a)
00585         else:
00586             usage(sys.argv[0])
00587             sys.exit()
00588         #end if
00589     #end for
00590 
00591     if None == bag_file_name:
00592         print "Error no bag file specified"
00593         usage(sys.argv[0])
00594         sys.exit()
00595     #end if
00596 
00597     # Prepare to run _BagProcessor.
00598     if not os.path.exists(output_directory_name):
00599         os.makedirs(output_directory_name)
00600     # end if
00601 
00602     if not os.path.isdir(output_directory_name):
00603         raise IOError('%s is not a directory' % output_directory_name)
00604     # end if
00605 
00606     # Do the processsing.
00607     bagProcessor = _BagProcessor(bag_file_name)
00608     bagProcessor.process(output_directory_name, rename_file, namespace)
00609     sys.exit(0)
00610 # end if


multisense_cal_check
Author(s):
autogenerated on Mon Oct 9 2017 03:06:29