#Import modules import os from unicodedata import mirrored import cv2 import numpy as np import math import time import json import sys # import transforms3d as tfs # 1、src 放标准图还是待检测图 # 2、问题一：两张图的尺寸要一致 # 3、问题二：相机的z轴 是手臂的y轴 # 4、所有参数应该是 第二个参数到第一个参数的差值::标准图必须做目标图像 # 5、if m.distance < 0.72*n.distance: 这个阈值是个关键参数，越小定位约精准 # 6、assert(isRotationMatrix(R)) 报错是因为截图或者拉伸的原因，使得角度旋转无解 # 7、截图比例会影响旋转矩阵，甚至会使得旋转矩阵无解。一定要4:3 # 8、原图中目标图像的像素数量与目标图像的像素数量，【缩放比】越接近，M的值越准确 # 9、缩放比需要打印 # 10、会改变M的参数变量包括： # img1 = cv2.resize # interpolation=cv2.INTER_AREA # cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0) 1.0 # if m.distance < 0.85 * n.distance: 0.85 # 11、编写自动化选择参数脚本！！！！自动找到最优解 # 12、可优化参数： # cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0) # cv2.resize ######################################################################## #Import pictures homo = "./image/2_origin_centor.jpg" homo_train = "./image/10_bigorigin_rz_x.jpg" homo = "./scp_file//o.jpg" homo_train = "./scp_file/6_-0.59803_0.264184_0.588617_-1.556_0.343_1.072.jpg" homo = "./error1009/grayscale1.jpg" homo_train = "./error1009/9983823d/GRAY_SCALE/WIDE/1/IMG_20221009_105934.jpg" # homo = "./error0926/c24.jpg" # homo = "./error0928/1g.jpg" # homo_train = "./error0926/IMG_20220926_165931.jpg" homo = "./error1010/sfrplus.jpg" homo_train = "./error1010/IMG_20221010_162855.jpg" # homo_train = "./060701/IMG_20230604_130953.jpg" # homo = "./error06042/C24.jpg" # homo_train = "./060701/IMG_20230603_185437.jpg" # homo_train = "./060701/IMG_20230603_185512.jpg" # homo_train = "./060701/IMG_20230603_185518.jpg" # homo_train = "./error0604/IMG_20230604_175620.jpg" # homo_train = "./error0604/IMG_20230604_175620.jpg" # homo_train = "./error06042/IMG_20230604_182406.jpg" homo = "./error0605/grayscale.jpg" homo_train = "./error0605/IMG_20230605_121151.jpg" # homo = "./error0605/sfrplus.jpg" # homo_train = "./error0605/IMG_20230605_132952.jpg" # homo = "./error0605 near/sfrplus.jpg" # homo_train = "./error0605 near/IMG_20230605_174504.jpg" homo = "./error0605gray/grayscale.jpg" homo_train = "./error0605gray/IMG_20230605_194543.jpg" def Pnp(objpos, imgpos, M, cameraMatrix, coff_dis): dst_imgpos = cv2.perspectiveTransform(imgpos, M) # imgp.append(dst_imgpos) # pnp 求解 _, rvec, tvec = cv2.solvePnP(objpos, dst_imgpos, cameraMatrix, None, flags=cv2.SOLVEPNP_UPNP) # print(cv2.Rodrigues(rvec)) rotM = cv2.Rodrigues(rvec)[0] # camera_postion = -np.mat(rotM).T * np.mat(tvec) # Ca = camera_postion.T # 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x thetaZ = math.atan2(rotM[1, 0], rotM[0, 0]) * 180.0 / math.pi thetaY = math.atan2(-1.0 * rotM[2, 0], math.sqrt(rotM[2, 1] ** 2 + rotM[2, 2] ** 2)) * 180.0 / math.pi thetaX = math.atan2(rotM[2, 1], rotM[2, 2]) * 180.0 / math.pi # 相机坐标系下值 x = tvec[0] y = tvec[1] z = tvec[2] # 进行三次旋转 def RotateByZ(Cx, Cy, thetaZ): rz = thetaZ * math.pi / 180.0 outX = math.cos(rz) * Cx - math.sin(rz) * Cy outY = math.sin(rz) * Cx + math.cos(rz) * Cy return outX, outY def RotateByY(Cx, Cz, thetaY): ry = thetaY * math.pi / 180.0 outZ = math.cos(ry) * Cz - math.sin(ry) * Cx outX = math.sin(ry) * Cz + math.cos(ry) * Cx return outX, outZ def RotateByX(Cy, Cz, thetaX): rx = thetaX * math.pi / 180.0 outY = math.cos(rx) * Cy - math.sin(rx) * Cz outZ = math.sin(rx) * Cy + math.cos(rx) * Cz return outY, outZ (x, y) = RotateByZ(x, y, -1.0 * thetaZ) (x, z) = RotateByY(x, z, -1.0 * thetaY) (y, z) = RotateByX(y, z, -1.0 * thetaX) Cx = x * -1 Cy = y * -1 Cz = z * -1 # 输出相机位置 camera_Location = [Cx, Cy, Cz] # print("相机位姿",camera_Location) # 输出相机旋转角 # print("相机旋转角度：",'[', thetaX, thetaY, -thetaZ, ']') angles = [thetaX, thetaY, thetaZ] #result = [camera_Location, "旋转角度", angles] return angles, camera_Location # 欧拉角->旋转矩阵 def eulerAnglesToRotationMatrix(theta) : R_x = np.array([[1, 0, 0 ], [0, math.cos(theta[0]), -math.sin(theta[0]) ], [0, math.sin(theta[0]), math.cos(theta[0]) ] ]) R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], [0, 1, 0 ], [-math.sin(theta[1]), 0, math.cos(theta[1]) ] ]) R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], [math.sin(theta[2]), math.cos(theta[2]), 0], [0, 0, 1] ]) R = np.dot(R_z, np.dot( R_y, R_x )) return R def _angel_zoom(rx, ry, rz, angel_zoom=1): ax, angle = tfs.euler.euler2axangle(rx, ry, rz, "sxyz") eulers = tfs.euler.axangle2euler(ax * angel_zoom, angle * angel_zoom, "sxyz") return eulers[0], eulers[1], eulers[2] # 固定 resolution = 1， sigma = 1.9 def cal_homo(homo_train, homo, mirror = False, resolution = 1, angle_zoom=1, sigma = 1.6): # 默认初始化参数 resolution = 1 angle_zoom = 1 # SIFT:该值越大，被选中的特征点越多，计算时间越长，推荐10 同时适用于广角和后置 edgeThreshold = 10 # SIFT:该值越大，精度越高，计算时间越长 sigma = 4.5 # if os.path.basename(homo) == "grayscale.jpg": sigma = 1.9 ransacReprojThreshold = 3.0 # Homography: 该值越小，精度越高，对时间没有影像 # imgWidth、imgHeight 越大，精度越高，计算时间越长 imgWidth = 1000 imgHeight = 750 # if os.path.basename(homo) == "grayscale.jpg": # imgWidth = 1600 # imgHeight = 1200 # 算法开始执行计时 start = time.time() # 读取图像与标准图像进行对比 img1 = cv2.imread(homo_train, cv2.IMREAD_GRAYSCALE) if mirror: img1 = cv2.flip(img1, 1) img1 = cv2.resize(img1, (imgWidth, imgHeight), interpolation=cv2.INTER_AREA) # 使用INTER_AREA可以提升特征点识别率 img2 = cv2.imread(homo, cv2.IMREAD_GRAYSCALE) img2 = cv2.resize(img2, (img1.shape[1], img1.shape[0]), interpolation=cv2.INTER_AREA) # 使用INTER_AREA可以提升特征点识别率，两张照片必须同样尺寸 # 1、使用 SIFT 进行特征提取 MIN_MATCH_COUNT = 20 MAX_MATCH_COUNT = 100 # 从优选择特征点，限定800个 if resolution == 1: sift = cv2.xfeatures2d.SIFT_create(nfeatures=1500, nOctaveLayers=10, contrastThreshold=0.02, edgeThreshold=edgeThreshold, sigma=sigma) else: sift = cv2.xfeatures2d.SIFT_create(nfeatures=1500) kp1, des1 = sift.detectAndCompute(img1, None) kp2, des2 = sift.detectAndCompute(img2, None) # 2、使用 flann 进行特征点匹配 FLANN_INDEX_KDTREE = 1 index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5) search_params = dict(checks = 50) flann = cv2.FlannBasedMatcher(index_params, search_params) matches = flann.knnMatch(des1, des2, k=2) # 挑选匹配情况较好的特征点 good = [] for m,n in matches: if m.distance < 0.85 * n.distance: good.append(m) # 从优选择特征点，限定MAX个特征 # good = sorted(good, key=lambda x: x.distance) # if len(good) >= MAX_MATCH_COUNT: good = good[:100] # 如果特征匹配点的数量不低于 MIN_MATCH_COUNT 设置的数量 if len(good) > MIN_MATCH_COUNT: src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2) dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2) # 3、查找单应矩阵 # 修改第三个参数 1.0 -> 5.0 # 利用基于RANSAC的鲁棒算法选择最优的四组配对点，再计算转换矩阵H(3*3)并返回,以便于反向投影错误率达到最小 # ransacReprojThreshold 设置为 1.0，精度最高 M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, ransacReprojThreshold, maxIters=1500, confidence=0.998) ## 修改 1.0 -> 5.0 # print("api_alg 单应性矩阵 M:\n", M) matchesMask = mask.ravel().tolist() # 用于测试显示，使用时注释 import matplotlib.pyplot as plt plt.ion() draw_params = dict(matchColor = (0,255,0), singlePointColor = None, matchesMask = matchesMask, flags = 2) img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, None, **draw_params) plt.imshow(img3, 'gray'),plt.show() # 4、采用PNP进行相机的姿态估计 h, w = img1.shape # 模拟实际物体世界坐标参数 if resolution == 1: objpos = np.float32([[0.001, 0.001, 0], [0.001, 1.200, 0], [1.600, 1.200, 0], [1.600, 0.001, 0]]).reshape(-1, 1, 3) else: objpos = np.float32([[0.0008, 0.0008, 0], [0.0008, 0.5994, 0], [0.7992, 0.5994, 0], [0.7992, 0.0008, 0]]).reshape(-1, 1, 3) imgpos = np.float32([[0,0], [0,h-1], [w-1,h-1], [w-1,0]]).reshape(-1, 1, 2) # 模拟相机参数 focal_length = img1.shape[1] # 使用摄像头的宽度(像素)代表焦距 center = (img1.shape[1] / 2, img1.shape[0] / 2) # 使用图像的中心代替光学中心 K = np.array([[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]]) angles, T = Pnp(objpos, imgpos, M, K, None) rand = [angles[0] / 180 * math.pi, angles[1] / 180 * math.pi, angles[2] / 180 * math.pi] R = eulerAnglesToRotationMatrix(angles) # print("api_alg 图像计算旋转矩阵 R: \n", R) # print("api_alg 图像计算角度偏移 a: ", angles) # print("api_alg 图像计算弧度偏移 r: ", rand) # print("api_alg 图像计算平移偏移 t: ", [[T[0][0]], [T[1][0]], [T[2][0]]]) # print("mmatches count: ", len(matches), " good count: ", len(good), " matchesMask count: ", len(matchesMask)) # print(rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2]) print("mmatches count: ", len(matches), " good count: ", len(good), " matchesMask count: ", len(matchesMask)) print(rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2]) end = time.time() print("api_alg 程序的运行时间为：{}".format(end-start)) # print(rand[0], rand[1], rand[2]) if angle_zoom != 1: rand[0], rand[1], rand[2] = _angel_zoom(rand[0], rand[1], rand[2], angle_zoom) return rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2], R, T # img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA) else: # print ("api_alg : Not enough matches are found - %d/%d") % (len(good), MIN_MATCH_COUNT) matchesMask = None # print("good features is too few!", "mmatches count: ", len(matches), " good count: ", len(good)) if __name__ == '__main__': cal_homo(homo_train, homo) # resp = {'code': 0, 'message': 'success'} # if len(sys.argv) < 3: # resp['code'] = -1 # resp['message'] = '参数错误！' # else: # img1 = sys.argv[1] # img2 = sys.argv[2] # mirror = False # resolution = 0 # angle_zoom = 1 # sigma = 1.3 # if len(sys.argv) >= 4: # mirror = sys.argv[3].lower() == 'true' # if len(sys.argv) >= 5: # resolution = int(sys.argv[4]) # if len(sys.argv) >= 6: # angle_zoom = float(sys.argv[5]) # if len(sys.argv) >= 7: # sigma = float(sys.argv[6]) # result = cal_homo(img1, img2, mirror, resolution, angle_zoom, sigma) # if result is not None: # resp['result'] = { # 'drx': result[0], # 'dry': result[1], # 'drz': result[2], # 'hs': result[3], # 'ws': result[4], # 'xf': result[5], # 'yf': result[6], # 'dx': result[8][0][0], # 'dy': result[8][1][0], # 'dz': result[8][2][0], # } # else: # resp['code'] = -1 # resp['message'] = '解析错误！' # print(json.dumps(resp, indent = 2, ensure_ascii = False))