def run(self, img: ndarray, road_ellipse: Ellipse,
angle: float) -> ndarray:
x_center = int(img.shape[1] / 2)
if angle > 0:
img_debug = cv2.arrowedLine(
img.copy(),
pt1=(x_center, 50),
pt2=(int(x_center + (20 * math.sin(angle * math.pi / 2))),
int(50 - (20 * math.cos(angle * math.pi / 2)))),
color=(255, 20, 100),
thickness=3,
tipLength=0.5)
else:
img_debug = cv2.arrowedLine(
img.copy(),
pt1=(x_center, 50),
pt2=(int(x_center - (20 * math.sin(-1 * angle * math.pi / 2))),
int(50 - (20 * math.cos(angle * math.pi / 2)))),
color=(255, 20, 100),
thickness=3,
tipLength=0.5)
if not road_ellipse:
return img
if road_ellipse.axes:
reduced_axes = (int(road_ellipse.axes[0] / 5),
int(road_ellipse.axes[1] / 5))
else:
reduced_axes = (1, 1)
green = int(road_ellipse.trust * 255)
red = 255 - int(road_ellipse.trust * 255)
img_debug = cv2.ellipse(img_debug,
center=road_ellipse.center,
axes=reduced_axes,
angle=road_ellipse.angle,
startAngle=0,
endAngle=360,
color=(20, green, red),
thickness=2)
img_debug = cv2.circle(img_debug,
center=road_ellipse.center,
radius=5,
color=(255, 0, 0))
img_debug = cv2.putText(img=img_debug,
text='{0:.2f}'.format(angle),
org=(10, 10),
fontFace=cv2.FONT_HERSHEY_PLAIN,
fontScale=1,
color=(255, 255, 255))
return img_debug
def lasso_hetero_gs(X: ndarray, y: ndarray, lam: ndarray, w_init: ndarray, tol: float,
verbose: bool=False) -> ndarray:
n, m = X.shape
scaler = np.sqrt(np.sum(X ** 2, axis=0))
X = X / scaler
w = w_init.copy()
if m == 0:
return w
def subgradient(w, r):
return -r @ X + lam * np.sign(w)
r = 0
for t in count():
if t % 100 == 0:
r = y - X @ w
sg = subgradient(w, r)
eff_lam = lam * (w == 0)
abs_g = abs(soft_threshold(sg, eff_lam))
i = np.argmax(abs_g)
if verbose:
print("lasso_hetero_gs: step: {}\tgrad: {}".format(t, abs_g[i]))
if t % 100 == 99:
import pdb
pdb.set_trace()
if abs_g[i] / np.fabs(w) < tol:
break
w_i_new = soft_threshold(w[i] + X[:, i] @ r, lam[i])
r += (w[i] - w_i_new) * X[:, i]
w[i] = w_i_new
return w * scaler
def normalize(img: ndarray, by="area", dtype=np.float64) -> ndarray:
"""Get a normalized copy of an ndarray, e.g. an image or kernel.
The returned array will be a float.
Args:
img: Input ndarray.
by : Used normalization method. Available methods are:\n
* 'area': normalize to sum one
* 'peak': normalize to maximum value
* 'l2': normalize by L2 norm of the array
dtype : Output dtype. Either np.float16, np.float32 or np.float64.
If input is float, output will be of same word size.
Returns:
Normalized array of same shape as input array.
See Also:
Based on :func:`normalize_in_place()`.
"""
if img.dtype not in [np.float16, np.float32, np.float64]:
d_type = dtype
else:
d_type = img.dtype
res = img.copy().astype(d_type)
normalize_in_place(res, by=by)
return res
def denoise_image_huber(img: ndarray,
n_iter: int,
w_lambda: Union[ndarray, float] = 0.5) -> ndarray:
"""Primal-dual algorithm for minimization of TV-Huber-norm-L1-functional.
Algotirhm is based on [R1]_.
Discrete functional:
- TV_Huber-L1: min_x( ||_nabla x||_h + lambda*||x - f||_1 )
Args:
img:
Input image.
n_iter:
Number of iterations
w_lambda:
Weight factor of data term. Pixel-wise weight is possible.
Returns:
The filtered image of the same shape as the input image.
"""
L2 = 8.0
alpha = 0.05
gamma = 5
delta = alpha
mu = 2 * np.sqrt(gamma * delta) / np.sqrt(L2)
tau = mu / 2 / gamma
theta = 1 / (1 + mu)
sigma = mu / 2 / delta
# Iterative primal-dual algorithm
u = img.copy()
y = _nabla(u)
for i in range(n_iter):
# Optimize dual variable ( prox_f ) TV
y = y + sigma * _nabla(u)
# Projection (TV with huber norm)
y = _prox_tv(y, 1 + sigma * alpha) / (1 + sigma * alpha)
# Optimize primal variable ( prox_g )
u_new = u - tau * _nablaT(y)
# l1-norm (shrink)
u_new = _prox_l1(u_new, img, w_lambda * tau)
# Extrapolate
u = u_new + theta * (u_new - u)
# Break if max accuracy reached
# if (np.abs(u[:]-u_new[:])).sum() < tol:
# print(i)
# break
return u
def _apply_horizon(self, img: ndarray):
horizon = int(img.shape[0] * self._config.horizon)
if horizon < 1:
return img.copy(), img.copy()
img_horizon = cv2.rectangle(img=img.copy(),
pt1=(0, 0),
pt2=(img.shape[1], horizon - 1),
thickness=cv2.FILLED,
color=(0, ))
img_debug = cv2.cvtColor(img.copy(), cv2.COLOR_GRAY2RGB)
img_debug = cv2.line(img=img_debug,
pt1=(0, horizon - 1),
pt2=(img.shape[1], horizon - 1),
thickness=2,
color=(0, 0, 250))
return img_horizon, img_debug
def add_visual_box(img_orig: ndarray, left: int, top: int, right: int,
bottom: int):
img = img_orig.copy()
max_value = np.max(img)
img[top:bottom + 2, left:left + 3, :] = max_value # left
img[top:bottom + 2, right - 1:right + 2, :] = max_value # right
img[top:top + 3, left:right + 2, :] = max_value # top
img[bottom - 1:bottom + 2, left:right + 2, :] = max_value # bottom
return img
def denoise_image_tvl1(img: ndarray,
n_iter: int,
w_lambda: Union[ndarray, float] = 0.5) -> ndarray:
"""Primal-dual algorithm for minimization of TV-L1-functional.
Algotirhm is based on [R1]_.
Discrete functional:
- TV-L1: min_x( ||_nabla x||_1 + lambda*||x - f||_1 )
Args:
img:
Input image.
n_iter:
Number of iterations
w_lambda:
Weight factor of data term. Pixel-wise weight is possible.
Returns:
The filtered image of the same shape as the input image.
"""
u = img.copy()
y = _nabla(u)
L2 = 8.0
tau = 0.02
sigma = 1.0 / (L2 * tau)
theta = 1.0
# Iterative primal-dual algorithm
for i in range(n_iter):
# Calculate gradient
u_grad = _nabla(u)
# Optimize dual variable ( prox_f ) TV
y = y + sigma * u_grad
# Projection
y = _prox_tv(y)
# Optimize primal variable ( prox_g )
u_new = u - tau * _nablaT(y)
# l1-norm (shrink) (TV-l1 denoising)
u_new = _prox_l1(u_new, img, w_lambda * tau)
# Extrapolate
u = u_new + theta * (u_new - u)
# Break if max accuracy reached
# if (np.abs(u[:]-u_new[:])).sum() < tol:
# print(i)
# break
return u
def draw_image_debug(self, centroid: Centroid, img_gray: ndarray,
shape: Shape, value: int) -> ndarray:
img_debug = cv2.cvtColor(img_gray.copy(), cv2.COLOR_GRAY2RGB)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img_debug, str(value), (20, 20), font, 1, (255, 255, 255),
1, cv2.LINE_AA)
cv2.circle(img_debug, centroid, 3, (0, 100, 100), 1)
cv2.drawContours(img_debug, shape, -1, (240, 40, 100), 1)
self._video_frame = img_debug
return img_debug
def run(self, img: ndarray, shapes: List[Shape]) -> ndarray:
try:
img_debug = img.copy()
nb_contours = self._config.number_centroids_to_use
colors = self._get_colors_index(nb_contours)
for i in range(nb_contours):
cv2.drawContours(img_debug, shapes[i:i + 1], -1, colors[i], 2)
return img_debug
except:
logging.exception("Unexpected error")
return np.zeros(img.shape)
def _process_contours(self,
img_gray: ndarray) -> (ndarray, List[Centroid]):
shapes, centroids = self._contours_detector.process_image(img_gray)
img = cv2.cvtColor(img_gray.copy(), cv2.COLOR_GRAY2RGB)
for centroid in centroids:
cv2.circle(img, centroid, 3, (0, 100, 100), 1)
cv2.drawContours(img, shapes, -1, (240, 40, 100), 1)
logger.debug("Centroids founds: %s", centroids)
return img, shapes, centroids
def lasso_hetero(X: ndarray, y: ndarray, lam: ndarray, w_init: ndarray, tol: float, return_obj: bool=False,
copy_X: bool=True, max_iter: int=100, verbose: bool=False) -> Union[ndarray, Tuple[ndarray, float]]:
n, m = X.shape
scale = np.sqrt(np.sum(X ** 2, axis=0))
if copy_X:
X = X / scale
else:
X /= scale
lam = lam / scale
w = w_init.copy()
if m == 0:
return w
r = y - X @ w
gap = np.inf
for t in range(max_iter):
w_max = 0
d_w_max = 0
for i in range(m):
w_i_new = soft_threshold(w[i] + X[:, i] @ r, lam[i])
d_w = w[i] - w_i_new
r += d_w * X[:, i]
w[i] = w_i_new
w_max = max(w_max, np.fabs(w_i_new))
d_w_max = max(d_w_max, np.fabs(d_w))
gap = d_w_max / (w_max + 1e-16)
if verbose:
print("lasso_hetero_gs: step: {}\tgrad: {}".format(t, gap))
if gap < tol:
break
else:
warnings.warn("not converged after {} iterations; gap: {}".format(max_iter, gap), ConvergenceWarning)
ww: ndarray = w / scale
if return_obj:
obj: float = 0.5 * np.sum(r) + np.sum(lam * np.fabs(ww))
return ww, obj
else:
return ww
def run(self, img: ndarray) -> ndarray:
try:
rows, columns, channel = np.shape(img)
middle = int(columns / 2)
mask = np.zeros(img.shape, np.uint8)
central_zone_delta = int(
((columns / 100) * self._config.central_zone_percent) / 2)
# Draw safe zone
cv2.rectangle(img=mask,
pt1=(middle - central_zone_delta, 0),
pt2=(middle + central_zone_delta, rows),
color=(0, 255, 0),
thickness=cv2.FILLED)
out_zone_delta = int(
((columns / 100) * self._config.out_zone_percent) / 2)
# Draw dangerous zone
cv2.rectangle(img=mask,
pt1=(0, 0),
pt2=(out_zone_delta, rows),
color=(255, 0, 0),
thickness=cv2.FILLED)
cv2.rectangle(img=mask,
pt1=(columns - out_zone_delta, 0),
pt2=(columns, rows),
color=(255, 0, 0),
thickness=cv2.FILLED)
# Apply mask
img_debug = cv2.addWeighted(src1=img.copy(),
alpha=0.7,
src2=mask,
beta=0.3,
gamma=0)
# Draw central axes
img_debug = cv2.line(img=img_debug,
pt1=(middle, 0),
pt2=(middle, img.shape[1]),
color=(0, 0, 255),
thickness=2)
return img_debug
except:
logging.exception("Unexpected error")
return np.zeros(img.shape)
def get_weights(edges_with_grouping_orig: ndarray, groups_members: ndarray,
affinities: ndarray, left: int, top: int, right: int,
bottom: int) -> List[float]:
edges_with_grouping = edges_with_grouping_orig.copy()
edges_with_grouping[top:bottom, left:right, 1] = -1
groups_not_in_box = np.unique(edges_with_grouping[:, :, 1])
def calculate_weight(affs: ndarray, group_id: int):
def generate_paths(group_len: int, length: int):
paths: list = [[group_id]]
for _ in range(length):
paths = [
p + [new_group_id] for p in paths
for new_group_id in range(group_len)
if new_group_id != p[-1] and
affs[new_group_id, p[-1]] > 0.0 and not (new_group_id in p)
]
return list(filter(lambda p: p[-1] in groups_not_in_box, paths))
if group_id in groups_not_in_box:
return 0.0
max_path_length = 10
max_chained_affinity = 0.0
for i in range(max_path_length):
for path in generate_paths(len(groups_members), i):
path1 = path[0:-1]
path2 = path[1:]
adjacent_path = zip(path1, path2)
affinity_path = map(lambda v12: affinities[v12[0], v12[1]],
adjacent_path)
affinity_reduced = reduce(lambda a1, a2: a1 * a2,
affinity_path)
max_chained_affinity = max(affinity_reduced,
max_chained_affinity)
return 1.0 - max_chained_affinity
w = [
calculate_weight(affinities, group_id)
for group_id in range(len(groups_members))
]
return w
def run(self, img_gray: ndarray) -> int:
try:
(_, binary) = cv2.threshold(img_gray.copy(),
self._config.centroid_value, 255, 0,
cv2.THRESH_BINARY)
(shapes, centroids) = self._contours_detector.process_image(
img_binarized=binary)
if not centroids:
return self._config.centroid_value
value = img_gray.item((centroids[0][1], centroids[0][0]))
self._config.centroid_value = value
logger.debug("Threshold value estimate: %s", value)
self.draw_image_debug(centroids[0], img_gray, [shapes[0]], value)
return value
except Exception:
import numpy
logging.exception("Unexpected error")
return self._config.centroid_value
def _display_bbs(img: ndarray, bbs: List[Tuple[Point, Size2D]]) -> ndarray:
_img = img.copy()
for bb in bbs:
_img = draw_bbox_on_image(_img, bb, color=(0, 255, 0))
return _img
def denoise_image_rof(img: ndarray,
n_iter: int,
w_lambda: Union[ndarray, float] = 5) -> ndarray:
"""Primal-dual algorithm for minimization of ROF-functional (TV-L2).
Fast form of primal-dual algorithm (faster than standard).
Algotirhm is based on [R1]_.
Discrete functional:
- ROF: min_x( ||_nabla x||_1 + 0.5*lambda*(||x - f||_1)**2 )
Args:
img:
Input image.
n_iter:
Number of iterations
w_lambda:
Weight factor of data term. Pixel-wise weight is possible.
Returns:
The filtered image of the same shape as the input image.
References:
.. [R1] Chambolle, Antonin; Pock, Thomas (2011): A First-Order
Primal-Dual Algorithm for Convex Problems with Applications
to Imaging. In: Journal of Mathematical Imaging and Vision 40 (1)
"""
u = img.copy()
y = _nabla(u)
L2 = 8
tau = 0.02
sigma = 1.0 / (L2 * tau)
gamma = 0.35 * w_lambda
for i in range(n_iter):
# Calculate gradient
u_grad = _nabla(u)
# Optimize dual variable ( prox_f )
y = y + sigma * u_grad
# Projection
y = _prox_tv(y)
# Optimize primal variable ( prox_g )
u_new = u - tau * _nablaT(y)
# l2-norm (ROF denoising)
u_new = _prox_l2(u_new, img, w_lambda * tau)
# Optimize step-size (faster convergence)
theta = 1 / np.sqrt(1 + 2 * gamma * tau)
tau = theta * tau
sigma = sigma / theta
# Extrapolate
u = u_new + theta * (u_new - u)
# Break if max accuracy reached
# if (np.abs(u[:]-u_new[:])).sum() < tol:
# print(i)
# break
return u