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):
self.node_name = rospy.get_name()
# Load parameters
self.config = self.setupParam("~config", "baseline")
# TODO cali_file_name is not needed and should be the robot name by default
self.cali_file_name = self.setupParam("~cali_file_name", "default")
self.image_type = self.setupParam("~image_type", "compressed")
# Setup publisher
self.pub_camera_info = rospy.Publisher("~camera_info",
CameraInfo,
queue_size=1)
# Get path to calibration yaml file
self.cali_file = (get_duckiefleet_root() +
"/calibrations/camera_intrinsic/" +
self.cali_file_name + ".yaml")
self.camera_info_msg = None
# Load calibration yaml file
if not os.path.isfile(self.cali_file):
rospy.logwarn(
"[%s] Can't find calibration file: %s.\nUsing default calibration instead."
% (self.node_name, self.cali_file))
self.cali_file = (get_duckiefleet_root() +
"/calibrations/camera_intrinsic/default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(self.cali_file):
rospy.signal_shutdown("Found no calibration file ... aborting")
# Print out and prepare message
rospy.loginfo("[%s] Using calibration file: %s" %
(self.node_name, self.cali_file))
self.camera_info_msg = load_camera_info(self.cali_file)
self.camera_info_msg.header.frame_id = rospy.get_namespace(
) + "camera_optical_frame"
rospy.loginfo("[%s] CameraInfo: %s" %
(self.node_name, self.camera_info_msg))
# self.timer_pub = rospy.Timer(rospy.Duration.from_sec(1.0/self.pub_freq),self.cbTimer)
img_type = CompressedImage if self.image_type == "compressed" else Image
typemsg = "CompressedImage" if self.image_type == "compressed" else "Image"
rospy.logwarn("[%s] ==============%s", self.node_name, typemsg)
self.sub_img_compressed = rospy.Subscriber("~compressed_image",
img_type,
self.cbCompressedImage,
queue_size=1)
def get_homography_info_config_file(robot_name):
strict = False
roots = [
os.path.join(dtu.get_duckiefleet_root(), 'calibrations'),
os.path.join(dtu.get_ros_package_path('duckietown'), 'config',
'baseline', 'calibration')
]
found = []
for df in roots:
# Load camera information
fn = os.path.join(df, 'camera_extrinsic', robot_name + '.yaml')
fn_default = os.path.join(df, 'camera_extrinsic', 'default.yaml')
if os.path.exists(fn):
found.append(fn)
dtu.logger.info("Using filename %s" % fn)
elif os.path.exists(fn_default):
found.append(fn_default)
dtu.logger.info("Using filename %s" % fn_default)
if len(found) == 0:
msg = 'Cannot find homography file for robot %r;\n%s' % (robot_name,
roots)
raise NoHomographyInfoAvailable(msg)
elif len(found) == 1:
return found[0]
else:
msg = 'Found more than one configuration file: \n%s' % "\n".join(found)
msg += "\n Please delete one of those."
if strict:
raise Exception(msg)
else:
dtu.logger.error(msg)
return found[0]
def get_images():
found = locate_files(get_duckiefleet_root(), '*.jpg')
basename2filename = dict((os.path.basename(_), _) for _ in found)
if not nopic in basename2filename:
msg = 'I expect a file %r to represent missing pictures.' % nopic
raise DTConfigException(msg)
return basename2filename
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 check(self):
try:
d = dtu.get_duckiefleet_root()
return "Duckiefleet root is detected as %r" % d
except dtu.DTConfigException as e:
msg = 'Could not get the duckiefleet root in any way.'
dtu.raise_wrapped(CheckFailed, e, msg)
def get_homography_info_config_file(robot_name, strict=False):
roots = [
os.path.join(dtu.get_duckiefleet_root(), 'calibrations'),
os.path.join(dtu.get_ros_package_path('duckietown'),
'config', 'baseline', 'calibration')
]
found = []
for root in roots:
fn = os.path.join(root, 'camera_extrinsic', '{0}.yaml')
for name in [robot_name, 'default']:
filename = fn.format(name)
if os.path.exists(filename):
found.append(filename)
dtu.logger.info('Using filename {0}'.format(filename))
if not found:
raise NoHomographyInfoAvailable('Cannot find homography file for robot '
'`{0}` in folders\n{1}'.format(
robot_name, '\n'.join(roots)))
else:
if len(found) > 1:
error_message = ('Found more than one configuration file:\n{0}\n'
'Please delete one of those'.format('\n'.join(found)))
if strict:
raise Exception(error_message)
else:
dtu.logger.error(error_message)
return found[0]
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 load_homography(veh):
path = '/calibrations/camera_'
filename = get_duckiefleet_root() + path + 'extrinsic/' + veh + '.yaml'
if not isfile(filename):
print('Extrinsic calibration for {} does not exist.'.format(veh))
exit(2)
with open(filename) as f:
contents = f.read()
data = yaml_load(contents)
logger.info('Loaded homography for {}'.format(veh))
return np.array(data['homography']).reshape(3,3)
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 __init__(self):
self.node_name = "Vehicle Filter Test"
self.bridge = CvBridge()
self.cali_file_name = self.setupParam("~cali_file_name", "default")
self.cali_file = (get_duckiefleet_root() +
"/calibrations/camera_intrinsic/" +
self.cali_file_name + ".yaml")
if not os.path.isfile(self.cali_file):
rospy.logwarn(
"[%s] Can't find calibration file: %s.\nUsing default calibration instead."
% (self.node_name, self.cali_file))
self.cali_file = (get_duckiefleet_root() +
"/calibrations/camera_intrinsic/default.yaml")
if not os.path.isfile(self.cali_file):
rospy.signal_shutdown("Found no calibration file ... aborting")
rospy.loginfo("[%s] Using calibration file: %s" %
(self.node_name, self.cali_file))
self.camera_info_msg = load_camera_info(self.cali_file)
self.camera_info_msg.header.frame_id = rospy.get_namespace(
) + "camera_optical_frame"
rospy.loginfo("[%s] CameraInfo: %s" %
(self.node_name, self.camera_info_msg))
self.sub_pose = rospy.Subscriber("~pose",
VehiclePose,
self.cbPose,
queue_size=1)
self.pub_corners = rospy.Publisher("~corners",
VehicleCorners,
queue_size=1)
self.pub_camera_info = rospy.Publisher("~camera_info",
CameraInfo,
queue_size=1)
rospy.loginfo("Initialization of [%s] completed" % (self.node_name))
pub_period = rospy.get_param("~pub_period", 1.0)
rospy.Timer(rospy.Duration.from_sec(pub_period), self.pubCorners)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.framerate_high = self.setupParam("~framerate_high", 30.0)
self.framerate_low = self.setupParam("~framerate_low", 15.0)
self.res_w = self.setupParam("~res_w", 640)
self.res_h = self.setupParam("~res_h", 480)
self.image_msg = CompressedImage()
# Setup PiCamera
self.k = rospy.get_param("~k")
self.reduction = rospy.get_param("~reduction")
self.camera = PiCamera()
self.framerate = self.framerate_high # default to high
self.camera.framerate = self.framerate
self.camera.resolution = (self.res_w, self.res_h)
# For intrinsic calibration
self.cali_file_folder = get_duckiefleet_root(
) + "/calibrations/camera_intrinsic/"
self.frame_id = rospy.get_namespace().strip(
'/') + "/camera_optical_frame"
self.has_published = False
self.pub_img = rospy.Publisher("~image/compressed",
CompressedImage,
queue_size=1)
self.sub_switch_high = rospy.Subscriber("~framerate_high_switch",
BoolStamped,
self.cbSwitchHigh,
queue_size=1)
# Create service (for camera_calibration)
self.srv_set_camera_info = rospy.Service("~set_camera_info",
SetCameraInfo,
self.cbSrvSetCameraInfo)
self.stream = io.BytesIO()
#self.camera.exposure_mode = 'off'
# self.camera.awb_mode = 'off'
self.is_shutdown = False
self.update_framerate = False
# Setup timer
rospy.loginfo("[%s] Initialized." % (self.node_name))
def get_camera_info_config_file(robot_name):
roots = [os.path.join(dtu.get_duckiefleet_root(), 'calibrations'),
os.path.join(dtu.get_ros_package_path('duckietown'), 'config', 'baseline', 'calibration')]
for df in roots:
# Load camera information
fn = os.path.join(df, 'camera_intrinsic', robot_name + '.yaml')
if os.path.exists(fn):
return fn
else:
print('%s does not exist' % fn)
msg = 'Cannot find intrinsic file for robot %r;\n%s' % (robot_name, roots)
raise NoCameraInfoAvailable(msg)
def load_camera_intrinsics(veh):
path = '/calibrations/camera_'
filename = get_duckiefleet_root() + path + 'intrinsic/' + veh + '.yaml'
if not isfile(filename):
print('Intrinsic calibration for {} does not exist.'.format(veh))
exit(3)
with open(filename) as f:
contents = f.read()
data = yaml_load(contents)
intrinsics = {}
intrinsics['K'] = np.array(data['camera_matrix']['data']).reshape(3, 3)
intrinsics['D'] = np.array(data['distortion_coefficients']['data']).reshape(1, 5)
intrinsics['R'] = np.array(data['rectification_matrix']['data']).reshape(3, 3)
intrinsics['P'] = np.array(data['projection_matrix']['data']).reshape((3,4))
intrinsics['distortion_model'] = data['distortion_model']
logger.info('Loaded camera intrinsics for {}'.format(veh))
return intrinsics
def search_all_configuration_files():
sources = []
# We look in $DUCKIETOWN_ROOT/catkin_ws/src
sources.append(dtu.get_catkin_ws_src())
# then we look in $DUCKIETOWN_FLEET
sources.append(dtu.get_duckiefleet_root())
configs = []
pattern = '*' + SUFFIX
dtu.logger.info('Looking for %s files.' % pattern)
for s in sources:
fs = dtu.locate_files(s, pattern)
dtu.logger.info('%4d files in %s' % (len(fs), s))
configs.extend(fs)
# logger.debug('I found:\n' + "\n".join(configs))
return configs
def getFilePath(self, name):
return (get_duckiefleet_root() + '/calibrations/kinematics/' + name +
".yaml")
def get_extrinsics_filename(robot_name):
fn = dtu.get_duckiefleet_root(
) + "/calibrations/camera_extrinsic/" + robot_name + ".yaml"
return fn
