def test_monocular(self):
ci = sensor_msgs.msg.CameraInfo()
ci.width = 640
ci.height = 480
print ci
cam = PinholeCameraModel()
cam.fromCameraInfo(ci)
print cam.rectifyPoint((0, 0))
print cam.project3dToPixel((0,0,0))
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)
def make_pixel_to_3d(self):
cm = PinholeCameraModel()
cm.fromCameraInfo(self.camera_info)
self.p3d = np.zeros(
[self.camera_info.height, self.camera_info.width, 3])
for v in range(0, self.camera_info.height):
for u in range(0, self.camera_info.width):
uv_rect = cm.rectifyPoint((u, v))
p = cm.projectPixelTo3dRay(uv_rect)
self.p3d[v, u, :] = (p[0] / p[2], p[1] / p[2], 1.0
) # make homogeneous
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 GroundProjection():
def __init__(self, robot_name="birdbot0"):
# 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 received 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 = int(cw * vec.x)
pixel.v = int(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 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:
#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))
"""
# hardcorded birdbot0 homography
#return np.array([[-1.3138222289537473e-05,-0.00016799247320246641, -0.18508764249409215],
# [0.0008702503150508597, -1.314730233433855e-06, -0.2779744199471874],
# [-7.940529271577331e-05,-0.006045110837995103,1.0]])
return np.array([[-5.987554218305854e-05,-0.00012542467699075597, -0.183339048091576],
[0.0008439191581241052, -3.098547600170272e-05, -0.28159137198032724],
[-0.00015406069103711482,-0.006343978853492832, 0.9999999999999999]])
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 Rectify:
"""
Handles the Rectification operations.
"""
ci: CameraInfo
pcm: PinholeCameraModel
_rectify_inited: bool
_distort_inited: bool
rmapx: np.ndarray
rmapy: np.ndarray
def __init__(self, camera_info: CameraInfo):
self.ci = camera_info
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(self.ci)
self._rectify_inited = False
self._distort_inited = False
def rectify_point(self, pixel: Point) -> Point:
p = (pixel.x, pixel.y)
return Point(*list(self.pcm.rectifyPoint(p)))
def _init_rectify_maps(self):
W = self.pcm.width
H = self.pcm.height
mapx = np.ndarray(shape=(H, W, 1), dtype="float32")
mapy = np.ndarray(shape=(H, W, 1), dtype="float32")
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(W, H), cv2.CV_32FC1, mapx,
mapy)
self.mapx = mapx
self.mapy = mapy
self._rectify_inited = True
def rectify(self,
cv_image_raw: np.ndarray,
interpolation=cv2.INTER_NEAREST):
"""Undistort an image.
To be more precise, pass interpolation= cv2.INTER_CUBIC
"""
if not self._rectify_inited:
self._init_rectify_maps()
#
# inter = cv2.INTER_NEAREST # 30 ms
# inter = cv2.INTER_CUBIC # 80 ms
# cv_image_rectified = np.zeros(np.shape(cv_image_raw))
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
cv_image_rectified)
return res
def distort(self, rectified: np.ndarray) -> np.ndarray:
if not self._rectify_inited:
self._init_rectify_maps()
if not self._distort_inited:
self.rmapx, self.rmapy = invert_map(self.mapx, self.mapy)
self._distort_inited = True
distorted = np.zeros(np.shape(rectified))
res = cv2.remap(rectified, self.rmapx, self.rmapy, cv2.INTER_NEAREST,
distorted)
return res
def rectify_full(self,
cv_image_raw: np.ndarray,
interpolation=cv2.INTER_NEAREST,
ratio=1):
"""
Undistort an image by maintaining the proportions.
To be more precise, pass interpolation= cv2.INTER_CUBIC
Returns the new camera matrix as well.
"""
W = int(self.pcm.width * ratio)
H = int(self.pcm.height * ratio)
# mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
# mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
print(f"K: {self.pcm.K}")
print(f"P: {self.pcm.P}")
# alpha = 1
# new_camera_matrix, validPixROI = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D, (H,
# W), alpha)
# print('validPixROI: %s' % str(validPixROI))
# Use the same camera matrix
new_camera_matrix = self.pcm.K.copy()
new_camera_matrix[0, 2] = W / 2
new_camera_matrix[1, 2] = H / 2
print(f"new_camera_matrix: {new_camera_matrix}")
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, new_camera_matrix,
(W, H), cv2.CV_32FC1)
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, mapx, mapy, interpolation,
cv_image_rectified)
return new_camera_matrix, res
class Rectify:
"""
Handles the Rectification operations.
"""
def __init__(self, camera_info):
self.ci = camera_info
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(self.ci)
self._rectify_inited = False
self._distort_inited = False
def rectify_point(self, pixel):
return Point(*list(self.pcm.rectifyPoint([pixel.x, pixel.y])))
def _init_rectify_maps(self):
W = self.pcm.width
H = self.pcm.height
mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D, self.pcm.R,
self.pcm.P, (W, H),
cv2.CV_32FC1, mapx, mapy)
self.mapx = mapx
self.mapy = mapy
self._rectify_inited = True
def rectify(self, cv_image_raw, interpolation=cv2.INTER_NEAREST):
''' Undistort an image.
To be more precise, pass interpolation= cv2.INTER_CUBIC
'''
if not self._rectify_inited:
self._init_rectify_maps()
#
# inter = cv2.INTER_NEAREST # 30 ms
# inter = cv2.INTER_CUBIC # 80 ms
# cv_image_rectified = np.zeros(np.shape(cv_image_raw))
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
cv_image_rectified)
return res
def distort(self, rectified):
if not self._rectify_inited:
self._init_rectify_maps()
if not self._distort_inited:
self.rmapx, self.rmapy = invert_map(self.mapx, self.mapy)
self._distort_inited = True
distorted = np.zeros(np.shape(rectified))
res = cv2.remap(rectified, self.rmapx, self.rmapy, cv2.INTER_NEAREST, distorted)
return res
def rectify_full(self, cv_image_raw, interpolation=cv2.INTER_NEAREST, ratio=1):
'''
Undistort an image by maintaining the proportions.
To be more precise, pass interpolation= cv2.INTER_CUBIC
Returns the new camera matrix as well.
'''
W = int(self.pcm.width * ratio)
H = int(self.pcm.height * ratio)
# mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
# mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
# print('K: %s' % self.pcm.K)
# print('P: %s' % self.pcm.P)
# alpha = 1
# new_camera_matrix, validPixROI = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D, (H, W), alpha)
# print('validPixROI: %s' % str(validPixROI))
# Use the same camera matrix
new_camera_matrix = self.pcm.K.copy()
new_camera_matrix[0, 2] = W / 2
new_camera_matrix[1, 2] = H / 2
# print('new_camera_matrix: %s' % new_camera_matrix)
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D, self.pcm.R,
new_camera_matrix, (W, H),
cv2.CV_32FC1)
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, mapx, mapy, interpolation,
cv_image_rectified)
return new_camera_matrix, res
def invert_map(mapx, mapy):
H, W = mapx.shape[0:2]
rmapx = np.empty_like(mapx)
rmapx.fill(np.nan)
rmapy = np.empty_like(mapx)
rmapy.fill(np.nan)
for y, x in itertools.product(range(H), range(W)):
tx = mapx[y, x]
ty = mapy[y, x]
tx = int(np.round(tx))
ty = int(np.round(ty))
if (0 <= tx < W) and (0 <= ty < H):
rmapx[ty, tx] = x
rmapy[ty, tx] = y
# fill holes
# if False:
fill_holes(rmapx, rmapy)
# D = 4
# for y, x in itertools.product(range(H), range(W)):
# v0 = max(y-D, 0)
# v1 = max(y+D, H-1)
# u0 = max(x-D, 0)
# u1 = max(x+D, W-1)
#
# rmapx[y,x] = np.median(rmapx[v0:v1,u0:u1].flatten())
# rmapy[y,x] = np.median(rmapy[v0:v1,u0:u1].flatten())
return rmapx, rmapy
def fill_holes(rmapx, rmapy):
H, W = rmapx.shape[0:2]
nholes = 0
R = 2
F = R * 2 + 1
def norm(x):
return np.hypot(x[0], x[1])
deltas0 = [(i - R - 1, j - R - 1) for i, j in itertools.product(range(F), range(F))]
deltas0 = [x for x in deltas0 if norm(x) <= R]
deltas0.sort(key=norm)
def get_deltas():
# deltas = list(deltas0)
#
return deltas0
holes = set()
for i, j in itertools.product(range(H), range(W)):
if np.isnan(rmapx[i, j]):
holes.add((i, j))
while holes:
nholes = len(holes)
nholes_filled = 0
for i, j in list(holes):
# there is nan
nholes += 1
for di, dj in get_deltas():
u = i + di
v = j + dj
if (0 <= u < H) and (0 <= v < W):
if not np.isnan(rmapx[u, v]):
rmapx[i, j] = rmapx[u, v]
rmapy[i, j] = rmapy[u, v]
nholes_filled += 1
holes.remove((i, j))
break
# print('holes %s filled: %s' % (nholes, nholes_filled))
if nholes_filled == 0:
break
class GroundProjectionGeometry(object):
"""
This class only knows about geometry, but not configuration files.
Conventions and coordinate frames:
"vector" Vector2D (x, y) normalized image coordinates in rectified image
"pixel" Pixel (u, v) pixel coordinates
"ground" Point (x, y, z=0) axle frame
A previous version of the code allowed for a hidden flag
to specify whether the points were rectified or not.
Now, "vector" is always rectified.
"""
@dtu.contract(camera_info=CameraInfo, homography='array[3x3]')
def __init__(self, camera_info, homography):
self.ci = camera_info
self.H = homography
self.Hinv = np.linalg.inv(self.H)
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(self.ci)
self._rectify_inited = False
self._distort_inited = False
def get_camera_info(self):
return self.ci
# # XXX: this needs to be removed
# self.rectified_input = False
@dtu.contract(vec=Vector2D, returns=Pixel)
def vector2pixel(self, vec):
""" Converts a [0,1]*[0,1] representation to [0, W]x[0, H]. """
pixel = Pixel()
cw = self.ci.width
ch = self.ci.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
return pixel
@dtu.contract(pixel=Pixel, returns=Vector2D)
def pixel2vector(self, pixel):
""" Converts a [0,W]*[0,H] representation to [0, 1]x[0, 1]. """
x = pixel.u / self.ci.width
y = pixel.v / self.ci.height
return Vector2D(x, y)
@dtu.contract(vec=Vector2D, returns=Point)
def vector2ground(self, vec):
""" Converts normalized coordinates to ground plane """
pixel = self.vector2pixel(vec)
return self.pixel2ground(pixel)
@dtu.contract(point=Point, returns=Pixel) # NO
def ground2vector(self, point):
pixel = self.ground2pixel(point)
return self.pixel2vector(pixel)
@dtu.contract(pixel=Pixel, returns=Point)
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
@dtu.contract(point=Point, returns=Pixel)
def ground2pixel(self, point):
if point.z != 0:
msg = 'This method assumes that the point is a ground point (z=0). '
msg += 'However, the point is (%s,%s,%s)' % (point.x, point.y,
point.z)
raise ValueError(msg)
ground_point = np.array([point.x, point.y, 1.0])
# An applied mathematician would cry for this
# image_point = np.dot(self.Hinv, ground_point)
# A better way:
image_point = np.linalg.solve(self.H, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
# if not self.rectified_input:
# dtu.logger.debug('project3dToPixel')
# 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]
return pixel
def rectify_point(self, p):
res1 = self.pcm.rectifyPoint(p)
#
# pcm = self.pcm
# point = np.zeros((2, 1))
# point[0] = p[0]
# point[1] = p[1]
#
# res2 = cv2.undistortPoints(point.T, pcm.K, pcm.D, R=pcm.R, P=pcm.P)
# print res1, res2
return res1
def _init_rectify_maps(self):
W = self.pcm.width
H = self.pcm.height
mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(W, H), cv2.CV_32FC1, mapx,
mapy)
self.mapx = mapx
self.mapy = mapy
self._rectify_inited = True
def rectify(self, cv_image_raw, interpolation=cv2.INTER_NEAREST):
''' Undistort an image.
To be more precise, pass interpolation= cv2.INTER_CUBIC
'''
if not self._rectify_inited:
self._init_rectify_maps()
#
# inter = cv2.INTER_NEAREST # 30 ms
# inter = cv2.INTER_CUBIC # 80 ms
# cv_image_rectified = np.zeros(np.shape(cv_image_raw))
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
cv_image_rectified)
return res
def distort(self, rectified):
if not self._rectify_inited:
self._init_rectify_maps()
if not self._distort_inited:
self.rmapx, self.rmapy = invert_map(self.mapx, self.mapy)
self._distort_inited = True
distorted = np.zeros(np.shape(rectified))
res = cv2.remap(rectified, self.rmapx, self.rmapy, cv2.INTER_NEAREST,
distorted)
return res
def rectify_full(self,
cv_image_raw,
interpolation=cv2.INTER_NEAREST,
ratio=1):
'''
Undistort an image by maintaining the proportions.
To be more precise, pass interpolation= cv2.INTER_CUBIC
Returns the new camera matrix as well.
'''
W = int(self.pcm.width * ratio)
H = int(self.pcm.height * ratio)
# mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
# mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
print('K: %s' % self.pcm.K)
print('P: %s' % self.pcm.P)
# alpha = 1
# new_camera_matrix, validPixROI = cv2.getOptimalNewCameraMatrix(self.pcm.K, self.pcm.D, (H, W), alpha)
# print('validPixROI: %s' % str(validPixROI))
# Use the same camera matrix
new_camera_matrix = self.pcm.K.copy()
new_camera_matrix[0, 2] = W / 2
new_camera_matrix[1, 2] = H / 2
print('new_camera_matrix: %s' % new_camera_matrix)
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, new_camera_matrix,
(W, H), cv2.CV_32FC1)
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, mapx, mapy, interpolation,
cv_image_rectified)
return new_camera_matrix, res
class BodyEstimator3D(object):
def __init__(self, camera_info):
self.pinhole_camera_model = PinholeCameraModel()
self.pinhole_camera_model.fromCameraInfo(camera_info)
self.body_tracker = SequentialTracker()
self.kalman_filter = {} # TODO
self.image_resolution = np.array(
[camera_info.width, camera_info.height])
self.bound_x = [0, self.image_resolution[0] - 1]
self.bound_y = [0, self.image_resolution[1] - 1]
self.kernel_size = 7
self.part_id_to_exclude = [8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18]
def estimate(self, bodies2D, np_depth_img):
filtered_bodies2D = [
self.__filter_body_parts(body2D) for body2D in bodies2D
]
tracked_bodies3D = self.__project_body_on_disparity(
filtered_bodies2D, np_depth_img)
# track the bodies regarding the last callback
self.body_tracker.track(tracked_bodies3D)
tracked_bodies3D = self.body_tracker.get_tracked_elements()
# smooth the poses
#stab_bodies3D = self.kalman_filter.stabilize(tracked_bodies3D)
return tracked_bodies3D
def reset(self):
self.body_tracker.clear_elements()
def __filter_body_parts(self, body):
return {
part_id: part
for (part_id, part) in body.items()
if part_id not in self.part_id_to_exclude and part[2] >= 0.5
}
def __project_body_on_disparity(self, bodies, np_depth_img):
tracked_bodies3D = []
for body in bodies:
body_dict = {}
for part_id, part in body.items():
np_pos3D = self.__project_point2D_on_disparity(
part[:2], np_depth_img)
if np_pos3D is not None:
body_dict[part_id] = np.array([np_pos3D, part[2]])
tracked_bodies3D.append(TrackedBody(body_dict))
return tracked_bodies3D
def __project_point2D_on_disparity(self, np_normalized_point,
np_depth_img):
np_pos = np.multiply(np_normalized_point, self.image_resolution)
np_rect_pos = np.array(
self.pinhole_camera_model.rectifyPoint((np_pos[0], np_pos[1])))
np_vector3D = np.array(
self.pinhole_camera_model.projectPixelTo3dRay(
(np_rect_pos[0], np_rect_pos[1])))
np_clipped_rect_pos = np.clip(np_rect_pos, [0, 0],
self.image_resolution)
x, y = int(np_clipped_rect_pos[0]), int(np_clipped_rect_pos[1])
bloc = np_depth_img[y - self.kernel_size:y + self.kernel_size,
x - self.kernel_size:x +
self.kernel_size] # TODO: check out of bounds
nzeros = np.nonzero(bloc)
if len(nzeros[0]) > 0:
depth = np.min(bloc[nzeros]) / 1000.0 # scale
return np_vector3D * depth
return None
