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
