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))
