class WorldProjector(object): def __init__(self, camera_info, h_matrix, distorted_input=True): 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.""" # 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._project_normalized_pixel(pixel_normalized) return point def _project_normalized_pixel(self, pixel_normalized): # create homogeneous coordinate pixel_coord = np.append(pixel_normalized, 1.0) # project to world ground_coord = np.dot(self.h_matrix, pixel_coord) # scale the coordinates appropriately -> transform to eucledian space ground_coord = ground_coord / ground_coord[2] return ground_coord 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)
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