def get_pairs_est(I, src_coords, dst_coords, trans_mat):
'''
This function performs forward and backward transforms
to get the estimate of destination and source regions,
Bilinear interpolation is used
Input:
I: RGB image
src_coords: 2xn matrix, 2D coordinates of n points in source region
dst_coords: 2xn matrix, 2D coordinates of n points in destination region
trans_mat: transformation matrix from source to destination
'''
trans_mat_inv = np.linalg.inv(trans_mat)
# get full coords of dst region
tl = (np.min(dst_coords[0,:]), np.min(dst_coords[1,:]))
br = (np.max(dst_coords[0,:]), np.max(dst_coords[1,:]))
w,h = (br[0] - tl[0], br[1] - tl[1])
x = np.linspace(tl[0], br[0]-1,w)
y = np.linspace(tl[1], br[1]-1,h)
xv,yv = np.meshgrid(x, y)
dst_shape = xv.shape
dst_coords_est_full = np.stack((xv.reshape((-1)), yv.reshape((-1))))
# get full coords of src region
tl = (np.min(src_coords[0,:]), np.min(src_coords[1,:]))
br = (np.max(src_coords[0,:]), np.max(src_coords[1,:]))
w,h = (br[0] - tl[0], br[1] - tl[1])
x = np.linspace(tl[0], br[0]-1,w)
y = np.linspace(tl[1], br[1]-1,h)
xv,yv = np.meshgrid(x, y)
src_shape = xv.shape
src_coords_est_full = np.stack((xv.reshape((-1)), yv.reshape((-1))))
# compute I_dst_est
srt_coords_est = cv2.transform(dst_coords_est_full.T[:,np.newaxis,:], trans_mat_inv[:2,:])
srt_coords_est = np.squeeze(srt_coords_est).T
mapX = np.reshape(srt_coords_est[0,:], dst_shape)
mapY = np.reshape(srt_coords_est[1,:], dst_shape)
dstMap1, dstMap2 = cv2.convertMaps(mapX.astype(np.float32),\
mapY.astype(np.float32), cv2.CV_16SC2)
I_dst_est = np.zeros((mapX.shape[0], mapX.shape[1], I.shape[2]))
for c in range(I.shape[2]):
I_dst_est[:,:,c] = cv2.remap(I[:,:,c], dstMap1, dstMap2, cv2.INTER_LINEAR)
# compute I_src_est
dst_coords_est = cv2.transform(src_coords_est_full.T[:,np.newaxis,:], trans_mat[:2,:])
dst_coords_est = np.squeeze(dst_coords_est).T
mapX = np.reshape(dst_coords_est[0,:], src_shape)
mapY = np.reshape(dst_coords_est[1,:], src_shape)
dstMap1, dstMap2 = cv2.convertMaps(mapX.astype(np.float32),\
mapY.astype(np.float32), cv2.CV_16SC2)
I_src_est = np.zeros((mapX.shape[0], mapX.shape[1], I.shape[2]))
for c in range(I.shape[2]):
I_src_est[:,:,c] = cv2.remap(I[:,:,c], dstMap1, dstMap2, cv2.INTER_LINEAR)
return I_dst_est.astype(dtype=np.uint8), I_src_est.astype(dtype=np.uint8)
def pushforward(image, affineT):
sizeImage = np.shape(image)
x_vector = np.arange(0, sizeImage[1])
y_vector = np.arange(0, sizeImage[0])
x_grid, y_grid = np.meshgrid(x_vector, y_vector)
x_grid_slim = np.reshape(x_grid, (np.size(x_grid, )))
y_grid_slim = np.reshape(y_grid, (np.size(y_grid, )))
xy_slim_combined = np.zeros((np.size(x_grid_slim), 3))
xy_slim_combined[:, 1] = x_grid_slim
xy_slim_combined[:, 0] = y_grid_slim
xy_slim_combined[:, 2] = 1
xy_slim_deformed = np.dot(affineT, xy_slim_combined.T)
x_slim_deformed = xy_slim_deformed[1, :]
y_slim_deformed = xy_slim_deformed[0, :]
x_grid_deformed = np.reshape(x_slim_deformed,
(np.shape(x_grid)[0], np.shape(x_grid)[1]))
y_grid_deformed = np.reshape(y_slim_deformed,
(np.shape(y_grid)[0], np.shape(y_grid)[1]))
dstMapX, dstMapY = cv2.convertMaps(x_grid_deformed.astype(np.float32),
y_grid_deformed.astype(np.float32),
cv2.CV_16SC2)
transformed_img = cv2.remap(image, dstMapY, dstMapX, cv2.INTER_LINEAR)
return transformed_img
def create_img_projection_maps(source_cam: Camera, destination_cam: Camera):
"""generates maps for cv2.remap to remap from one camera to another"""
u_map = np.zeros((destination_cam.height, destination_cam.width, 1),
dtype=np.float32)
v_map = np.zeros((destination_cam.height, destination_cam.width, 1),
dtype=np.float32)
destination_points_b = np.arange(destination_cam.height)
for u_px in range(destination_cam.width):
destination_points_a = np.ones(destination_cam.height) * u_px
destination_points = np.vstack(
(destination_points_a, destination_points_b)).T
source_points = source_cam.project_3d_to_2d(
destination_cam.project_2d_to_3d(destination_points,
norm=np.array([1])))
u_map.T[0][u_px] = source_points.T[0]
v_map.T[0][u_px] = source_points.T[1]
map1, map2 = cv2.convertMaps(u_map,
v_map,
dstmap1type=cv2.CV_16SC2,
nninterpolation=False)
return map1, map2
def warp(image, U, V):
"""Warp image using X and Y displacements (U and V).
Parameters
----------
image: grayscale floating-point image, values in [0.0, 1.0]
Returns
-------
warped: warped image, such that warped[y, x] = image[y + V[y, x], x + U[y, x]]
"""
r, c = np.shape(U)
imgArr = np.array(image)[0:r][0:c]
#r,c=np.shape(imgArr)
warped = np.zeros([r, c])
xrange = np.linspace(0, c - 1, num=c, endpoint=True)
yrange = np.linspace(0, r - 1, num=r, endpoint=True)
X, Y = np.meshgrid(xrange, yrange)
XU = X + U
YV = Y + V
XU1 = XU.astype(np.float32)
YV1 = YV.astype(np.float32)
(map1, map2) = cv2.convertMaps(XU1,
YV1,
cv2.CV_16SC2,
nninterpolation=True)
warped = cv2.remap(imgArr, map1, map2, cv2.INTER_NEAREST)
# TODO: Your code here
return warped
def viewRenderingFast(src,
dst,
depth,
K,
new_K,
R,
T,
undistortMap=None,
distCoeffs=None,
overwrite=False):
''' Project depth value unto RGB image coordinates '''
src = np.double(src)
depth = np.double(depth)
(h, w) = src.shape[:2]
# print(K_depth)
# print(R)
# print(T)
# print(K_rgb)
if (undistortMap is not None):
if (distCoeffs is None):
print('Cannot undistort if no distortion coefficients are passed')
else:
depth = undistortImage(depth, K, distCoeffs, crop=False)
src = undistortImage(src, K, distCoeffs, crop=False)
# src image sensor coordinates
[x, y] = np.meshgrid(range(w), range(h))
# world coordinates
X = (x - K[0, 2]) * depth / K[0, 0]
Y = (y - K[1, 2]) * depth / K[1, 1]
Z = depth
Points3D = np.array(
[np.reshape(X, X.size),
np.reshape(Y, Y.size),
np.reshape(Z, Z.size)])
# projection onto the new camera plane
new_Points3D = np.dot(R, Points3D) + T
new_X = np.reshape(new_Points3D[0], (h, w))
new_Y = np.reshape(new_Points3D[1], (h, w))
new_Z = np.reshape(new_Points3D[2], (h, w))
# conversion to sensor coordinates
new_x = new_X * new_K[0, 0] / new_Z + new_K[0, 2]
new_y = new_Y * new_K[1, 1] / new_Z + new_K[1, 2]
(new_x, new_y) = cv2.convertMaps((new_x.astype('float32')),
(new_y.astype('float32')),
dstmap1type=cv2.CV_32FC1)
temp = cv2.remap(src, new_x, new_y, cv2.INTER_LINEAR)
if overwrite:
dst[:, :, :] = temp[:, :, :]
def _elastic_transform(self, img, mask):
# convert to numpy
img = np.array(img) # hxwxc
mask = np.array(mask)
shape1=img.shape
alpha = self.alpha*shape1[0]
sigma = self.sigma*shape1[0]
x, y = np.meshgrid(np.arange(shape1[0]), np.arange(shape1[1]), indexing='ij')
blur_size = int(4 * sigma) | 1
dx = cv2.GaussianBlur((np.random.rand(shape1[0], shape1[1]) * 2 - 1), ksize=(blur_size, blur_size),sigmaX=sigma) * alpha
dy = cv2.GaussianBlur((np.random.rand(shape1[0], shape1[1]) * 2 - 1), ksize=(blur_size, blur_size), sigmaX=sigma) * alpha
if (x is None) or (y is None):
x, y = np.meshgrid(np.arange(shape1[0]), np.arange(shape1[1]), indexing='ij')
map_x = (x + dx).astype(np.float32)
map_y = (y + dy).astype(np.float32)
# convert map
map_x, map_y = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2)
img = cv2.remap(img, map_y, map_x, interpolation=cv2.INTER_LINEAR, borderMode = cv2.BORDER_CONSTANT).reshape(shape1)
mask = cv2.remap(mask, map_y, map_x, interpolation=cv2.INTER_NEAREST, borderMode = cv2.BORDER_CONSTANT).reshape(shape1)
img=Image.fromarray(img,mode=self.img_type)
mask=Image.fromarray(mask, mode='L')
return img,mask
def init_map_from_matrix(self, m):
rospy.loginfo("Building map from calibration matrix...")
self.maps_ready = False
Hd = self.height()
Wd = self.width()
self.map_x = np.zeros((Hd, Wd), np.float32)
self.map_y = np.zeros((Hd, Wd), np.float32)
m = np.linalg.inv(m)
for y in range(0, int(Hd - 1)):
for x in range(0, int(Wd - 1)):
self.map_x.itemset((y, x),
(m[0, 0] * x + m[0, 1] * y + m[0, 2]) /
(m[2, 0] * x + m[2, 1] * y + m[2, 2]))
self.map_y.itemset((y, x),
(m[1, 0] * x + m[1, 1] * y + m[1, 2]) /
(m[2, 0] * x + m[2, 1] * y + m[2, 2]))
self.map_x, self.map_y = cv2.convertMaps(self.map_x, self.map_y,
cv2.CV_16SC2)
self.maps_ready = True
rospy.loginfo("Done!")
def init_map_from_matrix(self, m):
rospy.loginfo("Building map from calibration matrix...")
self.maps_ready = False
Hd = self.height()
Wd = self.width()
self.map_x = np.zeros((Hd, Wd), np.float32)
self.map_y = np.zeros((Hd, Wd), np.float32)
m = np.linalg.inv(m)
for y in range(0, int(Hd - 1)):
for x in range(0, int(Wd - 1)):
self.map_x.itemset(
(y, x), (m[0, 0] * x + m[0, 1] * y + m[0, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2]))
self.map_y.itemset(
(y, x), (m[1, 0] * x + m[1, 1] * y + m[1, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2]))
self.map_x, self.map_y = cv2.convertMaps(
self.map_x, self.map_y, cv2.CV_16SC2)
self.maps_ready = True
try:
with open(self.map_path, 'wb') as f:
pickle.dump((self.map_x, self.map_y), f)
except (IOError, OSError) as e:
rospy.logerr("Failed to store map to file: " + str(e))
rospy.loginfo("Done!")
def convert_distortion_maps(image):
distortion_length = image.distortion_width * image.distortion_height
xmap = np.zeros(distortion_length / 2, dtype=np.float32)
ymap = np.zeros(distortion_length / 2, dtype=np.float32)
for i in range(0, distortion_length, 2):
xmap[distortion_length / 2 - i / 2 -
1] = image.distortion[i] * image.width
ymap[distortion_length / 2 - i / 2 -
1] = image.distortion[i + 1] * image.height
xmap = np.reshape(xmap,
(image.distortion_height, image.distortion_width / 2))
ymap = np.reshape(ymap,
(image.distortion_height, image.distortion_width / 2))
#resize the distortion map to equal desired destination image size
resized_xmap = cv2.resize(xmap, (image.width, image.height), 0, 0,
cv2.INTER_LINEAR)
resized_ymap = cv2.resize(ymap, (image.width, image.height), 0, 0,
cv2.INTER_LINEAR)
#Use faster fixed point maps
coordinate_map, interpolation_coefficients = cv2.convertMaps(
resized_xmap, resized_ymap, cv2.CV_32FC1, nninterpolation=False)
return coordinate_map, interpolation_coefficients
def detectTrafficLine(self, img):
Ni, Nj = (90, 1300)
M = np.array([[-1.86073726e-01, -5.02678929e-01, 4.72322899e+02],
[-1.39150388e-02, -1.50260445e+00, 1.00507430e+03],
[-1.77785988e-05, -1.65517173e-03, 1.00000000e+00]])
iM = inv(M)
xy = np.zeros((640, 640, 2), dtype=np.float32)
for py in range(640):
for px in range(640):
xy[py, px] = np.array([px, py], dtype=np.float32)
ixy = cv2.perspectiveTransform(xy, iM)
mpx, mpy = cv2.split(ixy)
mapx, mapy = cv2.convertMaps(mpx, mpy, cv2.CV_16SC2)
gray = preprocessing(img)
canny = cv2.Canny(gray, 30, 90, apertureSize=3)
Amplitude, theta = getGD(canny)
indices, patches = zip(*sliding_window(
Amplitude, theta,
patch_size=(Ni, Nj))) #use sliding_window get indices and patches
labels = predict(
patches, False
) #predict zebra crossing for every patches 1 is zc 0 is background
indices = np.array(indices)
ret, location = getlocation(indices, labels, Ni, Nj)
return ret, location
def weak_calibration(img_path, value=5):
image = cv.imread(img_path)
image = cv.resize(image,
dsize=None,
fx=4 / 3,
fy=1,
interpolation=cv.INTER_CUBIC)
height, width = image.shape[:2]
cx, cy = width / 2, height / 2
calib_value = -value / 100_000_000
X, Y = np.meshgrid(np.arange(width), np.arange(height))
D = np.hypot(X - cx, Y - cy)
D2 = D + calib_value * np.power(D, 3)
X2 = (cx + (X - cx) / (D + np.finfo(float).eps) * D2).astype(np.float32)
Y2 = (cy + (Y - cy) / (D + np.finfo(float).eps) * D2).astype(np.float32)
(map1, map2) = cv.convertMaps(X2, Y2, dstmap1type=0)
dst = cv.remap(image, map1, map2, interpolation=cv.INTER_CUBIC)
dst = cv.resize(dst,
dsize=None,
fx=3 / 4,
fy=1,
interpolation=cv.INTER_CUBIC)
return dst
def create_spherical_proj(K, xi, D, plus_theta, zi, rho_limit, R, R1):
rvec = np.zeros(3)
tvec = np.zeros(3)
mapx = np.zeros((des_height, des_length), dtype=np.float32)
mapy = np.zeros((des_height, des_length), dtype=np.float32)
height, width = mapx.shape
step_theta = 2 * np.pi / width
step_phi = np.pi / height
for i in range(height):
j = np.array(range(width))
theta = j * step_theta - np.pi + plus_theta
phi = i * step_phi - np.pi / 2
d = toSphereRad(theta, phi)
rho = np.arccos(d[2, :])
d = R1.dot(d)
d[2, :] += zi
d = (R).dot(d)
imagePoints, _ = cv2.omnidir.projectPoints(
np.reshape(np.transpose(d), (1, des_length, 3)), rvec, tvec, K, xi,
D)
ip = np.transpose(np.reshape(imagePoints, (des_length, 2)))
ip[:, rho > rho_limit] = -1
mapx[i, :] = ip[0, :]
mapy[i, :] = ip[1, :]
mask = mapx != -1
mapx, mapy = cv2.convertMaps(mapx, mapy, cv2.CV_16SC2)
return mapx, mapy, mask
def warpImage(ref, flow, grid_x, grid_y):
h, w = grid_x.shape
flow_x = np.clip(flow[:, :, 1] + grid_x, 0, w - 1)
flow_y = np.clip(flow[:, :, 0] + grid_y, 0, h - 1)
flow_x, flow_y = cv2.convertMaps(flow_x.astype(np.float32),
flow_y.astype(np.float32), cv2.CV_32FC2)
warped_img = cv2.remap(ref, flow_x, flow_y, cv2.INTER_LINEAR)
return warped_img
def fine_dewarp(im, lines):
im_h, im_w = im.shape[:2]
debug = cv2.cvtColor(im, cv2.COLOR_GRAY2BGR)
points = []
y_offsets = []
for line in lines:
if len(line) < 10 or abs(line.fit_line().angle()) > 0.001: continue
line.fit_line().draw(debug, thickness=1)
base_points = np.array([letter.base_point() for letter in line.inliers()])
median_y = np.median(base_points[:, 1])
y_offsets.append(median_y - base_points[:, 1])
points.append(base_points)
for underline in line.underlines:
mid_contour = (underline.top_contour() + underline.bottom_contour()) / 2
all_mid_points = np.stack([
underline.x + np.arange(underline.w), mid_contour,
])
mid_points = all_mid_points[:, ::4]
points.append(mid_points)
for p in base_points:
pt = tuple(np.round(p).astype(int))
cv2.circle(debug, (pt[0], int(median_y)), 2, lib.RED, -1)
cv2.circle(debug, pt, 2, lib.GREEN, -1)
cv2.imwrite('points.png', debug)
points = np.concatenate(points)
y_offsets = np.concatenate(y_offsets)
mesh = np.mgrid[:im_w, :im_h].astype(np.float32)
xmesh, ymesh = mesh
# y_offset_interp = interpolate.griddata(points, y_offsets, xmesh, ymesh, method='nearest')
# y_offset_interp = y_offset_interp.clip(-5, 5)
# mesh[1] += y_offset_interp # (mesh[0], mesh[1], grid=False)
y_offset_interp = interpolate.SmoothBivariateSpline(
points[:, 0], points[:, 1], y_offsets.clip(-3, 3),
s=4 * points.shape[0]
)
ymesh -= y_offset_interp(xmesh, ymesh, grid=False).clip(-3, 3)
conv_xmesh, conv_ymesh = cv2.convertMaps(xmesh, ymesh, cv2.CV_16SC2)
out = cv2.remap(im, conv_xmesh, conv_ymesh,
interpolation=cv2.INTER_LINEAR,
borderValue=np.median(im)).T
cv2.imwrite('corrected.png', out)
debug = cv2.cvtColor(out, cv2.COLOR_GRAY2BGR)
for line in lines:
base_points = np.array([letter.base_point() for letter in line.inliers()[1:-1]])
base_points[:, 1] -= y_offset_interp(base_points[:, 0], base_points[:, 1], grid=False)
Line.fit(base_points).draw(debug, thickness=1)
cv2.imwrite('corrected_line.png', debug)
return out
def warp(image, U, V, interpolation, border_mode):
"""Warps image using X and Y displacements (U and V).
This function uses cv2.remap. The autograder will use cubic
interpolation and the BORDER_REFLECT101 border mode. You may
change this to work with the problem set images.
See the cv2.remap documentation to read more about border and
interpolation methods.
Args:
image (numpy.array): grayscale floating-point image, values
in [0.0, 1.0].
U (numpy.array): displacement (in pixels) along X-axis.
V (numpy.array): displacement (in pixels) along Y-axis.
interpolation (Inter): interpolation method used in cv2.remap.
border_mode (BorderType): pixel extrapolation method used in
cv2.remap.
Returns:
numpy.array: warped image, such that
warped[y, x] = image[y + V[y, x], x + U[y, x]]
"""
rows, cols = image.shape
print image.shape
X, Y = np.meshgrid(range(cols), range(rows))
mapX, mapY = cv2.convertMaps(X.astype(np.float32), Y.astype(np.float32),
cv2.CV_16SC2)
mapU, mapV = cv2.convertMaps(U.astype(np.float32), V.astype(np.float32),
cv2.CV_16SC2)
X = np.ndarray.flatten(mapU + mapX)
Y = np.ndarray.flatten(mapV + mapX)
warped = cv2.remap(image,
X,
Y,
interpolation=interpolation,
borderMode=border_mode)
return warped
def computeremaps(w, h, pathcost):
rm1 = np.zeros((96, 7, h, w, 2), np.int16)
rm2 = np.zeros((96, 7, h, w), np.int16)
pcosts = np.zeros((96, 7, h, w), np.float32)
for ang in range(96):
for a in range(7):
rmf = remaptable(w, h, action_dx[a], action_dy[a], ang)
rm1[ang, a], rm2[ang, a] = cv2.convertMaps(rmf, None, cv2.CV_16SC2)
pcosts[ang, a] = action_ds[a] * action_oov[a] * pathcost[ang]
return rm1, rm2, pcosts
def correct_geometry(orig, mesh, interpolation=cv2.INTER_LINEAR):
# coordinates (u, v) on mesh -> mesh[u][v] = (x, y) in distorted image
mesh32 = mesh.astype(np.float32)
xmesh, ymesh = mesh32[:, :, 0], mesh32[:, :, 1]
conv_xmesh, conv_ymesh = cv2.convertMaps(xmesh, ymesh, cv2.CV_16SC2)
out = cv2.remap(orig, conv_xmesh, conv_ymesh, interpolation=interpolation,
borderMode=cv2.BORDER_CONSTANT, borderValue=(255, 255, 255))
lib.debug_imwrite('corrected.png', out)
return out
def buildMap(self, Wd, Hd, Ws, Hs, R1, R2, Cx, Cy, angle):
map_x = np.zeros((Hd, Wd), np.float32)
map_y = np.zeros((Hd, Wd), np.float32)
rMap = np.linspace(R1, R1 + (R2 - R1), Hd)
thetaMap = np.linspace(angle, angle + float(Wd) * 2.0 * np.pi, Wd)
sinMap = np.sin(thetaMap)
cosMap = np.cos(thetaMap)
for y in xrange(0, Hd):
map_x[y] = Cx + rMap[y] * sinMap
map_y[y] = Cy + rMap[y] * cosMap
(self.map1, self.map2) = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2)
def buildMap(self, Wd, Hd, Ws, Hs, R1, R2, Cx, Cy, angle):
map_x = np.zeros((Hd, Wd), np.float32)
map_y = np.zeros((Hd, Wd), np.float32)
rMap = np.linspace(R1, R1 + (R2 - R1), Hd)
thetaMap = np.linspace(angle, angle + float(Wd) * 2.0 * np.pi, Wd)
sinMap = np.sin(thetaMap)
cosMap = np.cos(thetaMap)
for y in xrange(0, Hd):
map_x[y] = Cx + rMap[y] * sinMap
map_y[y] = Cy + rMap[y] * cosMap
(self.map1, self.map2) = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2)
def getMap(self, refFrame):
"""
Returns the x and y maps used to remap the pixels in the remap function.
:param refFrame: An image with the same property as the calibration and target frames.
"""
h, w = refFrame.shape[:2]
mapX, mapY = cv2.initUndistortRectifyMap(self.cameraMatrix, self.distortionCoeffs, None,
self.optimalCameraMatrix, (w, h), 5)
mapX2, mapY2 = cv2.convertMaps(mapX, mapY, cv2.CV_16SC2)
return (mapX2, mapY2)
def dense_image_warp(im, dx, dy, interp=cv2.INTER_LINEAR, do_optimisation=True):
map_x, map_y = deformation_to_transformation(dx, dy)
# The following command converts the maps to compact fixed point representation
# this leads to a ~20% increase in speed but could lead to accuracy losses
# Can be uncommented
if do_optimisation:
map_x, map_y = cv2.convertMaps(map_x, map_y, dstmap1type=cv2.CV_16SC2)
return cv2.remap(im, map_x, map_y, interpolation=interp,
borderMode=cv2.BORDER_REFLECT) # borderValue=float(np.min(im)))
def getMap(self, refFrame):
"""
Returns the x and y maps used to remap the pixels in the remap function.
:param refFrame: An image with the same property as the calibration and target frames.
"""
h, w = refFrame.shape[:2]
mapX, mapY = cv2.initUndistortRectifyMap(self.cameraMatrix,
self.distortionCoeffs, None,
self.optimalCameraMatrix,
(w, h), 5)
mapX2, mapY2 = cv2.convertMaps(mapX, mapY, cv2.CV_16SC2)
return (mapX2, mapY2)
def buildLUT(self, Wd, Hd, Rmax, Rmin, Cx, Cy): map_x = np.zeros((Hd, Wd), np.float32) map_y = np.zeros((Hd, Wd), np.float32) # polar to Cartesian # x = r*cos(t) # y = r*sin(t) for i in range(0,int(Hd)): for j in range(0,int(Wd)): theta = -float(j)/float(Wd)*2.0*np.pi rho = float(Rmin + i) map_x.itemset((i,j), Cx + rho*np.cos(theta)) map_y.itemset((i,j), Cy + rho*np.sin(theta)) (self.map1, self.map2) = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2)
def generate_maps(self):
# src, dst, tform = _warp_test()
x, y = np.meshgrid(np.arange(self.dma_dim[0]),
np.arange(self.dma_dim[1]))
powers = (x**(i - j) * y**j for i in range(self.order + 1)
for j in range(i + 1))
mapx = np.zeros(self.dma_dim[::-1], dtype='float32')
mapy = np.zeros(self.dma_dim[::-1], dtype='float32')
for i, p in enumerate(powers):
mapx += self.inv_transform[0, i] * p
mapy += self.inv_transform[1, i] * p
self.map1, self.map2 = cv2.convertMaps(map1=mapx,
map2=mapy,
dstmap1type=cv2.CV_16SC2,
nninterpolation=False)
def _get_inverse_mappings(self):
w, h = self.img_shape
yy, xx = np.meshgrid(range(h), range(w), indexing='ij')
p_vector = np.stack((xx, yy), axis=2).reshape(
(1, -1, 2)).astype(np.float)
u_points = cv.fisheye.undistortPoints(p_vector,
self.k,
self.d,
P=self.k_optimal).reshape(
(h, -1, 2))
map1, map2 = cv.convertMaps(u_points.astype(np.float32),
None,
dstmap1type=cv.CV_16SC2)
return map1, map2
def warpImage(proj_points_infra_frame_IRL, SampledImage):
newPointsX = proj_points_infra_frame_IRL[:, 0]
newPointsY = proj_points_infra_frame_IRL[:, 1]
inRow_One = 0 <= newPointsY
inRow_Two = newPointsY < np.shape(SampledImage)[0]
inRow = inRow_One * inRow_Two
inCol_One = 0 <= newPointsX
inCol_Two = newPointsX < np.shape(SampledImage)[1]
inCol = inCol_One * inCol_Two
totCond = inRow * inCol
notTotCond = ~totCond
#newPointsX = newPointsX[totCond]
#newPointsY = newPointsY[totCond]
newPointsX[notTotCond] = np.nan
newPointsY[notTotCond] = np.nan
xnew = newPointsX
ynew = newPointsY
# xnew= newPointsX
# ynew = newPointsY
SampledImage_r = SampledImage[:, :, 0]
SampledImage_g = SampledImage[:, :, 1]
SampledImage_b = SampledImage[:, :, 2]
xv_deformed_grid = np.reshape(
xnew, (SampledImage.shape[0], SampledImage.shape[1]))
yv_deformed_grid = np.reshape(
ynew, (SampledImage.shape[0], SampledImage.shape[1]))
dstMapX, dstMapY = cv2.convertMaps(xv_deformed_grid.astype(np.float32),
yv_deformed_grid.astype(np.float32),
cv2.CV_16SC2)
mapped_img_r = cv2.remap(SampledImage_r, dstMapY, dstMapX, cv2.INTER_CUBIC)
mapped_img_g = cv2.remap(SampledImage_g, dstMapY, dstMapX, cv2.INTER_CUBIC)
mapped_img_b = cv2.remap(SampledImage_b, dstMapY, dstMapX, cv2.INTER_CUBIC)
mapped_img = np.zeros_like(SampledImage)
mapped_img[:, :, 0] = mapped_img_r
mapped_img[:, :, 1] = mapped_img_g
mapped_img[:, :, 2] = mapped_img_b
#mapped_img = cv2.cvtColor(mapped_img, cv2.COLOR_GRAY2RGB)
return mapped_img, xv_deformed_grid, yv_deformed_grid
def _compute_H(self, cam, precomp_path='/tmp/be_precomp'):
try: # speedup startup by loading precomputed values if they exists
data = np.load(precomp_path+'.npz')
print('found precomputed data in {}'.format(precomp_path))
self.H = data['H']
self.unwrapped_xmap = data['unwrapped_xmap']
self.unwrapped_ymap = data['unwrapped_ymap']
self.unwrap_undist_xmap_int = data['unwrap_undist_xmap']
self.unwrap_undist_ymap_int = data['unwrap_undist_ymap']
except IOError:
print('precomputed data {} does not exist'.format(precomp_path))
va_corners_img = cam.project(self.borders_isect_be_cam)
va_corners_imp = cam.undistort_points(va_corners_img)
va_corners_unwarped = self.lfp_to_unwarped(cam, self.borders_isect_be_cam.squeeze())
self.H, status = cv2.findHomography(srcPoints=va_corners_imp, dstPoints=va_corners_unwarped, method=cv2.RANSAC, ransacReprojThreshold=0.01)
print('computed H unwarped: ({}/{} inliers)\n{}'.format( np.count_nonzero(status), len(va_corners_imp), self.H))
print('computing H maps')
# make homography maps
self.unwrapped_xmap = np.zeros((self.h, self.w), np.float32)
self.unwrapped_ymap = np.zeros((self.h, self.w), np.float32)
Hinv = np.linalg.inv(self.H)
for y in range(self.h):
for x in range(self.w):
pt_be = np.array([[x], [y], [1]], dtype=np.float32)
pt_imp = np.dot(Hinv, pt_be)
pt_imp /= pt_imp[2]
self.unwrapped_xmap[y,x], self.unwrapped_ymap[y,x] = pt_imp[:2]
# make combined undistortion + homography maps
self.unwrap_undist_xmap = np.zeros((self.h, self.w), np.float32)
self.unwrap_undist_ymap = np.zeros((self.h, self.w), np.float32)
for y in range(self.h):
for x in range(self.w):
pt_be = np.array([[x, y, 1]], dtype=np.float32).T
pt_imp = np.dot(cam.inv_undist_K, np.dot(Hinv, pt_be))
pt_imp /= pt_imp[2]
pt_img = cv2.projectPoints(pt_imp.T, np.zeros(3), np.zeros(3), cam.K, cam.D)[0]
self.unwrap_undist_xmap[y,x], self.unwrap_undist_ymap[y,x] = pt_img.squeeze()
self.unwrap_undist_xmap_int, self.unwrap_undist_ymap_int = cv2.convertMaps(self.unwrap_undist_xmap, self.unwrap_undist_ymap, cv2.CV_16SC2)
print('dome computing H maps, saving for reuse')
np.savez(precomp_path, H=self.H, unwrapped_xmap=self.unwrapped_xmap, unwrapped_ymap=self.unwrapped_ymap,
unwrap_undist_xmap=self.unwrap_undist_xmap_int, unwrap_undist_ymap=self.unwrap_undist_ymap_int)
def buildMap(self, innerRadius, outerRadius, centerX, centerY, angle):
absoluteOuterRadius = centerY + outerRadius
absoluteInnerRadius = centerY + innerRadius
outerCircumference = 2*numpy.pi * outerRadius
mapWidth = int(outerCircumference)
#TODO find actual vertical FOV angle (instead of 90)
mapHeight = int(mapWidth * (90/360))
rMap = numpy.linspace(outerRadius, innerRadius, mapHeight)
thetaMap = numpy.linspace(angle, angle + float(mapWidth) * 2.0 * numpy.pi, mapWidth)
sinMap = numpy.sin(thetaMap)
cosMap = numpy.cos(thetaMap)
map_x = numpy.zeros((mapHeight, mapWidth), numpy.float32)
map_y = numpy.zeros((mapHeight, mapWidth), numpy.float32)
for y in range(0, mapHeight):
map_x[y] = centerX + rMap[y] * sinMap
map_y[y] = centerY + rMap[y] * cosMap
(self.map1, self.map2) = cv2.convertMaps(map_x, map_y, cv2.CV_16SC2)
def undistort_image(image, camera):
h, w = image.shape[:2]
fx, fy = camera.K[0, 0], camera.K[1, 1]
cx, cy = camera.K[0, 2], camera.K[1, 2]
grid_x = (np.arange(w, dtype=np.float32) - cx) / fx
grid_y = (np.arange(h, dtype=np.float32) - cy) / fy
meshgrid = np.stack(np.meshgrid(grid_x, grid_y), axis=2).reshape(-1, 2)
# distort meshgrid points
k = camera.dist[:3].copy()
k[2] = camera.dist[-1]
p = camera.dist[2:4].copy()
r2 = meshgrid[:, 0]**2 + meshgrid[:, 1]**2
radial = meshgrid * (1 + k[0] * r2 + k[1] * r2**2 + k[2] * r2**3).reshape(
-1, 1)
tangential_1 = p.reshape(1, 2) * np.broadcast_to(
meshgrid[:, 0:1] * meshgrid[:, 1:2], (len(meshgrid), 2))
tangential_2 = p[::-1].reshape(
1, 2) * (meshgrid**2 + np.broadcast_to(r2.reshape(-1, 1),
(len(meshgrid), 2)))
meshgrid = radial + tangential_1 + tangential_2
# move back to screen coordinates
meshgrid *= np.array([fx, fy]).reshape(1, 2)
meshgrid += np.array([cx, cy]).reshape(1, 2)
# cache (save) distortion maps
meshgrid = cv2.convertMaps(meshgrid.reshape((h, w, 2)), None, cv2.CV_16SC2)
image_undistorted = cv2.remap(image, *meshgrid, cv2.INTER_CUBIC)
return image_undistorted
def make_composite_map(be, cam, img):
Hinv = np.linalg.inv(be.H)
Kinv = cam.inv_undist_K
dest_w, dest_h = be.w, be.h
xmap = np.zeros((dest_h, dest_w), np.float32)
ymap = np.zeros((dest_h, dest_w), np.float32)
for y in range(dest_h):
for x in range(dest_w):
pt_be = np.array([[x, y, 1]], dtype=np.float32).T
pt_imp = np.dot(Kinv, np.dot(Hinv, pt_be))
pt_imp /= pt_imp[2]
pt_img = cv2.projectPoints(pt_imp.T, np.zeros(3), np.zeros(3), cam.K, cam.D)[0]
xmap[y,x], ymap[y,x] = pt_img.squeeze()
xmap_int, ymap_int = cv2.convertMaps(xmap, ymap, cv2.CV_16SC2)
be_img_mapped2 = cv2.remap(img, xmap_int, ymap_int, cv2.INTER_LINEAR)
#pdb.set_trace()
be_img_mapped = cv2.remap(img, xmap, ymap, cv2.INTER_LINEAR)
be_img = cv2.warpPerspective(cam.undistort_img(img), be.H, (be.w, be.h), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
cv2.imshow('bird eye mapped', be_img_mapped)
cv2.imshow('bird eye mapped2', be_img_mapped2)
cv2.imshow('bird eye full', be_img)
cv2.waitKey(0)
w = trans_lineal_w(w)
real_w = np.around(w.real)
real_w = real_w.astype(np.int32)
imag_w = np.around(w.imag)
imag_w = imag_w.astype(np.int32)
if real_w >= 0 and real_w <= (
width - 1) and imag_w >= 0 and imag_w <= (hight - 1):
w_plane[imag_w, real_w] = z
mapX[imag_w, real_w] = z.real
mapY[imag_w, real_w] = z.imag
print("Tiempo en construir la matriz de reordenamiento %.fms" %
((timer() - t) * 1000))
Map1, Map2 = cv2.convertMaps(mapX, mapY, cv2.CV_16SC2)
while (cap.isOpened()):
t1 = timer()
# Capture frame-by-frame
ret, frame = cap.read()
if ret == True:
# Display the resulting frame
#roi = frame[y1:y2, x1:x2]
img = frame[40:1040, 460:1460]
resized_image = cv2.resize(img, (740, 740))
img = resized_image
#bckgrd_white = 160
#img[0,0] = (bckgrd_white,bckgrd_white,bckgrd_white)
w = img.shape[1]
h = 100
map_xf = np.zeros((h, w), np.float32)
map_yf = np.zeros((h, w), np.float32)
for x in xrange(w):
for y in xrange(h):
theta = x * math.pi * 2 / w
r = 120.0 + y * 0.9
map_xf.itemset((y, x), 474 + math.cos(theta) * r)
map_yf.itemset((y, x), 494 + math.sin(theta) * r)
map_x = np.zeros((h, w), np.uint16)
map_y = np.zeros((h, w), np.uint16)
map_x, map_y = cv2.convertMaps(map_xf, map_yf, cv2.CV_16SC2)
import time
start = time.time()
out = cv2.remap(img, map_x, map_y, cv2.INTER_LINEAR)
end = time.time()
print end - start
cv2.imwrite("unwarped.jpg", out)
cv2.imshow('dst', out)
if cv2.waitKey(100000) == 27:
pass
import matplotlib
from matplotlib import pyplot
from numpy import matlib
(w,h,_) = frame1.shape
#2362,2362
hx = np.matlib.repmat(np.arange(h),w,1).astype(np.float64)
hy = np.matlib.repmat(np.arange(w),h,1).astype(np.float64)
hy = np.transpose(hy)
pyplot.ion()
def_frame = frame1
#def_frame = cv2.imread('/Users/timrudge/Pictures/TimRudge(PS)/MaxProj/halves2.png')
#def_frame = cv2.imread('/Users/timrudge/Code/cf1test.png')
for f in range(n_frames):
#frame = n_frames-1-f
frame = f
hhx = (hx - flow[:,:,0,frame]).astype('float32')
hhy = (hy - flow[:,:,1,frame]).astype('float32')
(chhx,chhy) = cv2.convertMaps(hhx,hhy,cv2.CV_32FC1)
def_frame = cv2.remap(def_frame, hhx, hhy, cv2.INTER_CUBIC)
pyplot.imshow(def_frame)
cv2.imwrite('mim_def_frame_fwd_%05d.png'%(frame), def_frame)
strain = div(flow[:,:,:,f])*255;
cv2.imwrite('mim_def_frame_fwd_strain_%05d.png'%(frame), strain)
# Write to file
flow.tofile(outfilename,sep=',',format='%10.5f')
def calibrate_stereo(left_filenames, right_filenames):
left_images = sorted(glob.glob(left_filenames))
right_images = sorted(glob.glob(right_filenames))
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*8,3), np.float32)
objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2)
obj_points = []
img_pointsL = []
img_pointsR = []
for i in range(len(left_images)):
nameL = left_images[i]
nameR = right_images[i]
imgL = cv2.imread(nameL, 0)
imgR = cv2.imread(nameR, 0)
retL, cornersL = cv2.findChessboardCorners(imgL, (8,6), None)
retR, cornersR = cv2.findChessboardCorners(imgR, (8,6), None)
if retL and retR:
obj_points.append(objp)
cv2.cornerSubPix(imgL, cornersL, (11,11), (-1,-1), criteria)
img_pointsL.append(cornersL)
cv2.cornerSubPix(imgR, cornersR, (11,11), (-1,-1), criteria)
img_pointsR.append(cornersR)
yield False, True, (nameL,cornersL), (nameR,cornersR)
else:
yield False, False, (nameL,None), (nameR,None)
image_size = imgL.shape[::-1]
# Find individual intrinsic parameters first
retL, matrixL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
obj_points, img_pointsL, image_size, None, None)
retR, matrixR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
obj_points, img_pointsR, image_size, None, None)
# Calibrate stereo camera, with calculated intrinsic parameters
ret, matrixL, distL, matrixR, distR, R, T, E, F = cv2.stereoCalibrate(
obj_points, img_pointsL, img_pointsR, image_size,
matrixL, distL, matrixR, distR, flags=cv2.CALIB_FIX_INTRINSIC)
# Calculate rectification transforms for each camera
R1 = np.zeros((3,3), np.float32)
R2 = np.zeros((3,3), np.float32)
P1 = np.zeros((3,4), np.float32)
P2 = np.zeros((3,4), np.float32)
Q = np.zeros((4,4), np.float32)
R1, R2, P1, P2, Q, roiL, roiR = cv2.stereoRectify(
matrixL, distL, matrixR, distR, image_size, R, T)
# Create undistortion/rectification map for each camera
mapsL = cv2.initUndistortRectifyMap(matrixL, distL, R1, P1,
image_size, cv2.CV_32FC1)
mapsR = cv2.initUndistortRectifyMap(matrixR, distR, R2, P2,
image_size, cv2.CV_32FC1)
# Convert maps to fixed-point representation (faster)
mapsL = cv2.convertMaps(mapsL[0], mapsL[1], cv2.CV_32FC1)
mapsR = cv2.convertMaps(mapsR[0], mapsR[1], cv2.CV_32FC1)
calib = {
"intrinsicL": matrixL, "intrinsicR": matrixR,
"mapsL": mapsL, "mapsR": mapsR,
"R1": R1, "R2": R2, "P1": P1, "P2": P2, "Q": Q,
"roiL": roiL, "roiR": roiR
}
yield True, calib, None, None
def map_coordinates(cls, image, dx, dy, order=1, cval=0, mode="constant"):
# small debug message to be sure that the right function is executed
print("map_coordinates() with cv2")
if order == 0 and image.dtype.name in ["uint64", "int64"]:
raise Exception(("dtypes uint64 and int64 are only supported in ElasticTransformation for order=0, "
+ "got order=%d with dtype=%s.") % (order, image.dtype.name))
input_dtype = image.dtype
if image.dtype.name == "bool":
image = image.astype(np.float32)
elif order == 1 and image.dtype.name in ["int8", "int16", "int32"]:
image = image.astype(np.float64)
elif order >= 2 and image.dtype.name == "int8":
image = image.astype(np.int16)
elif order >= 2 and image.dtype.name == "int32":
image = image.astype(np.float64)
shrt_max = 32767
backend = "cv2"
if order == 0:
bad_dtype_cv2 = (image.dtype.name in ["uint32", "uint64", "int64", "float128", "bool"])
elif order == 1:
bad_dtype_cv2 = (image.dtype.name in ["uint32", "uint64", "int8", "int16", "int32", "int64", "float128",
"bool"])
else:
bad_dtype_cv2 = (image.dtype.name in ["uint32", "uint64", "int8", "int32", "int64", "float128", "bool"])
bad_dx_shape_cv2 = (dx.shape[0] >= shrt_max or dx.shape[1] >= shrt_max)
bad_dy_shape_cv2 = (dy.shape[0] >= shrt_max or dy.shape[1] >= shrt_max)
if bad_dtype_cv2 or bad_dx_shape_cv2 or bad_dy_shape_cv2:
backend = "scipy"
ia.do_assert(image.ndim == 3)
result = np.copy(image)
height, width = image.shape[0:2]
# False was added here, only difference to usual code
if False or backend == "scipy":
h, w = image.shape[0:2]
y, x = np.meshgrid(np.arange(h).astype(np.float32), np.arange(w).astype(np.float32), indexing='ij')
x_shifted = x + (-1) * dx
y_shifted = y + (-1) * dy
for c in sm.xrange(image.shape[2]):
remapped_flat = ndimage.interpolation.map_coordinates(
image[..., c],
(x_shifted.flatten(), y_shifted.flatten()),
order=order,
cval=cval,
mode=mode
)
remapped = remapped_flat.reshape((height, width))
result[..., c] = remapped
else:
h, w, nb_channels = image.shape
y, x = np.meshgrid(np.arange(h).astype(np.float32), np.arange(w).astype(np.float32), indexing='ij')
x_shifted = x + (-1) * dx
y_shifted = y + (-1) * dy
if image.dtype.kind == "f":
cval = float(cval)
else:
cval = int(cval)
border_mode = cls._MAPPING_MODE_SCIPY_CV2[mode]
interpolation = cls._MAPPING_ORDER_SCIPY_CV2[order]
is_nearest_neighbour = (interpolation == cv2.INTER_NEAREST)
map1, map2 = cv2.convertMaps(x_shifted, y_shifted, cv2.CV_16SC2, nninterpolation=is_nearest_neighbour)
# remap only supports up to 4 channels
if nb_channels <= 4:
result = cv2.remap(image, map1, map2, interpolation=interpolation, borderMode=border_mode, borderValue=cval)
if image.ndim == 3 and result.ndim == 2:
result = result[..., np.newaxis]
else:
current_chan_idx = 0
result = []
while current_chan_idx < nb_channels:
channels = image[..., current_chan_idx:current_chan_idx+4]
result_c = cv2.remap(channels, map1, map2, interpolation=interpolation, borderMode=border_mode,
borderValue=cval)
if result_c.ndim == 2:
result_c = result_c[..., np.newaxis]
result.append(result_c)
current_chan_idx += 4
result = np.concatenate(result, axis=2)
if result.dtype.name != input_dtype.name:
result = meta.restore_dtypes_(result, input_dtype)
return result
