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 []
