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 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_camera_info(self, filename): 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'] return cam_info
def parse_calibration_yaml(self, calib_file): params = yaml_load_file(calib_file) #with file(calib_file, 'r') as f: # params = yaml.load(f) self.pcm_ = CameraInfo() self.pcm_.height = params['image_height'] self.pcm_.width = params['image_width'] self.pcm_.distortion_model = params['distortion_model'] self.pcm_.K = params['camera_matrix']['data'] self.pcm_.D = params['distortion_coefficients']['data'] self.pcm_.R = params['rectification_matrix']['data'] self.pcm_.P = params['projection_matrix']['data']
def load_board_info(self, filename=''): '''Load calibration checkerboard info''' if not os.path.isfile(filename): filename = get_ros_package_path('ground_projection') + '/config/ground_projection_node/default.yaml' target_data = yaml_load_file(filename) target_info = { 'width': target_data['board_w'], 'height': target_data['board_h'], 'square_size': target_data['square_size'], 'x_offset': target_data['x_offset'], 'y_offset': target_data['y_offset'], 'offset': np.array([target_data['x_offset'], -target_data['y_offset']]), 'size': (target_data['board_w'], target_data['board_h']), } logger.info("Loaded checkerboard parameters") return target_info
def load_homography(self, robotname): ''' Function to load homography file for ground projection. Input: - robotname: e.g., "default" if using simulator Output: - homo_mat: homography matrix (3 x 3 ndarray) ''' if robotname == "default": homo_path = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/default.yaml" ) # simulator else: homo_path = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + robotname + ".yaml") # real duckiebot data = yaml_load_file(homo_path) homo_mat = np.array(data['homography']).reshape((3, 3)) # homo_mat = np.array([-1.27373e-05, -0.0002421778, -0.1970125, 0.001029818, -1.578045e-05, -0.337324, -0.0001088811, -0.007584862, 1.]).reshape((3, 3)) return homo_mat
def _load_homography(self, filename): data = yaml_load_file(filename) return np.array(data['homography']).reshape((3, 3))