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))
|