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