class GroundProjection(): def __init__(self, robot_name="neo"): # defaults overwritten by param self.robot_name = robot_name self.rectified_input = False # Load homography self.H = self.load_homography() self.Hinv = np.linalg.inv(self.H) self.pcm_ = PinholeCameraModel() # Load checkerboard information self.board_ = self.load_board_info() # wait until we have recieved the camera info message through ROS and then initialize def initialize_pinhole_camera_model(self, camera_info): self.ci_ = camera_info self.pcm_.fromCameraInfo(camera_info) print("pinhole camera model initialized") def vector2pixel(self, vec): pixel = Pixel() cw = self.ci_.width ch = self.ci_.height pixel.u = cw * vec.x pixel.v = ch * vec.y if (pixel.u < 0): pixel.u = 0 if (pixel.u > cw - 1): pixel.u = cw - 1 if (pixel.v < 0): pixel.v = 0 if (pixel.v > ch - 1): pixel.v = 0 return pixel def pixel2vector(self, pixel): vec = Vector2D() vec.x = pixel.u / self.ci_.width vec.y = pixel.v / self.ci_.height return vec def vector2ground(self, vec): pixel = self.vector2pixel(vec) return self.pixel2ground(pixel) def ground2vector(self, point): pixel = self.ground2pixel(point) return self.pixel2vector(pixel) def pixel2ground(self, pixel): uv_raw = np.array([pixel.u, pixel.v]) if not self.rectified_input: uv_raw = self.pcm_.rectifyPoint(uv_raw) #uv_raw = [uv_raw, 1] uv_raw = np.append(uv_raw, np.array([1])) ground_point = np.dot(self.H, uv_raw) point = Point() x = ground_point[0] y = ground_point[1] z = ground_point[2] point.x = x / z point.y = y / z point.z = 0.0 return point def ground2pixel(self, point): ground_point = np.array([point.x, point.y, 1.0]) image_point = np.dot(self.Hinv, ground_point) image_point = image_point / image_point[2] pixel = Pixel() if not self.rectified_input: distorted_pixel = self.pcm_.project3dToPixel(image_point) pixel.u = distorted_pixel[0] pixel.v = distorted_pixel[1] else: pixel.u = image_point[0] pixel.v = image_point[1] def rectify(self, cv_image_raw): '''Undistort image''' cv_image_rectified = np.zeros(np.shape(cv_image_raw)) mapx = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1), dtype='float32') mapy = np.ndarray(shape=(self.pcm_.height, self.pcm_.width, 1), dtype='float32') mapx, mapy = cv2.initUndistortRectifyMap( self.pcm_.K, self.pcm_.D, self.pcm_.R, self.pcm_.P, (self.pcm_.width, self.pcm_.height), cv2.CV_32FC1, mapx, mapy) return cv2.remap(cv_image_raw, mapx, mapy, cv2.INTER_CUBIC, cv_image_rectified) 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 # 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 = sys.path[ 0] + "/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(self): '''Load homography (extrinsic parameters)''' filename = (sys.path[0] + "/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 = (sys.path[0] + "/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: rospy.loginfo("Using extrinsic calibration of " + self.robot_name) data = yaml_load_file(filename) logger.info("Loaded homography for {}".format( os.path.basename(filename))) return np.array(data['homography']).reshape((3, 3)) def write_homography(self, filename): ob = {'homography': sum(self.H.reshape(9, 1).tolist(), [])} print ob yaml_write_to_file(ob, filename) def load_board_info(self, filename=''): '''Load calibration checkerboard info''' if not os.path.isfile(filename): filename = get_ros_package_path( 'duckietown' ) + '/config/baseline/ground_projection/ground_projection/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 ####################################################### # OLD STUFF # ####################################################### 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_homography(self, filename): data = yaml_load_file(filename) return np.array(data['homography']).reshape((3, 3)) 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 _rectify(self, cv_image_raw): '''old''' #cv_image_rectified = cvMat() cv_image_rectified = np.zeros(np.shape(cv_image_raw)) # TODO: debug PinholeCameraModel() self.pcm_.rectifyImage(cv_image_raw, cv_image_rectified) return cv_image_rectified
class GroundProjector(object): def __init__(self, camera_info, h_matrix, distorted_input=True): """ The GroundProjector can rectify pixels or the whole image and the ground coordinates of a pixel by using the homography matrix H Args: camera_info: h_matrix: Homography matrix, that maps normalized undistorted pixel coordinates into the ground plane. distorted_input: Should only be False if simulation without simulated camera distortion is used. """ self.distorted_input = distorted_input self.camera_info = camera_info self.fisheye = False if camera_info.distortion_model == "fisheye": self.camera = FisheyeCameraModel() self.camera.from_camera_info(camera_info) self.fisheye = True else: self.camera = PinholeCameraModel() self.camera.fromCameraInfo(camera_info) self.h_matrix = h_matrix def pixel2ground(self, pixel_coord, distorted_input=None): """Returns the ground projection of a pixel. For distorted input the undistorted coordinates of the pixel will be calculcated. Afterwards the pixel will get noramlized and projected to the ground plane by applying the homography matrix. Args: pixel_coord (tuple/list/numpy.ndarray): Pixel coordinates of the point that will be projected into the ground plane. distorted_input (bool): Can be set to override the default value defined when creating the instance. Returns: Coordinates of the pixel projected into the ground plane expressed in the vehicles coordinate system. """ # use default value if 'distorted_input' is not set if distorted_input is None: distorted_input = self.distorted_input if distorted_input: pixel_coord = self.rectify_pixel(pixel_coord) pixel_coord = np.array(pixel_coord) # normalize coordinates pixel_normalized = normalize_image_coordinates( pixel_coord, self.camera.width, self.camera.height) point = self._fake_project(pixel_normalized) return point def _project_normalized_pixel(self, pixel_normalized): """ TODO: WRITE CODE TO COMPUTE THE PIXEL'S CORRESPONDING GROUND PLANE POINT Args: pixel_normalized: Normalized pixel coordinate Returns: 3D-coordinates of the projection expressed in the vehicle's frame """ point = Point32() # YOUR CALCULATION NEEDS TO BE DONE HERE. REPLACE THE ASSIGNMENT # OF THE POINT'S COORDINATES BY REASONABLE ASSIGNMENTS OF YOUR GROUND # PROJECTION. point.x = 0.0 point.y = 0.0 point.z = 0.0 return point def _fake_project(self, pixel_normalized): """ THIS METHOD IS JUST A DUMMY. REPLACE ALL CALLS OF THIS METHOD BY '_project_normalized_pixel()' Args: pixel_normalized: Returns: """ point = Point32() point.x = 1.0 - pixel_normalized[1] point.y = 0.5 - pixel_normalized[0] point.z = 0.0 return point def rectify_pixel(self, pixel_coord): """Calculates the rectified undistorted image coordinate of a pixel. Args: pixel_coord: Distorted pixel coordinates Returns: Undistorted pixel coordinates """ if self.fisheye: return self.camera.undistort_point(pixel_coord) else: return self.camera.rectifyPoint(pixel_coord) def rectify_image(self, img): if self.fisheye: return self.camera.undistort_image(img) else: output = np.empty_like(img) self.camera.rectifyImage(img, output) return output
class BirdseyeNode(DTROS): def __init__(self, node_name): # Initialize the DTROS parent class super(BirdseyeNode, self).__init__(node_name=node_name) self.veh_name = rospy.get_namespace().strip("/") # XXX: do we really need a member variable for every field in the dict? self.verbose = None self.parameters['~verbose'] = None self.rectify = None self.parameters['~rectify'] = None self.parameters['~image_size'] = None self.updateParameters() self.refresh_parameters() self.pcm = None # initialized when CameraInfo is received self.bridge = CvBridge() self.active = True # Load default camera calibration H = get_homography_for_robot(self.veh_name) cam_info = get_camera_info_for_robot(self.veh_name) self.scaled_homography = ScaledHomography(H, cam_info.height, cam_info.width) # Scale the homography based on actual resolution requested from camera_node self.image_size_height = self.parameters['~image_size']['height'] self.image_size_width = self.parameters['~image_size']['width'] self.scaled_homography.update_homography(self.image_size_height, self.image_size_width) rospy.set_param('/{}/camera_node/res_w'.format(self.veh_name), self.image_size_width) rospy.set_param('/{}/camera_node/res_h'.format(self.veh_name), self.image_size_height) self.log('Waiting for CameraInfo matching requested resolution: %dx%d'%(self.image_size_width,self.image_size_height)) # Subscribers self.sub_switch = self.subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1) self.sub_camera_info = self.subscriber('~camera_info', CameraInfo, self.cb_camera_info, queue_size=1) self.sub_image_in = None # initialized when CameraInfo is received # Publishers self.pub_rectified = self.publisher('~verbose/rectified/compressed', CompressedImage, queue_size=1) self.pub_image_out = self.publisher('~image_out/compressed', CompressedImage, queue_size=1) def cb_camera_info(self, msg): # wait for the camera_node to switch to the requested resolution if self.image_size_width != msg.width or self.image_size_height != msg.height: return self.log('Received updated camera info.', 'info') # This topic subscription is only needed initially, so it can be unregistered. self.sub_camera_info.unregister() self.pcm = PinholeCameraModel() self.pcm.fromCameraInfo(msg) buffer_size = msg.width * msg.height * 3 * 2 # 2 is a safety factor self.log('Buffer size set to {}.'.format(buffer_size), 'info') # Now the node can proceed to process images self.sub_image_in = self.subscriber('~image_in/compressed', CompressedImage, self.cb_image_in, queue_size=1, buff_size=buffer_size) self.log('Initialized.') def cbSwitch(self, switch_msg): self.log('Switch ' + str(switch_msg.data)) self.active = switch_msg.data def cb_image_in(self, msg): if self.active and not self.is_shutdown: if self.verbose: self.log('Received image.', 'info') if self.parametersChanged: self.log('Parameters changed.', 'info') self.refresh_parameters() self.parametersChanged = False img = utils.read_image(msg) if img is None: self.log('Got empty image msg.') height, width, depth = img.shape if self.rectify: img_temp = img.copy() self.pcm.rectifyImage(img_temp, img_temp) img = img_temp if self.verbose: utils.publish_image(self.bridge, self.pub_rectified, img) img_out = self.camera_img_to_birdseye(img) utils.publish_image(self.bridge, self.pub_image_out, img_out, msg.header) def camera_img_to_birdseye(self, cam_img): # Update homography based on image size height, width, _ = cam_img.shape if height != self.image_size_height: self.scaled_homography.update_homography(self.image_size_height, self.image_size_width) H = self.scaled_homography.for_image() # Apply image transformation # TODO: Test different flags (https://docs.opencv.org/3.1.0/da/d54/group__imgproc__transform.html#ga5bb5a1fea74ea38e1a5445ca803ff121 ) birdseye_img = cv2.warpPerspective(cam_img, H, (width, height), flags=cv2.INTER_LINEAR+cv2.WARP_FILL_OUTLIERS) return birdseye_img def refresh_parameters(self): self.verbose = self.parameters['~verbose'] self.rectify = self.parameters['~rectify'] def onShutdown(self): self.log("Stopping birdseye_node.") super(BirdseyeNode, self).onShutdown()
class ImageProcessor(object): def __init__(self): self.coordinate_publisher = rospy.Publisher('cv/object_coordinates', PointStamped) self._cam_info_sub = rospy.Subscriber('/pico_flexx/camera_info', CameraInfo, self.camera_info) self._camera_model = PinholeCameraModel() self._bridge = CvBridge() self.database_path = ["hu_mug.db", "hu_wine.db"] self.classifier = ImageClassifier(self.database_path) # Image containers for use outside of object self.ir_image_rect = None self.latest_point_cloud = None def camera_info(self, camera_info): rospy.loginfo(rospy.get_caller_id() + " :: PROJECTION MATRIX: %s", camera_info.P) self._camera_model.fromCameraInfo(camera_info) # Kills the subscriber, camera intrinsics are constant so 1 message is enough self._cam_info_sub.unregister() def undistort(self, src, dst): try: self._camera_model.rectifyImage(src, dst) return True except Exception: print(Exception) return False def ir_callback(self, image_frame): try: img = self._bridge.imgmsg_to_cv2(image_frame, desired_encoding='passthrough') self.ir_image_rect = np.zeros((img.shape[0], img.shape[1]), dtype='uint8') assert self.undistort( img, self.ir_image_rect), "Undistortion of IR img failed" result, segmented, objects, object_types = self.classifier.evaluate( self.ir_image_rect) # self.classifier.show(result, 500, 0, self.ir_image_rect) coordinates = self.get_position(objects) for i in range(0, len(coordinates)): message = PointStamped() message.point.x = coordinates[i][0] message.point.y = coordinates[i][1] message.point.z = coordinates[i][ 2] + 0.04 # +0.04 to approximate the center of objects instead of the edge message.header.frame_id = object_types[i] self.coordinate_publisher.publish(message) # self.save_image(segmented, result, path='images/ir_images/') except CvBridgeError: print(CvBridgeError) def pcl_callback(self, point_cloud): self.latest_point_cloud = point_cloud return def save_image(self, img, img_rect, path='images/'): timestamp = str(datetime.now()) try: ret1 = cv2.imwrite(path + 'raw/{}.jpg'.format(timestamp), img) ret2 = cv2.imwrite( path + 'rectified/{}-rect.jpg'.format(timestamp), img_rect) assert ret1 and ret2, "Failed saving images" rospy.loginfo(rospy.get_caller_id() + ": Images saved") except SystemError as e: print(e) def get_position(self, image_objects): try: object_coordinates = [] for image_object in image_objects: m = cv2.moments(image_object) cx = int(m['m10'] / m['m00']) cy = int(m['m01'] / m['m00']) pcl = pc2.read_points(self.latest_point_cloud, field_names=("x", "y", "z"), skip_nans=True, uvs=[[cx, cy]]) for point in pcl: print(point[0], point[1], point[2]) object_coordinates.append([point[0], point[1], point[2]]) return object_coordinates except AssertionError as e: print(e) return []