def load_camera_info(self): '''Load camera intrinsics''' filename = (sys.path[0] + "/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 = (sys.path[0] + "/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 load_all_logs(which='*'): pattern = which + '.bag' basename2filename = look_everywhere_for_bag_files(pattern=pattern) logs = OrderedDict() for basename, filename in basename2filename.items(): log_name = basename if not is_valid_name(basename): msg = 'Ignoring Bag file with invalid file name "%r".' % (basename) msg += '\n Full path: %s' % filename logger.warn(msg) continue date = None size = os.stat(filename).st_size l = PhysicalLog(log_name=log_name, map_name=None, description=None, length=None, t0=None, t1=None, date=date, size=size, has_camera=None, vehicle=None, filename=filename, bag_info=None, valid=True, error_if_invalid=None) l = read_stats(l) logs[l.log_name] = l return logs
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 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 __init__(self): # defaults overwritten by param self.robot_name = "shamrock" self.rectified_input = False # read calibrations self.extrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/" + self.robot_name + ".yaml") if not os.path.isfile(extrinsics_filename): logger.warn("no robot specific extrinsic calibration, trying default") alternate_extrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/default.yaml") if not os.path.isfile(alternate_extrinsics_filename): logger.warn("can't find default either, something's wrong") else: self.H = self.load_homography(alternate_extrinsics_filename) else: self.H = self.load_homography(self.extrinsics_filename) self.H_inv = np.linalg.inv(self.H) intrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_intrinsics/" + self.robot_name + ".yaml") if not os.path.isfile(intrinsics_filename): logger.warn("no robot specific calibration, trying default") intrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_extrinsic/default.yaml") if not os.path.isfile(extrinsics_filename): logger.error("can't find default either, something's wrong") self.ci_ = self.load_camera_info(intrinsics_filename) self.pcm_ = PinholeCameraModel() self.pcm_.fromCameraInfo(self.ci)
def look_everywhere_for_bag_files(pattern='*.bag'): """ Looks for all the bag files Returns a list of basename -> filename. """ sources = [] # We look in $DUCKIETOWN_ROOT/catkin_ws/src # sources.append(get_catkin_ws_src()) # then we look in $DUCKIETOWN_FLEET sources.append(get_duckiefleet_root()) sources.append(get_duckietown_data()) # downloads p = get_duckietown_local_log_downloads() if os.path.exists(p): sources.append(p) logger.debug('Looking for files with pattern %s...' % pattern) results = OrderedDict() for s in sources: filenames = locate_files(s, pattern) logger.debug('%5d files in %s' % (len(filenames), friendly_path(s))) for filename in filenames: basename, _ = os.path.splitext(os.path.basename(filename)) if basename in results: one = filename two = results[basename] if not same_file_content(one, two): msg = 'Two bags with same name but different content:\n%s\n%s' %(one, two) raise DTConfigException(msg) else: msg = 'Two copies of bag found:\n%s\n%s' %(one, two) logger.warn(msg) continue results[basename] = filename return results