PnP(Perspective-n-Point)是求解3D到2D点的对应方法。不论是相机和雷达的标定还是相机和相机的标定都可以使用PNP来解决，即通过不同坐标系下相同的点对求解变换矩阵。
这里相机多用棋盘格中的角点来实现点的提取。流行方法为张正友标定法，至于详细原理可点击我的博客https://www.cnblogs.com/tangjunjun/p/16240878.html查看，本博客主要使用代码
实现外参求解与相机标定的内参和畸变系数求解。
代码及标定图链接：https://pan.baidu.com/s/1ujX19IUV0EPSIMyIcBnClA?pwd=r63z （相机标定与外参求解.zip文件）
提取码：r63z
提取码：r63z
①相机标定，求解内参与畸变系数；
②通过畸变系数矫正，获得矫正的图像；
③使用PnP方法求解外参；
④附带三点对应求解空间变换的旋转向量(矩阵)和平移向量(主要记录此方法)；
def rigid_transform_RT(lidarPoints, rtkPoints):
'''
:param lidarPoints: 世界坐标系<---原始坐标，需要转换的坐标系
:param rtkPoints: 目标世界坐标系<---目标三维坐标，已转换的坐标
:return:
# Input: expects Nx3 matrix of points
# Returns R,t
# R = 3x3 rotation matrix
# t = 3x1 column vector
demo:
rigid_transform_RT(L1,S1)
'''
Pa = np.array(lidarPoints)
Pb = np.array(rtkPoints)
N = Pa.shape[0] # total points
centroid_Pa = np.mean(Pa, axis=0)
centroid_Pb = np.mean(Pb, axis=0)
# centre the points
H_Pa = Pa - np.tile(centroid_Pa, (N, 1))
H_Pb = Pb - np.tile(centroid_Pb, (N, 1))
H = np.matmul(np.transpose(H_Pa), H_Pb)
U, S, V = np.linalg.svd(H)
R = np.matmul(V.T, U.T)
if np.linalg.det(R) < 0:
print("det(R) < R, reflection detected!, correcting for it ...\n")
V[2, :] *= -1
R = np.matmul(V.T, U.T)
T = centroid_Pb - np.matmul(R, centroid_Pa)
print("R:\n{}\nT:\n{}".format(R, T))
return R, T
以上为世界坐标系转到像素坐标系方法的代码，以下代码为整体代码：
import numpy as np
import os
import cv2
import glob
# *******************utils*********************#
import matplotlib.pyplot as plt
import random
def draw_circle(img, coord):
coord = np.array(coord).reshape(-1)
cv2.circle(img, (int(coord[0]), int(coord[1])), 2, (0, 0, 255), -1)
return img
def draw_axis(img, O, OX, OY, OZ):
color = (0, 0, 255)
frontsize = 0.5
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OX[0]), int(OX[1])), color, 1)
cv2.putText(img, 'X', (int(OX[0]), int(OX[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OY[0]), int(OY[1])), color, 1)
cv2.putText(img, 'Y', (int(OY[0]), int(OY[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OZ[0]), int(OZ[1])), color, 1)
cv2.putText(img, 'Z', (int(OZ[0]), int(OZ[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
return img
def show_img(img):
plt.imshow(img)
plt.show()
# *******************************通过内参联合求解外参*******************#
def get_pixel(coord, mtx, R, T):
mtx = np.array(mtx)
R = np.array(R)
T = np.array(T).reshape(3, 1)
coord = np.array(coord).reshape(-1, 1)
RT = np.hstack((R, T))
XYZ_camera = np.matmul(RT, coord)
# XYZ_camera = XYZ_camera / XYZ_camera[-1, -1]
x, y, z = np.matmul(mtx, XYZ_camera)
x = x / z
y = y / z
print('x={}\ty={}'.format(x[0], y[0]))
return [x[0], y[0]]
# ***********************利用opencv相机标定**************#
# 相机标定，主要求内参和畸变系数
def calibration_camera(img_root, rand_count=20):
board_h = 8
board_w = 11
objp = np.zeros((board_h * board_w, 3), np.float32)
objp[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = 60 * objp # 打印棋盘格一格的边长为2.6cm
obj_points = [] # 存储3D点
img_points = [] # 存储2D点
images = glob.glob(os.path.join(img_root, '*.jpg')) # 黑白棋盘的图片路径
random.shuffle(images)
if rand_count > len(images):
rand_count = len(images)
for fname in images[:rand_count]:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (board_w, board_h), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
cv2.drawChessboardCorners(img, (board_w, board_h), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
cv2.imshow("img", img)
cv2.waitKey(10)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
# v_rot_mat, _ = cv2.Rodrigues(np.array(v_rot).reshape(-1))
# print("旋转矩阵=", v_rot_mat)
# print("内参=", mtx)
# print("畸变系数=", dist)
# print("旋转向量=", rvecs)
# print("平移向量=", tvecs)
# 反投影误差
total_error = 0
for i in range(len(obj_points)):
imgpoints2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(img_points[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
total_error += error
# print("total error: ", total_error / len(obj_points))
return mtx, dist, total_error
# 旋转向量和平移向量求解
def calibration_RT(points_3D, points_2D, cameraMatrix, distCoeffs):
points_3D = np.array(points_3D)
points_2D = np.array(points_2D)
cameraMatrix = np.array(cameraMatrix).astype(np.float32)
distCoeffs = np.array(distCoeffs).astype(np.float32)
_, rvecs, tvecs, inliers = cv2.solvePnPRansac(points_3D.reshape(-1, 1, 3),
points_2D.reshape(-1, 1, 2),
cameraMatrix,
distCoeffs
)
R, _ = cv2.Rodrigues(rvecs)
print('R:\n', R)
print('rvecs:\n', rvecs)
print('tvecs:\n', tvecs)
return R, tvecs
def get_calibration_pixel(coord, rvecs, tvecs, mtx, dist):
imgpts, jac = cv2.projectPoints(coord, rvecs, tvecs, mtx, dist)
return imgpts
# 消除畸变
def revise_img(img_root, mtx, dist):
mtx = np.array(mtx)
dist = np.array(dist).reshape(1, 5)
img = cv2.imread(img_root)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('./revise_img.jpg', dst)
return dst
# 自动寻找3d点与像素点对应坐标（只针对标定版）
def find_point_3d2d(img_root, save=True):
board_h = 8
board_w = 11
objp = np.zeros((board_h * board_w, 3), np.float32)
objp[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
points_3d = 60 * objp # 打印棋盘格一格的边长为2.6cm
points_2d = None # 存储2D点
img = cv2.imread(img_root)
img_rand = img.copy()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
rand_p2, rand_p3 = None, None
ret, corners = cv2.findChessboardCorners(gray, (board_w, board_h), None)
print('ret=', ret)
if ret:
corners = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
cv2.drawChessboardCorners(img, (board_w, board_h), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
points_2d = corners.reshape(-1, 2)
if save:
file_write_obj = open('./save_points_3d2d.txt', 'w', encoding='utf-8') # 以写的方式打开文件，如果文件不存在，就会自动创建
for i, p2 in enumerate(points_2d):
text = '3d:\t' + str(points_3d[i]) + '\t2d\t' + str(p2) + '\n'
file_write_obj.writelines(text)
k = 10
rand_int = random.sample(range(1, len(points_2d)), k)
# print('rand_int=',rand_int)
rand_int = [48, 55, 85, 22, 73, 39, 68, 31, 78, 71]
rand_p2 = points_2d[rand_int]
rand_p3 = points_3d[rand_int]
file_write_obj.close()
# 图像展示选择坐标点
for inds in rand_int:
pp2 = points_2d[inds]
img_rand = draw_circle(img_rand, pp2)
cv2.imwrite('./calibration_point.jpg', img_rand)
show_img(img)
return points_3d, points_2d, rand_p3, rand_p2
def draw_board(img_root, mtx, R, T, dist=None):
img = cv2.imread(img_root)
mtx = np.array(mtx)
dist = np.array(dist).reshape(1, 5) if dist is not None else None
for i in range(10):
for j in range(8):
coord = [i * 60, j * 60, 0.0, 1]
coord = np.array(coord)
coord_pixel = get_pixel(coord, mtx, R, T)
RMat = cv2.Rodrigues(R)[0]
coord = [[i * 60, j * 60, 0.0]]
# coord_pixel, _ = cv2.projectPoints(coord,RMat, T, mtx, dist)
img = draw_circle(img, coord_pixel)
# # 画坐标轴
L = 80
O = get_pixel([0, 0, 0, 1], mtx, R, T)
OX = get_pixel([L, 0, 0, 1], mtx, R, T)
OY = get_pixel([0, L, 0, 1], mtx, R, T)
OZ = get_pixel([0, 0, L, 1], mtx, R, T)
img = draw_axis(img, O, OX, OY, OZ)
show_img(img)
cv2.imwrite('./predict_result.jpg', img)
def draw_space(img_root, coord, mtx, R, T):
img = cv2.imread(img_root)
for c in coord:
c = np.array(c)
coord_pixel = get_pixel(c, mtx, R, T)
img = draw_circle(img, coord_pixel)
# # 画坐标轴
L = 2
O = get_pixel([0, 0, 0, 1], mtx, R, T)
OX = get_pixel([L, 0, 0, 1], mtx, R, T)
OY = get_pixel([0, L, 0, 1], mtx, R, T)
OZ = get_pixel([0, 0, L, 1], mtx, R, T)
img = draw_axis(img, O, OX, OY, OZ)
show_img(img)
cv2.imwrite('./predict_result.jpg', img)
def get_coord(M=6, N=15):
# M, N = 6, 15 # 行与列
coord_size = 1
coord = []
for i in range(M):
for j in range(N):
coord.append([i * coord_size, j * coord_size, 0, 1])
# for i in [-1, -2, -3, -4]:
# for j in range(N):
# coord.append([i * coord_size, j * coord_size, 0, 1])
return coord
if __name__ == '__main__':
predict_space = False # 实际空间坐标标定
predict_board = False # 决定测试标定版输出结果
calibration = True
if calibration:
img_root = r'C:\Users\vrc\Desktop\RadarCamera\code\camera_main\data\data09206\img_ori'
mtx_best, dist_best, total_error_best = None, None, 100000
for i in range(50):
mtx, dist, total_error = calibration_camera(img_root, rand_count=14)
if total_error < total_error_best:
total_error_best = total_error
mtx_best = mtx
dist_best = dist
print("最小误差=", total_error_best)
print("最优内参=", mtx_best)
print("最优畸变系数=", dist_best)
print("最小误差=", total_error_best)
mtx = [[1.90555223e+03, 0.00000000e+00, 1.01084265e+03],
[0.00000000e+00, 1.89395850e+03, 5.29217920e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]
dist = [-0.39262705, 0.16798904, -0.00529946, 0.0008667, -0.37552875]
if predict_board:
#只针对标定板
img_root = r'C:\Users\vrc\Desktop\RadarCamera\data\phone\2.jpg'
img = revise_img(img_root, mtx, dist)
img_root = './revise_img.jpg'
_, _, rand_p3, rand_p2 = find_point_3d2d(img_root)
# dist = [0, 0, 0, 0, 0]
R, T = calibration_RT(rand_p3, rand_p2, mtx, dist)
# 通过内外参求解像素坐标
# img_root = './revise_img.jpg'
draw_board(img_root, mtx, R, T, dist=dist)
if predict_space:
img_root = r'C:\Users\vrc\Desktop\RadarCamera\code\camera_main\data\data0926-2\1.png'
img = revise_img(img_root, mtx, dist)
img_root = './revise_img.jpg'
p3 = [[1, 1, 0], [0.0, 3, 0], [2, 2.0, 0], [1, 5, 0], [2, 1, 0]]
p2 = [[939.0, 462], [702.0, 234], [1239, 344], [947, 134], [1312, 478]]
dist = [0, 0, 0, 0, 0]
R, T = calibration_RT(p3, p2, mtx, dist)
# coord = [[1, 3.0, 0, 1], [4, 5, 0, 1], [2, 6, 0, 1], [2, 2,0, 1], [-1, 4, 0, 1]]
coord = get_coord(M=3, N=15)
draw_space(img_root, coord, mtx, R, T)
世界坐标系转像素结果展示:
添加验证场景(2022-0929)：
添加场景一：
添加场景二：
介绍cv2.projectPoints函数，该函数可以将世界坐标系转为对应原始图像像素坐标系，无需上面那样矫正。
imgpts, jac = cv2.projectPoints(coord, rvecs, tvecs, mtx, dist)
参数格式如下图：
其中mtx与dist分别为相机标定求解的内参与畸变系数，rvecs与tvecs为旋转向量和平移向量，即为外参，返回imgpts坐标为
原始图像坐标(未矫正的坐标)。
我也将直接使用原始图像求解代码直接粘贴此处：
import numpy as np
import os
import cv2
import glob
# *******************utils*********************#
import matplotlib.pyplot as plt
import random
import shutil
def draw_circle(img, coord):
coord = np.array(coord).reshape(-1)
try:
cv2.circle(img, (int(coord[0]), int(coord[1])), 2, (0, 0, 255), -1)
except:
pass
return img
def build_dir(root):
import os
if not os.path.exists(root):
os.makedirs(root)
return root
def draw_axis(img, O, OX, OY, OZ):
color = (0, 255, 0)
frontsize = 0.5
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OX[0]), int(OX[1])), color, 1)
cv2.putText(img, 'X', (int(OX[0]), int(OX[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OY[0]), int(OY[1])), color, 1)
cv2.putText(img, 'Y', (int(OY[0]), int(OY[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
img = cv2.line(img, (int(O[0]), int(O[1])), (int(OZ[0]), int(OZ[1])), color, 1)
cv2.putText(img, 'Z', (int(OZ[0]), int(OZ[1])), cv2.FONT_HERSHEY_SIMPLEX, frontsize, color, 1)
return img
def show_img(img):
plt.imshow(img)
plt.show()
# *******************************通过内参联合求解外参*******************#
def get_pixel(coord, mtx, R, T):
mtx = np.array(mtx)
R = np.array(R)
T = np.array(T).reshape(3, 1)
coord = np.array(coord).reshape(-1, 1)
RT = np.hstack((R, T))
XYZ_camera = np.matmul(RT, coord)
# XYZ_camera = XYZ_camera / XYZ_camera[-1, -1]
x, y, z = np.matmul(mtx, XYZ_camera)
x = x / z
y = y / z
# print('x={}\ty={}'.format(x[0], y[0]))
return [x[0], y[0]]
# ***********************利用opencv相机标定**************#
# 相机标定，主要求内参和畸变系数
def calibration_camera(img_root, rand_count=6):
board_h = 8
board_w = 11
objp = np.zeros((board_h * board_w, 3), np.float32)
objp[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = 60 * objp # 打印棋盘格一格的边长为2.6cm
obj_points = [] # 存储3D点
img_points = [] # 存储2D点
images = glob.glob(os.path.join(img_root, '*.jpg')) # 黑白棋盘的图片路径
random.shuffle(images)
if rand_count > len(images):
rand_count = len(images)
for fname in images[:rand_count]:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (board_w, board_h), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
cv2.drawChessboardCorners(img, (board_w, board_h), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
cv2.imshow("img", img)
cv2.waitKey(10)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
# v_rot_mat, _ = cv2.Rodrigues(np.array(v_rot).reshape(-1))
# print("旋转矩阵=", v_rot_mat)
# print("内参=", mtx)
# print("畸变系数=", dist)
# print("旋转向量=", rvecs)
# print("平移向量=", tvecs)
# 反投影误差
total_error = 0
for i in range(len(obj_points)):
imgpoints2, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(img_points[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
total_error += error
# print("total error: ", total_error / len(obj_points))
return mtx, dist, total_error
# 旋转向量和平移向量求解
def calibration_RT(points_3D, points_2D, cameraMatrix, distCoeffs):
points_3D = np.array(points_3D)
points_2D = np.array(points_2D)
cameraMatrix = np.array(cameraMatrix).astype(np.float32)
distCoeffs = np.array(distCoeffs).astype(np.float32)
_, rvecs, tvecs, inliers = cv2.solvePnPRansac(points_3D.reshape(-1, 1, 3),
points_2D.reshape(-1, 1, 2),
cameraMatrix,
distCoeffs
)
R, _ = cv2.Rodrigues(rvecs)
# print('R:\n', R)
# print('rvecs:\n', rvecs)
# print('tvecs:\n', tvecs)
return R, rvecs, tvecs
def get_calibration_pixel(coord, rvecs, tvecs, mtx, dist):
mtx = np.array(mtx)
dist = np.array(dist).reshape(1, 5)
imgpts, jac = cv2.projectPoints(coord, rvecs, tvecs, mtx, dist)
return imgpts
# 消除畸变
def revise_img(img_root, mtx, dist):
mtx = np.array(mtx)
dist = np.array(dist).reshape(1, 5)
img = cv2.imread(img_root)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('./revise_img.jpg', dst)
return dst
# 自动寻找3d点与像素点对应坐标（只针对标定版）
def find_point_3d2d(img_root, save=True):
board_h = 8
board_w = 11
objp = np.zeros((board_h * board_w, 3), np.float32)
objp[:, :2] = np.mgrid[0:board_w, 0:board_h].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
points_3d = 60 * objp # 打印棋盘格一格的边长为2.6cm
points_2d = None # 存储2D点
img = cv2.imread(img_root)
img_rand = img.copy()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
rand_p2, rand_p3 = None, None
ret, corners = cv2.findChessboardCorners(gray, (board_w, board_h), None)
print('ret=', ret)
if ret:
corners = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
cv2.drawChessboardCorners(img, (board_w, board_h), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
points_2d = corners.reshape(-1, 2)
if save:
file_write_obj = open('./save_points_3d2d.txt', 'w', encoding='utf-8') # 以写的方式打开文件，如果文件不存在，就会自动创建
for i, p2 in enumerate(points_2d):
text = '3d:\t' + str(points_3d[i]) + '\t2d\t' + str(p2) + '\n'
file_write_obj.writelines(text)
k = 10
rand_int = random.sample(range(1, len(points_2d)), k)
# print('rand_int=',rand_int)
rand_int = [48, 55, 85, 22, 73, 39, 68, 31, 78, 71]
rand_p2 = points_2d[rand_int]
rand_p3 = points_3d[rand_int]
file_write_obj.close()
# 图像展示选择坐标点
for inds in rand_int:
pp2 = points_2d[inds]
img_rand = draw_circle(img_rand, pp2)
cv2.imwrite('./calibration_point.jpg', img_rand)
show_img(img)
return points_3d, points_2d, rand_p3, rand_p2
def draw_board(img_root, mtx, R, T, dist=None):
img = cv2.imread(img_root)
mtx = np.array(mtx)
dist = np.array(dist).reshape(1, 5) if dist is not None else None
for i in range(10):
for j in range(8):
coord = [i * 60, j * 60, 0.0, 1]
coord = np.array(coord)
coord_pixel = get_pixel(coord, mtx, R, T)
RMat = cv2.Rodrigues(R)[0]
coord = [[i * 60, j * 60, 0.0]]
# coord_pixel, _ = cv2.projectPoints(coord,RMat, T, mtx, dist)
img = draw_circle(img, coord_pixel)
# # 画坐标轴
L = 80
O = get_pixel([0, 0, 0, 1], mtx, R, T)
OX = get_pixel([L, 0, 0, 1], mtx, R, T)
OY = get_pixel([0, L, 0, 1], mtx, R, T)
OZ = get_pixel([0, 0, L, 1], mtx, R, T)
img = draw_axis(img, O, OX, OY, OZ)
show_img(img)
cv2.imwrite('./predict_result.jpg', img)
def draw_space(img_root, coord, mtx, R, T):
img = cv2.imread(img_root)
for c in coord:
c = np.array(c)
coord_pixel = get_pixel(c, mtx, R, T)
img = draw_circle(img, coord_pixel)
# # 画坐标轴
L = 2000
O = get_pixel([0, 0, 0, 1], mtx, R, T)
OX = get_pixel([L, 0, 0, 1], mtx, R, T)
OY = get_pixel([0, L, 0, 1], mtx, R, T)
OZ = get_pixel([0, 0, L, 1], mtx, R, T)
img = draw_axis(img, O, OX, OY, OZ)
show_img(img)
cv2.imwrite('./predict_result.jpg', img)
def draw_space_new(img_root, coord, mtx, rvecs, tvecs, dist):
img = cv2.imread(img_root)
for c in coord:
c = np.array(c)
c = np.array([c[0], c[1], c[2]], dtype=np.float32)
coord_pixel = get_calibration_pixel(c, rvecs, tvecs, mtx, dist)
coord_pixel = coord_pixel.reshape(1, 2)
img = draw_circle(img, coord_pixel)
# # 画坐标轴
L = 2000
coord_pixel = get_calibration_pixel(np.array([0, 0, 0.0]), rvecs, tvecs, mtx, dist)
O = coord_pixel.reshape(-1)
coord_pixel = get_calibration_pixel(np.array([L, 0, 0.0]), rvecs, tvecs, mtx, dist)
OX = coord_pixel.reshape(-1)
coord_pixel = get_calibration_pixel(np.array([0, L, 0.0]), rvecs, tvecs, mtx, dist)
OY = coord_pixel.reshape(-1)
coord_pixel = get_calibration_pixel(np.array([0, 0.0, L]), rvecs, tvecs, mtx, dist)
OZ = coord_pixel.reshape(-1)
img = draw_axis(img, O, OX, OY, OZ)
show_img(img)
cv2.imwrite('./predict_result.jpg', img)
def get_coord(X_range=[-5,8], Y_range=[-3,15], coord_size=1):
# M, N = 6, 15 # 行与列
# coord_size = 1
coord = []
for i in list(range(X_range[0], X_range[1], 1)):
for j in list(range(Y_range[0], Y_range[1], 1)):
coord.append([i * coord_size+500, j * coord_size+500, 0, 1])
return coord
def get_best_pnp_index(p3, p2, mtx, dist, N_pnp=4, loop_n=200):
'''
:param p3: 空间坐标
:param p2: 图像坐标
:param mtx: 内参
:param dist: 畸变系数
:param N_pnp: 决定多少个点求解pnp外参
:param loop_n: 迭代次数
:return: p3与p2对应中用于求解pnp的索引对应及最佳R与T
'''
N_p3 = N_pnp # 设置选择计算pnp点数
index = np.arange(len(p3))
p3 = np.array(p3)
p2 = np.array(p2)
d_error_best = 10000000
index_best = None
for _ in range(loop_n):
random.shuffle(index)
index_pnp = index[:N_p3]
index_test = index[N_p3:]
p3_new = p3[index_pnp, :]
p2_new = p2[index_pnp, :]
R, rvecs, tvecs = calibration_RT(p3_new, p2_new, mtx, dist)
p3_test = p3[index_test, :]
p2_test = p2[index_test, :]
d_error = 0
print('\n\n')
for i, p in enumerate(p3_test):
coord = np.array([p[0], p[1], p[2]], dtype=np.float32)
pre_xy = get_calibration_pixel(coord, rvecs, tvecs, mtx, dist)
x, y = np.array(pre_xy).reshape(-1)
d = np.sqrt(np.square(p2_test[i][0] - x) + np.square(p2_test[i][1] - y))
d_error = d_error + d
print("坐标索引:{}\t原始坐标--预测坐标:{}--{}\terror:{}".format(index_test[i], (p2[i][0], p2[i][1]), (x, y), d))
d_error = d_error / (len(p3) - N_pnp)
if d_error < d_error_best:
d_error_best = d_error
index_best = index[:N_p3]
print("更新pnp最佳点index:", index_best)
print("更新最佳距离误差:", d_error)
print("pnp最佳点index:", index[:N_p3])
print("最佳距离误差:", d_error_best)
# 求解最佳对应外参值
p3_new = p3[index_best, :]
p2_new = p2[index_best, :]
R, rvecs, tvecs = calibration_RT(p3_new, p2_new, mtx, dist)
print("best rvecs=", rvecs)
print("best tvecs=", tvecs)
return index_best, rvecs, tvecs
if __name__ == '__main__':
calibration = False
predict_space = True # 实际空间坐标标定
predict_space_onepoint = False # 测试空间中一点
predict_board = False # 决定测试标定版输出结果
resize_img = False # 将一个文件夹图片进行resize畸变矫正
if calibration:
img_root = r'C:\Users\vrc\Desktop\RadarCamera\data\123_0927\3'
mtx_best, dist_best, total_error_best = None, None, 100000
for i in range(60):
mtx, dist, total_error = calibration_camera(img_root, rand_count=20)
if total_error < total_error_best:
total_error_best = total_error
mtx_best = mtx
dist_best = dist
print("最小误差=", total_error_best)
print("最优内参=", mtx_best)
print("最优畸变系数=", dist_best)
print("最小误差=", total_error_best)
mtx = [[1.91272616e+03, 0.00000000e+00, 1.00189492e+03],
[0.00000000e+00, 1.90272200e+03, 5.08191451e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]
dist = [-4.47559224e-01, 3.19804795e-01, 3.29548350e-04, 1.01405096e-03,
-1.99962118e-01]
if predict_board:
# 只针对标定板
img_root = r'C:\Users\vrc\Desktop\RadarCamera\data\123_0927\2\5.jpg'
img = revise_img(img_root, mtx, dist)
img_root = './revise_img.jpg'
_, _, rand_p3, rand_p2 = find_point_3d2d(img_root)
# dist = [0, 0, 0, 0, 0]
R, rvecs, T = calibration_RT(rand_p3, rand_p2, mtx, dist)
# 通过内外参求解像素坐标
# img_root = './revise_img.jpg'
draw_board(img_root, mtx, R, T, dist=dist)
if predict_space:
img_root = r'C:\Users\vrc\Desktop\RadarCamera\data\data_0929\img_ori\13.jpg'
# p3 = [[1000, 1000, 0], [-1000, 11000.0, 0], [-1000, 2000, 100], [1000, 2000, 0]]
# p2 = [[861, 755.0], [320, 236], [178, 639], [787, 634]]
p3 = [[2000, 2000, 0.0], [6000, 3000, 0], [3000, 5000, 0], [10000, 4000, 0]]
p2 = [[1170, 555.0], [1252, 457], [1508, 525], [1303, 393]]
R, rvecs, T = calibration_RT(p3, p2, mtx, dist)
# coord = [[1, 3.0, 0, 1], [4, 5, 0, 1], [2, 6, 0, 1], [2, 2, 0, 1], [-1, 4, 0, 1]]
coord = get_coord(X_range=[-6,40], Y_range=[-5,10], coord_size=1000)
draw_space_new(img_root, coord, mtx, rvecs, T, dist)
if predict_space_onepoint:
p3 = [[1, 5.1, 0.77, 0],
[2, 6.14, -0.26, 0],
[3, 4.86, -0.9, 0],
[4, 7.42, -1.28, 0],
[5, 8.7, 0.51, 0],
[6, 11.14, -1.41, 0],
[7, 12.29, -0.26, 0],
[8, 14.72, 1.79, 0],
[9, 15.74, -7.4, 0],
[10, 16.64, 1.79, 0],
[12, 28.67, 3.33, 0],
[13, 26.24, -2.05, 0],
[15, 11.78, -2.05, 0],
[16, 10.37, 3.2, 0],
[17, 5.76, 2.56, 0],
[18, 7.68, 0.51, 0]]
p2 = [[1, 590, 359.0],
[2, 1023, 309.0],
[3, 1189, 352.0],
[4, 1174, 274.0],
[5, 778, 255.0],
[6, 1143, 215],
[7, 957, 204],
[8, 670, 206],
[9, 979, 186],
[10, 710, 188],
[12, 698, 147.0],
[13, 1069, 141],
[15, 1242, 211],
[16, 1316, 259],
[17, 156, 352],
[18, 745, 283.0]]
p3 = np.array(p3, dtype=np.float32)[:, 1:]
p2 = np.array(p2, dtype=np.float32)[:, 1:]
index=[0,3,8,12]
p3_new,p2_new=p3[index],p2[index]
R, rvecs, tvecs = calibration_RT(p3_new, p2_new, mtx, dist)
# index, rvecs, tvecs = get_best_pnp_index(p3, p2, mtx, dist, N_pnp=6, loop_n=10000)
# 计算剩余点坐标
# img_root=r''
for i, p in enumerate(p3):
if i not in index:
coord = np.array([p[0], p[1], p[2]], dtype=np.float32)
pre_xy = get_calibration_pixel(coord, rvecs, tvecs, mtx, dist)
x, y = np.array(pre_xy).reshape(-1)
print("坐标索引:{}\t原始坐标--预测坐标:{}--{}".format(i, (p2[i][0], p2[i][1]), (x, y)))
if resize_img:
img_file = r'C:\Users\vrc\Desktop\RadarCamera\data\data_0928\img_ori'
out_dir = build_dir(os.path.join(img_file, 'out_dir'))
for img_name in os.listdir(img_file):
if img_name[-3:] == 'jpg':
img_root = os.path.join(img_file, img_name)
img = revise_img(img_root, mtx, dist)
cv2.imwrite(os.path.join(out_dir, img_name), img)以上代码包含相机标定/原始图像外参求解
推荐标定误差分析博客:https://blog.csdn.net/weixin_48934707/article/details/124295140
评论（0）
您还未登录，请登录后发表或查看评论