404 lines
13 KiB
Python
404 lines
13 KiB
Python
import cv2
|
|
import numpy as np
|
|
|
|
from mower.utils import typealias as tp
|
|
from mower.utils.image import cropimg
|
|
from mower.utils.log import logger
|
|
from mower.utils.vector import va, vs
|
|
|
|
GOOD_DISTANCE_LIMIT = 0.8
|
|
|
|
ORB = cv2.ORB_create(nfeatures=1000000, edgeThreshold=0)
|
|
ORB_no_pyramid = cv2.ORB_create(nfeatures=1000000, edgeThreshold=0, nlevels=1)
|
|
|
|
|
|
def keypoints_scale_invariant(img: tp.GrayImage, mask: tp.GrayImage | None = None):
|
|
return ORB.detectAndCompute(img, mask)
|
|
|
|
|
|
def keypoints(img: tp.GrayImage, mask: tp.GrayImage | None = None):
|
|
return ORB_no_pyramid.detectAndCompute(img, mask)
|
|
|
|
|
|
FLANN_INDEX_LSH = 6
|
|
index_params = dict(
|
|
algorithm=FLANN_INDEX_LSH,
|
|
table_number=1,
|
|
key_size=18,
|
|
multi_probe_level=0,
|
|
)
|
|
search_params = dict(checks=50)
|
|
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
|
|
|
|
|
def getHash(data: list[float]) -> tp.Hash:
|
|
"""calc image hash"""
|
|
avreage = np.mean(data)
|
|
return np.where(data > avreage, 1, 0)
|
|
|
|
|
|
def hammingDistance(hash1: tp.Hash, hash2: tp.Hash) -> int:
|
|
"""calc Hamming distance between two hash"""
|
|
return np.count_nonzero(hash1 != hash2)
|
|
|
|
|
|
def aHash(img1: tp.GrayImage, img2: tp.GrayImage) -> int:
|
|
"""calc image hash"""
|
|
data1 = cv2.resize(img1, (8, 4)).flatten()
|
|
data2 = cv2.resize(img2, (8, 4)).flatten()
|
|
hash1 = getHash(data1)
|
|
hash2 = getHash(data2)
|
|
return hammingDistance(hash1, hash2)
|
|
|
|
|
|
class Matcher:
|
|
def __init__(self, origin: tp.GrayImage) -> None:
|
|
logger.debug(f"{origin.shape=}")
|
|
self.origin = origin
|
|
self.kp, self.des = keypoints(self.origin)
|
|
|
|
def in_scope(self, scope):
|
|
if scope is None:
|
|
return self.kp, self.des
|
|
ori_kp, ori_des = [], []
|
|
for _kp, _des in zip(self.kp, self.des):
|
|
if (
|
|
scope[0][0] <= _kp.pt[0] <= scope[1][0]
|
|
and scope[0][1] <= _kp.pt[1] <= scope[1][1]
|
|
):
|
|
ori_kp.append(_kp)
|
|
ori_des.append(_des)
|
|
logger.debug(f"{scope=}, {len(self.kp)=} -> {len(ori_kp)=}")
|
|
return np.array(ori_kp), np.array(ori_des)
|
|
|
|
def match(
|
|
self,
|
|
query: tp.GrayImage,
|
|
draw: bool = False,
|
|
scope: tp.Scope | None = None,
|
|
) -> tuple[float, tp.Scope | None]:
|
|
if self.des is None:
|
|
logger.debug(f"{self.des=}")
|
|
return -1, None
|
|
ori_kp, ori_des = self.in_scope(scope)
|
|
if len(ori_kp) < 2:
|
|
logger.debug(f"{len(ori_kp)=} < 2")
|
|
return -1, None
|
|
|
|
qry_kp, qry_des = keypoints(query)
|
|
matches = flann.knnMatch(qry_des, ori_des, k=2)
|
|
good = []
|
|
for pair in matches:
|
|
if (len_pair := len(pair)) == 2:
|
|
x, y = pair
|
|
if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
|
|
good.append(x)
|
|
elif len_pair == 1:
|
|
good.append(pair[0])
|
|
|
|
if draw:
|
|
from matplotlib import pyplot as plt
|
|
|
|
result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
|
|
plt.imshow(result)
|
|
plt.show()
|
|
|
|
if len(good) <= 4:
|
|
logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
|
|
return -1, None
|
|
|
|
qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
M, mask = cv2.estimateAffine2D(qry_pts, ori_pts, None, cv2.RANSAC)
|
|
|
|
if M is None:
|
|
logger.debug("M is None")
|
|
return -1, None
|
|
else:
|
|
logger.debug(f"M={M.tolist()}")
|
|
|
|
M[0][1] = M[1][0] = 0
|
|
M[0][0] = M[1][1] = 1
|
|
|
|
h, w = query.shape
|
|
|
|
if draw:
|
|
matches_mask = mask.ravel().tolist()
|
|
pts = np.int32([[[0, 0]], [[w - 1, h - 1]]])
|
|
dst = np.int32(cv2.transform(pts, M)).tolist()
|
|
dst = [i[0] for i in dst]
|
|
dst = np.int32(
|
|
[
|
|
[dst[0]],
|
|
[[dst[0][0], dst[1][1]]],
|
|
[dst[1]],
|
|
[[dst[1][0], dst[0][1]]],
|
|
]
|
|
)
|
|
disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
|
|
disp = cv2.polylines(disp, [dst], True, (255, 0, 0), 2, cv2.LINE_AA)
|
|
disp = cv2.drawMatches(
|
|
query,
|
|
qry_kp,
|
|
disp,
|
|
ori_kp,
|
|
good,
|
|
None,
|
|
(0, 255, 0),
|
|
matchesMask=matches_mask,
|
|
flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
|
|
)
|
|
plt.imshow(disp)
|
|
plt.show()
|
|
|
|
pts = np.int32([[[-20, -20]], [[w + 19, h + 19]]])
|
|
dst = cv2.transform(pts, M)
|
|
rect = dst.reshape(2, 2).tolist()
|
|
rect = np.array(rect, dtype=int).tolist()
|
|
disp = cropimg(self.origin, rect)
|
|
rh, rw = disp.shape
|
|
if rh < h or rw < w:
|
|
logger.debug(f"{rect=} {rh=} {h=} {rw=} {w=}")
|
|
return -1, None
|
|
|
|
result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
|
|
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
|
|
|
|
if draw:
|
|
disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
|
|
plt.subplot(1, 2, 1)
|
|
plt.imshow(query, cmap="gray", vmin=0, vmax=255)
|
|
plt.subplot(1, 2, 2)
|
|
plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
|
|
plt.show()
|
|
|
|
top_left = va(rect[0], max_loc)
|
|
scope = top_left, va(top_left, (w, h))
|
|
logger.debug(f"{max_val=} {scope=}")
|
|
return max_val, scope
|
|
|
|
def match2d(
|
|
self,
|
|
query: tp.GrayImage,
|
|
draw: bool = False,
|
|
scope: tp.Scope | None = None,
|
|
) -> tuple[float, tp.Scope | None]:
|
|
if self.des is None:
|
|
logger.debug(f"{self.des=}")
|
|
return -1, None
|
|
ori_kp, ori_des = self.in_scope(scope)
|
|
if len(ori_kp) < 2:
|
|
logger.debug(f"{len(ori_kp)=} < 2")
|
|
return -1, None
|
|
|
|
qry_kp, qry_des = keypoints_scale_invariant(query)
|
|
matches = flann.knnMatch(qry_des, ori_des, k=2)
|
|
good = []
|
|
for pair in matches:
|
|
if (len_pair := len(pair)) == 2:
|
|
x, y = pair
|
|
if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
|
|
good.append(x)
|
|
elif len_pair == 1:
|
|
good.append(pair[0])
|
|
|
|
if draw:
|
|
from matplotlib import pyplot as plt
|
|
|
|
result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
|
|
plt.imshow(result)
|
|
plt.show()
|
|
|
|
if len(good) <= 4:
|
|
logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
|
|
return -1, None
|
|
|
|
qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
M, mask = cv2.estimateAffine2D(qry_pts, ori_pts, None, cv2.RANSAC)
|
|
|
|
if M is None:
|
|
logger.debug("M is None")
|
|
return -1, None
|
|
else:
|
|
logger.debug(f"M={M.tolist()}")
|
|
|
|
M[0][1] = M[1][0] = 0
|
|
M[0][0] = M[1][1] = scale = (M[0][0] + M[1][1]) / 2
|
|
|
|
logger.debug(f"{scale=}")
|
|
|
|
h, w = query.shape
|
|
|
|
if draw:
|
|
matches_mask = mask.ravel().tolist()
|
|
pts = np.int32([[[0, 0]], [[w - 1, h - 1]]])
|
|
dst = np.int32(cv2.transform(pts, M)).tolist()
|
|
dst = [i[0] for i in dst]
|
|
dst = np.int32(
|
|
[
|
|
[dst[0]],
|
|
[[dst[0][0], dst[1][1]]],
|
|
[dst[1]],
|
|
[[dst[1][0], dst[0][1]]],
|
|
]
|
|
)
|
|
disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
|
|
disp = cv2.polylines(disp, [dst], True, (255, 0, 0), 2, cv2.LINE_AA)
|
|
disp = cv2.drawMatches(
|
|
query,
|
|
qry_kp,
|
|
disp,
|
|
ori_kp,
|
|
good,
|
|
None,
|
|
(0, 255, 0),
|
|
matchesMask=matches_mask,
|
|
flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
|
|
)
|
|
plt.imshow(disp)
|
|
plt.show()
|
|
|
|
pts = np.int32([[[-20, -20]], [[w + 19, h + 19]]])
|
|
dst = cv2.transform(pts, M)
|
|
rect = dst.reshape(2, 2).tolist()
|
|
rect = np.array(rect, dtype=int).tolist()
|
|
|
|
rect_img = cropimg(self.origin, rect)
|
|
rh, rw = rect_img.shape
|
|
if rh <= 0 or rw <= 0:
|
|
logger.debug(f"{rh=} {rw=}")
|
|
return -1, None
|
|
|
|
disp = cv2.resize(rect_img, dsize=None, fx=1 / scale, fy=1 / scale)
|
|
dh, dw = disp.shape
|
|
if dh < h or dw < w:
|
|
logger.debug(f"{dw=} {dh=} {w=} {h=}")
|
|
return -1, None
|
|
|
|
result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
|
|
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
|
|
|
|
if draw:
|
|
disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
|
|
plt.subplot(1, 2, 1)
|
|
plt.imshow(query, cmap="gray", vmin=0, vmax=255)
|
|
plt.subplot(1, 2, 2)
|
|
plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
|
|
plt.show()
|
|
|
|
top_left = vs(max_loc, (20, 20))
|
|
scope = top_left, va(top_left, (w, h))
|
|
scope = np.float32(scope).reshape(-1, 1, 2)
|
|
scope = cv2.transform(scope, M)
|
|
scope = np.int32(scope).reshape(2, 2).tolist()
|
|
logger.debug(f"{max_val=} {scope=}")
|
|
return max_val, scope
|
|
|
|
def match3d(
|
|
self,
|
|
query: tp.GrayImage,
|
|
draw: bool = False,
|
|
scope: tp.Scope | None = None,
|
|
) -> tuple[float, tp.Scope | None]:
|
|
if self.des is None:
|
|
logger.debug(f"{self.des=}")
|
|
return -1, None
|
|
ori_kp, ori_des = self.in_scope(scope)
|
|
if len(ori_kp) < 2:
|
|
logger.debug(f"{len(ori_kp)=} < 2")
|
|
return -1, None
|
|
|
|
qry_kp, qry_des = keypoints_scale_invariant(query)
|
|
matches = flann.knnMatch(qry_des, ori_des, k=2)
|
|
good = []
|
|
for pair in matches:
|
|
if (len_pair := len(pair)) == 2:
|
|
x, y = pair
|
|
if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
|
|
good.append(x)
|
|
elif len_pair == 1:
|
|
good.append(pair[0])
|
|
|
|
if draw:
|
|
from matplotlib import pyplot as plt
|
|
|
|
result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
|
|
plt.imshow(result)
|
|
plt.show()
|
|
|
|
if len(good) <= 4:
|
|
logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
|
|
return -1, None
|
|
|
|
qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
|
|
M, mask = cv2.findHomography(qry_pts, ori_pts, cv2.RANSAC)
|
|
|
|
if M is None:
|
|
logger.debug("M is None")
|
|
return -1, None
|
|
else:
|
|
logger.debug(f"M={M.tolist()}")
|
|
|
|
h, w = query.shape
|
|
|
|
if draw:
|
|
matches_mask = mask.ravel().tolist()
|
|
pts = np.float32(
|
|
[
|
|
[0, 0],
|
|
[0, h - 1],
|
|
[w - 1, h - 1],
|
|
[w - 1, 0],
|
|
]
|
|
).reshape(-1, 1, 2)
|
|
dst = cv2.perspectiveTransform(pts, M)
|
|
disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
|
|
disp = cv2.polylines(
|
|
disp, [np.int32(dst)], True, (255, 0, 0), 2, cv2.LINE_AA
|
|
)
|
|
disp = cv2.drawMatches(
|
|
query,
|
|
qry_kp,
|
|
disp,
|
|
ori_kp,
|
|
good,
|
|
None,
|
|
(0, 255, 0),
|
|
matchesMask=matches_mask,
|
|
flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
|
|
)
|
|
plt.imshow(disp)
|
|
plt.show()
|
|
|
|
offset = (20, 20)
|
|
A = np.array(
|
|
[
|
|
[1, 0, -offset[0]],
|
|
[0, 1, -offset[1]],
|
|
[0, 0, 1],
|
|
]
|
|
)
|
|
|
|
disp = cv2.warpPerspective(
|
|
self.origin, M.dot(A), va((w, h), (40, 40)), None, cv2.WARP_INVERSE_MAP
|
|
)
|
|
result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
|
|
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
|
|
|
|
if draw:
|
|
disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
|
|
plt.subplot(1, 2, 1)
|
|
plt.imshow(query, cmap="gray", vmin=0, vmax=255)
|
|
plt.subplot(1, 2, 2)
|
|
plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
|
|
plt.show()
|
|
|
|
top_left = vs(max_loc, offset)
|
|
scope = top_left, va(top_left, (w, h))
|
|
scope = np.float32(scope).reshape(-1, 1, 2)
|
|
scope = cv2.perspectiveTransform(scope, M)
|
|
scope = np.int32(scope).reshape(2, 2).tolist()
|
|
logger.debug(f"{max_val=} {scope=}")
|
|
return max_val, scope
|