def rgb_from_jpg_by_JPEG_library(data): try: import jpeg4py as jpeg except ImportError as e: installation = """ sudo apt-get install -y libturbojpeg python-cffi sudo pip install jpeg4py """ logger.error(installation) raise jpg_data = np.fromstring(data, dtype=np.uint8) image_cv = jpeg.JPEG(jpg_data).decode() return image_cv
def rgb_from_jpg_by_JPEG_library(data): try: import jpeg4py as jpeg except ImportError as e: installation = """ sudo apt-get install -y libturbojpeg python-cffi sudo pip install jpeg4py """ logger.error(installation) raise jpg_data = np.fromstring(data, dtype=np.uint8) image_cv = jpeg.JPEG(jpg_data).decode() return image_cv
def load_homography(self): '''Load homography (extrinsic parameters)''' filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn("no extrinsic calibration parameters for {}, trying default".format(self.robot_name)) filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong") else: data = yaml_load_file(filename) else: data = yaml_load_file(filename) logger.info("Loaded homography for {}".format(os.path.basename(filename))) return np.array(data['homography']).reshape((3,3))
def safe_pickle_dump(value, filename, protocol=pickle.HIGHEST_PROTOCOL, **safe_write_options): with safe_write(filename, **safe_write_options) as f: try: pickle.dump(value, f, protocol) except KeyboardInterrupt: raise except Exception: msg = 'Cannot pickle object of class %s' % type(value).__name__ logger.error(msg) # msg = find_pickling_error(value, protocol) # logger.error(msg) raise
def estimate_homography(self,cv_image): '''Estimate ground projection using instrinsic camera calibration parameters''' cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") ret, corners = cv2.findChessboardCorners(cv_image_rectified, (self.board_['width'], self.board_['height']), cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE) if ret == False: logger.error("No corners found in image") exit(1) if len(corners) != self.board_['width'] * self.board_['height']: logger.error("Not all corners found in image") exit(2) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11,11), (-1,-1), criteria) cv2.drawChessboardCorners(cv_image_rectified, (7,5), corners2, ret) cv2.imshow('corners',cv_image_rectified) cv2.waitKey(10000) cv2.destroyAllWindows() #TODO flip checks src_pts = [] for r in range(self.board_['height']): for c in range(self.board_['width']): src_pts.append(np.array([r * self.board_['square_size'] , c * self.board_['square_size']] , dtype='float32') + self.board_['offset']) # OpenCV labels corners left-to-right, top-to-bottom # We're having a problem with our pattern since it's not rotation-invariant # only reverse order if first point is at bottom right corner if ((corners[0])[0][0] < (corners[self.board_['width']*self.board_['height']-1])[0][0] and (corners[0])[0][0] < (corners[self.board_['width']*self.board_['height']-1])[0][1]): rospy.loginfo("Reversing order of points.") src_pts.reverse() # Compute homography from image to ground self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2), np.array(src_pts), cv2.RANSAC) extrinsics_filename = get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml" self.write_homography(extrinsics_filename) logger.info("Wrote ground projection to {}".format(extrinsics_filename)) # Check if specific point in matrix is larger than zero (this would definitly mean we're having a corrupted rotation matrix) if(self.H[1][2] > 0): rospy.logerr("WARNING: Homography could be corrupt!")
def estimate_homography(self,cv_image, board_size, offset): cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") corners = cv2.findChessboardCorners(cv_image, board_size) if corners is None: logger.error("No corners found in image") criteria = (cv2.CV_TERMCRIT_EPS + cv2.CV_TERMCRIT_ITER,30,0.1) cv2.cornerSubPix(cv_image,corners,cv2.Size(11,11),cv2.Size(-1,-1),criteria) #TODO flip checks for r in range(board_h): for c in range(board_w): src_pts[r,c] = np.float32([r*square_size,c*square_size]) + offset self.H, mask = cv2.findHomography(src_pts, corners, method=cv2.CV_RANSAC) write_homography(self.extrinsic_filename)
def get_cached(cache_name, f, quiet='not-given'): """ Caches the result of f() in a file called ${DUCKIETOWN_ROOT}/caches/![name].cache.pickle """ cache = '${DUCKIETOWN_ROOT}/caches/%s.cache.pickle' % cache_name cache = expand_all(cache) if quiet == 'not-given': should_be_quiet = False else: should_be_quiet = quiet if os.path.exists(cache): if not should_be_quiet: logger.info('Using cache %s' % friendly_path(cache)) try: return safe_pickle_load(cache) except: msg = 'Removing cache that I cannot read: %s' % friendly_path( cache) logger.error(msg) os.unlink(cache) ob = f() if not should_be_quiet: logger.info('Writing to cache %s' % friendly_path(cache)) try: os.makedirs(os.path.dirname(cache)) except: pass safe_pickle_dump(ob, cache) # with open(cache, 'w') as f: # cPickle.dump(ob, f) # # return ob
def load_camera_info(self): '''Load camera intrinsics''' filename = (os.environ['DUCKIEFLEET_ROOT'] + "/calibrations/camera_intrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn("no intrinsic calibration parameters for {}, trying default".format(self.robot_name)) filename = (os.environ['DUCKIEFLEET_ROOT'] + "/calibrations/camera_intrinsic/default.yaml") if not os.path.isfile(filename): logger.error("can't find default either, something's wrong") calib_data = yaml_load_file(filename) # logger.info(yaml_dump(calib_data)) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape((3,3)) cam_info.D = np.array(calib_data['distortion_coefficients']['data']).reshape((1,5)) cam_info.R = np.array(calib_data['rectification_matrix']['data']).reshape((3,3)) cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape((3,4)) cam_info.distortion_model = calib_data['distortion_model'] logger.info("Loaded camera calibration parameters for {} from {}".format(self.robot_name, os.path.basename(filename))) return cam_info
def estimate_homography(self, cv_image): '''Estimate ground projection using instrinsic camera calibration parameters''' cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) cv_image_rectified = self.rectify(cv_image) logger.info("image rectified") ret, corners = cv2.findChessboardCorners( cv_image_rectified, (self.board_['width'], self.board_['height'])) if ret == False: logger.error("No corners found in image") exit(1) if len(corners) != self.board_['width'] * self.board_['height']: logger.error("Not all corners found in image") exit(2) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01) corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11, 11), (-1, -1), criteria) #TODO flip checks src_pts = [] for r in range(self.board_['height']): for c in range(self.board_['width']): src_pts.append( np.array([ r * self.board_['square_size'], c * self.board_['square_size'] ], dtype='float32') + self.board_['offset']) # OpenCV labels corners left-to-right, top-to-bottom # so we reverse the groud points src_pts.reverse() # Compute homography from image to ground self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2), np.array(src_pts), cv2.RANSAC) extrinsics_filename = get_duckiefleet_root( ) + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml" self.write_homography(extrinsics_filename) logger.info( "Wrote ground projection to {}".format(extrinsics_filename))
def wrap_script_entry_point(function, exceptions_no_traceback=(DTUserError, )): """ Wraps the main() of a script. For Exception: we exit with value 2. :param exceptions_no_traceback: tuple of exceptions for which we just print the error, and exit with value 1. """ try: ret = function() if ret is None: ret = 0 sys.exit(ret) except exceptions_no_traceback as e: s = "Error during execution of %s:" % function.__name__ s += '\n' + str(e) logger.error(s) sys.exit(1) except Exception as e: logger.error(traceback.format_exc(e)) sys.exit(2)
def process_one(bag_filename, t0, t1, processors, log_out): logger.info('job_one()') logger.info(' input: %s' % bag_filename) logger.info(' processors: %s' % processors) logger.info(' out: %s' % log_out) d8n_make_sure_dir_exists(log_out) tmpdir = create_tmpdir() tmpfiles = [] def get_tmp_bag(): i = len(tmpfiles) f = os.path.join(tmpdir, 'tmp%d.bag' % i) tmpfiles.append(f) return f easy_algo_db = get_easy_algo_db() # instantiate processors processors_instances = [ easy_algo_db.create_instance('processor', _) for _ in processors ] tmpfiles = [] try: for i, p in enumerate(processors_instances): next_bag_filename = get_tmp_bag() in_bag = rosbag.Bag(bag_filename) if i == 0: in_bag = BagReadProxy(in_bag, t0, t1) out_bag = rosbag.Bag(next_bag_filename, 'w') logger.info('Processing:\n in = %s\n out = %s' % (bag_filename, next_bag_filename)) p.process_log(in_bag, out_bag) in_bag.close() out_bag.close() bag_filename = next_bag_filename logger.info('Creating output file %s' % log_out) if not processors: # just create symlink logger.info('(Just creating symlink, because there ' 'was no processing done.)') os.symlink(os.path.realpath(bag_filename), log_out) else: try: shutil.copy(bag_filename, log_out) except: logger.error('Could not create %s' % log_out) logger.info('I created %s' % log_out) finally: for f in tmpfiles: logger.info(' deleting %s' % f) os.unlink(f) return log_out
def write_calibration(self): '''Load kinematic calibration file''' home = expanduser("~") filename = (home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml") if not os.path.isfile(filename): logger.warn( "no kinematic calibration parameters for {}, taking some from default" .format(self.robot_name)) filename = (home + "/duckietown_sysid/kinematics/default.yaml") if not os.path.isfile(filename): logger.error( "can't find default either, something's wrong, is the duckiefleet root correctly set?" ) else: data = yaml_load_file(filename) else: rospy.loginfo("Loading some kinematic calibration parameters of " + self.robot_name) data = yaml_load_file(filename) logger.info("Loaded homography for {}".format( os.path.basename(filename))) # Load some of the parameters that will not be changed param_k = data['k'] param_limit = data['limit'] param_radius = data['radius'] baseline = data['baseline'] # simply to increase readability gain = self.fit_['gain'] trim = self.fit_['trim'] # Write to yaml #datasave = { # This is similar to the inverse_kinematics_node, but it did not work... # "calibration_time": time.strftime("%Y-%m-%d-%H-%M-%S"), # "gain": gain, # "trim": trim, # "baseline": baseline, # "radius": param_radius, # "k": param_k, # "limit": param_limit, #} datasave = \ "calibration_time: {}".format(time.strftime("%Y-%m-%d-%H-%M-%S")) + \ "\ngain: {}".format(gain) + \ "\ntrim: {}".format(trim) + \ "\nbaseline: {}".format(baseline) + \ "\nradius: {}".format(param_radius) + \ "\nk: {}".format(param_k) + \ "\nlimit: {}".format(param_limit) print("\nThe Estimated Kinematic Calibration is:") print("gain = {}".format(gain)) print("trim = {}".format(trim)) # Write to yaml file filename = (home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml") with open(filename, 'w') as outfile: outfile.write(datasave) #outfile.write(yaml.dump(datasave, default_flow_style=False)) # This did not work and gave very weird results print("Saved Parameters to " + self.robot_name + ".yaml") print( "\nPlease check the plots and judge if the parameters are reasonable." ) print( "Once done inspecting the plot, close them to terminate the program." )
def value_as_float(x): try: return float(x) except Exception as e: logger.error('Cannot convert to float %r: %s' % (x, e)) raise