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
