import copy
import timeimport cv2
import numpy as npdef draw_kpts(image0, image1, mkpts0, mkpts1, margin=10):H0, W0 = image0.shapeH1, W1 = image1.shapeH, W = max(H0, H1), W0 + W1 + marginout = 255 * np.ones((H, W), np.uint8)out[:H0, :W0] = image0out[:H1, W0+margin:] = image1out = np.stack([out]*3, -1)mkpts0, mkpts1 = np.round(mkpts0).astype(int), np.round(mkpts1).astype(int)c = (0, 255, 0)for (new, old) in zip(mkpts0, mkpts1):x0, y0 = new.ravel()x1, y1 = old.ravel()cv2.circle(out, (x0, y0), 2, c, -1, lineType=cv2.LINE_AA)cv2.circle(out, (x1 + margin + W0, y1), 2, c, -1,lineType=cv2.LINE_AA)return outif __name__ == "__main__":img0Path = "/training/datasets/orchard/orchard_imgs_/000130.jpg"img1Path = "/training/datasets/orchard/orchard_imgs_/000132.jpg"img0 = cv2.imread(img0Path, 0)img1 = cv2.imread(img1Path, 0)h, w = img0.shapemask = np.zeros_like(img0)mask[int(0.02 * h): int(0.98 * h), int(0.02 * w): int(0.98 * w)] = 255keypoints = cv2.goodFeaturesToTrack(img0,mask=mask,maxCorners=2048,qualityLevel=0.01,minDistance=1,blockSize=3,useHarrisDetector=False,k=0.04)print(f"keypoints , size : {keypoints.shape}")next_keypoints, status, err = cv2.calcOpticalFlowPyrLK(img0, img1, keypoints, None)H, _ = cv2.estimateAffinePartial2D(keypoints, next_keypoints, cv2.RANSAC)print(f"H : {H}")out = draw_kpts(img0, img1, keypoints , next_keypoints)cv2.imwrite("keypoints.jpg", out)