210 lines
7.4 KiB
Python
210 lines
7.4 KiB
Python
#Import modules
|
||
import cv2
|
||
import numpy as np
|
||
import math
|
||
import time
|
||
import sys
|
||
import json
|
||
|
||
# 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"
|
||
|
||
def cal_homo(homo_train, homo):
|
||
|
||
start = time.time()
|
||
# interpolation=cv2.INTER_AREA 必须加
|
||
img1 = cv2.imread(homo_train, cv2.IMREAD_GRAYSCALE)
|
||
img1 = cv2.resize(img1, (int(img1.shape[1] / 4), int(img1.shape[0] / 4)), interpolation=cv2.INTER_AREA) # 必须加
|
||
img2 = cv2.imread(homo, cv2.IMREAD_GRAYSCALE)
|
||
img2 = cv2.resize(img2, (img1.shape[1], img1.shape[0]), interpolation=cv2.INTER_AREA) # 必须加，两张照片必须同样尺寸
|
||
|
||
#Feature Extraction
|
||
MIN_MATCH_COUNT = 20 ##修改
|
||
sift = cv2.xfeatures2d.SIFT_create()
|
||
|
||
kp1, des1 = sift.detectAndCompute(img1, None)
|
||
kp2, des2 = sift.detectAndCompute(img2, None)
|
||
|
||
FLANN_INDEX_KDTREE = 0
|
||
|
||
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)
|
||
|
||
# store all the good matches as per Lowe's ratio test.
|
||
good = []
|
||
for m,n in matches:
|
||
if m.distance < 0.85 * n.distance:
|
||
good.append(m)
|
||
|
||
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)
|
||
|
||
|
||
#Finds homography
|
||
# M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 1.0) ## 修改 1.0 -> 5.0
|
||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3, maxIters=2000, confidence=0.99) ## 修改 1.0 -> 5.0
|
||
print("api_alg 单应性矩阵 M:\n", M)
|
||
|
||
matchesMask = mask.ravel().tolist()
|
||
|
||
h,w = img1.shape
|
||
pts = np.float32([ [0,0], [0,h-1], [w-1,h-1], [w-1,0] ]).reshape(-1, 1, 2)
|
||
dst = cv2.perspectiveTransform(pts, M)
|
||
|
||
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
|
||
|
||
# for draw
|
||
# draw_params = dict(matchColor = (0,255,0), # draw matches in green color
|
||
# singlePointColor = None,
|
||
# matchesMask = matchesMask, # draw only inliers
|
||
# flags = 2)
|
||
# img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, None, **draw_params)
|
||
# plt.imshow(img3, 'gray'),plt.show()
|
||
|
||
# camera pose from H whitout K
|
||
def cameraPoseFromHomography(H):
|
||
norm1 = np.linalg.norm(H[:, 0])
|
||
norm2 = np.linalg.norm(H[:, 1])
|
||
tnorm = (norm1 + norm2) / 2.0
|
||
H1 = H[:, 0] / norm1
|
||
H2 = H[:, 1] / norm2
|
||
H3 = np.cross(H1, H2)
|
||
T = H[:, 2] / tnorm
|
||
return np.array([H1, H2, H3, T]).transpose()
|
||
|
||
RT = cameraPoseFromHomography(M)
|
||
# print("api_alg RT whitout K", RT)
|
||
|
||
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]])
|
||
|
||
# camera pose from H whit K
|
||
num, Rs, Ts, Ns = cv2.decomposeHomographyMat(M, K)
|
||
# print("api_alg Rs", Rs)
|
||
# print("api_alg Ts", Ts)
|
||
|
||
# Checks if a matrix is a valid rotation matrix.
|
||
def isRotationMatrix(R) :
|
||
Rt = np.transpose(R)
|
||
shouldBeIdentity = np.dot(Rt, R)
|
||
I = np.identity(3, dtype = R.dtype)
|
||
n = np.linalg.norm(I - shouldBeIdentity)
|
||
return n < 1e-6
|
||
|
||
# Calculates rotation matrix to euler angles
|
||
# The result is the same as MATLAB except the order
|
||
# of the euler angles ( x and z are swapped ).
|
||
def rotationMatrixToEulerAngles(R) :
|
||
|
||
# assert(isRotationMatrix(R))
|
||
if not isRotationMatrix(R):
|
||
# print("api_alg : assert(isRotationMatrix(R)) !!!!")
|
||
return np.array([0, 0, 0])
|
||
|
||
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
|
||
|
||
singular = sy < 1e-6
|
||
|
||
if not singular :
|
||
x = math.atan2(R[2,1] , R[2,2])
|
||
y = math.atan2(-R[2,0], sy)
|
||
z = math.atan2(R[1,0], R[0,0])
|
||
else :
|
||
x = math.atan2(-R[1,2], R[1,1])
|
||
y = math.atan2(-R[2,0], sy)
|
||
z = 0
|
||
|
||
return np.array([x, y, z])
|
||
|
||
# Conver the 4 rotation matrix solutions into XYZ Euler angles
|
||
# 获取最后一组值
|
||
i = 0
|
||
for i in range(0, len(Rs)):
|
||
# 获取最后一组值
|
||
angles = rotationMatrixToEulerAngles(Rs[i])
|
||
x = np.degrees(angles[0])
|
||
y = np.degrees(angles[1])
|
||
z = np.degrees(angles[2])
|
||
anglesDeg = np.array([x,y,z])
|
||
# print("api_alg 角度: ", anglesDeg)
|
||
|
||
R = Rs[num - 1]
|
||
T = Ts[num - 1]
|
||
|
||
# print("api_alg : Rs矩阵")
|
||
# for i in range(len(Rs)): print(Rs[i])
|
||
# print("api_alg : Ts矩阵\n", Ts)
|
||
|
||
rand = [x / 180 * math.pi, y / 180 * math.pi, z / 180 * math.pi]
|
||
|
||
print("api_alg 图像计算旋转矩阵 R: \n", R)
|
||
print("api_alg 图像计算角度偏移 a: ", anglesDeg)
|
||
print("api_alg 图像计算弧度偏移 r: ", rand)
|
||
print("api_alg 图像计算平移偏移 t: ", [T[0][0], T[1][0], T[2][0]])
|
||
|
||
end = time.time()
|
||
print("api_alg 程序的运行时间为：{}".format(end-start))
|
||
print(rand[0], rand[1], rand[2])
|
||
|
||
return rand[0], rand[1], rand[2], M[0][0], M[1][1], M[0][2], M[1][2], R, T
|
||
|
||
if __name__ == '__main__':
|
||
resp = {'code': 0, 'message': 'success'}
|
||
if len(sys.argv) != 3:
|
||
resp['code'] = -1
|
||
resp['message'] = '参数错误！'
|
||
else:
|
||
img1 = sys.argv[1]
|
||
img2 = sys.argv[2]
|
||
result = cal_homo(img1, img2)
|
||
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],
|
||
}
|
||
|
||
print(json.dumps(resp, indent = 0, ensure_ascii = False))
|