Opencv-python 用solvepnp实现棋盘测距和欧拉角测定
作者：互联网
Opencv-python 用solvepnp实现棋盘测距和欧拉角测定
- 相机标定，获取相机内参 （内参数矩阵，畸变系数）
用以标定的图像
相机标定代码：
import cv2
import numpy as np
import glob
# 相机标定
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
# 获取标定板角点的位置
objp = np.zeros((6 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
obj_points = [] # 存储3D点
img_points = [] # 存储2D点
images=glob.glob(r"C:\\*.jpg") #黑白棋盘的图片路径
i=0;
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
#print(corners2)
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
cv2.drawChessboardCorners(img, (9, 6), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
i+=1;
cv2.imwrite(r'Calibration_IMG\conimg'+str(i)+'.jpg', img)
cv2.waitKey(150)
print(len(img_points))
cv2.destroyAllWindows()
# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
print("ret:", ret)
print("mtx:\n", mtx) # 内参数矩阵--内参
print("dist:\n", dist) # 畸变系数--内参
print("rvecs:\n", rvecs) # 旋转向量--外参
print("tvecs:\n", tvecs ) # 平移向量--外参
-
测量棋盘的长宽 手动测量棋盘长宽
-
将测量完棋盘的长宽和相机内参放进config配置文件中 （下为我的config文件）在Camera_intrinsic 中替换成自己的相机内参，在Object 替换自己的棋盘大小。icol 内置了几种用以检测颜色的HSV数值，可根据实际情况进行更改。
# -*- coding: utf-8 -*-
import numpy as np
# 内参数矩阵
Camera_intrinsic = {
"mtx": np.array([[652.90522946, 0., 321.31072448], [0., 653.00569689, 255.77694781], [0., 0., 1.]],
dtype=np.double),
"dist": np.array([[-0.23397356, 0.39554466, 0.01463653, -0.00176085, -0.48223752]], dtype=np.double),
}
Object = {"height": 190, "width": 279}
# 初始的HSV GUI滑块值在程序启动时加载
icol = {
# lowHue,highHue,lowSat,highSat,lowVal,highVal
"icol_black": (0, 180, 0, 255, 0, 46), # 找黑色 适合白天
"icol_gray": (0, 180, 0, 43, 46, 220),
"icol_white": (0, 180, 0, 30, 221, 255), # 找白色 适合晚上没灯
"icol_red": (156, 180, 43, 255, 46, 255),
}
- 配置完成后先确定相机是电脑自带相机还是外接相机。电脑自带相机在VideoCapture中参数默认为0。import cv2.cv2 as cv2 这句报错的话改成import cv2 即可。
from __future__ import division
import numpy as np
import math
import cv2.cv2 as cv2
from config import *
def nothing(*arg):
pass
half_Weight = int(Object["width"] / 2)
half_Height = int(Object["height"] / 2)
# 滑动条用以调节HSV的参数
cv2.namedWindow('colorTest') # 为滑动条创建窗口
Icol = icol["icol_black"] # 初始化滑动条内的起始数据
cv2.createTrackbar('lowHue', 'colorTest', Icol[0], 255, nothing)
cv2.createTrackbar('highHue', 'colorTest', Icol[1], 255, nothing)
cv2.createTrackbar('lowSat', 'colorTest', Icol[2], 255, nothing)
cv2.createTrackbar('highSat', 'colorTest', Icol[3], 255, nothing)
cv2.createTrackbar('lowVal', 'colorTest', Icol[4], 255, nothing)
cv2.createTrackbar('highVal', 'colorTest', Icol[5], 255, nothing)
camera = cv2.VideoCapture(0)
while True:
(grabbed, frame) = camera.read()
# Get HSV values from the GUI sliders.
lowHue = cv2.getTrackbarPos('lowHue', 'colorTest')
lowSat = cv2.getTrackbarPos('lowSat', 'colorTest')
lowVal = cv2.getTrackbarPos('lowVal', 'colorTest')
highHue = cv2.getTrackbarPos('highHue', 'colorTest')
highSat = cv2.getTrackbarPos('highSat', 'colorTest')
highVal = cv2.getTrackbarPos('highVal', 'colorTest')
frameBGR = cv2.GaussianBlur(frame.copy(), (7, 7), 0) # 高斯模糊
hsv = cv2.cvtColor(frameBGR, cv2.COLOR_BGR2HSV) # 颜色转换
colorLow = np.array([lowHue, lowSat, lowVal]) # 获取low HSV 的值
colorHigh = np.array([highHue, highSat, highVal]) # 获取high HSV 的值
mask = cv2.inRange(hsv, colorLow, colorHigh)
edged = cv2.Canny(mask, 35, 100) # canny算子找轮廓
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))
dst = cv2.dilate(edged.copy(), kernel) # 扩张，使轮廓闭合。
(cnts, _) = cv2.findContours(dst.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 找轮廓
area = []
# 求最大面积
try:
for k in range(len(cnts)):
area.append(cv2.contourArea(cnts[k]))
max_idx = np.argmax(np.array(area))
cv2.drawContours(mask, cnts, max_idx, (255, 255, 255), cv2.FILLED)
(cnts, _) = cv2.findContours(mask.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)
c = max(cnts, key=cv2.contourArea)
marker = cv2.minAreaRect(c)
box = np.int0(cv2.boxPoints(marker))
cv2.drawContours(frame, [box], -1, (255, 255, 255), 2)
obj = np.array([[-half_Weight, -half_Height, 0], [half_Weight, -half_Height, 0], [half_Weight, half_Height, 0],
[-half_Weight, half_Height, 0]], dtype=np.float64) # 世界坐标
pnts = np.array(box, dtype=np.float64) # 像素坐标
# rotation_vector 旋转向量 translation_vector 平移向量
(success, rvec, tvec) = cv.solvePnP(obj, pnts, Camera_intrinsic["mtx"], Camera_intrinsic["dist"])
distance=math.sqrt(tvec[0]**2+tvec[1]**2+tvec[2]**2)/10 # 测算距离
rvec_matrix = cv2.Rodrigues(rvec)[0]
proj_matrix = np.hstack((rvec_matrix, rvec))
eulerAngles = -cv2.decomposeProjectionMatrix(proj_matrix)[6] # 欧拉角
pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]
rot_params = np.array([yaw, pitch, roll]) # 欧拉角 数组
# 这里pitch要变为其相反数(不知道为啥)
cv2.putText(frame, "%.2fcm,%.2f,%.2f,%.2f" % (distance, yaw, -pitch, roll),
(frame.shape[1] - 500, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 3)
except Exception:
pass
cv2.imshow('frame', frame)
cv2.imshow('mask-plain', mask)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
mask-plain内为经过图像处理后的图像，也是用来找轮廓的图像。调试时可参照此图进行调节。
效果图：
有问题可以在评论区留言。如果喜欢这篇文章记得点赞哦！！！
标签：欧拉角,python,cv2,solvepnp,colorTest,half,np,import,255 来源： https://blog.csdn.net/Harry_2002/article/details/115152225