333 lines
12 KiB
Python
333 lines
12 KiB
Python
#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))
|