当前位置：首页 > news >正文

【立体匹配】双目相机外参自标定方法介绍

news 2025/5/9 9:09:01

双目相机外参自标定方法

原理
实践

双目相机外参自标定方法是一种无需固定标定板，在拍摄实际场景的两张图像时，通过计算两幅图像之间的匹配特征点对，结合相机的内参矩阵，来实时求解两个相机之间相对位置（即外参）的方法。

原理

双目相机外参自标定基于双目立体视觉原理，通过匹配两幅图像中的对应点，利用这些匹配点对以及相机的内参矩阵，计算出两个相机之间的旋转矩阵和平移向量，即外参矩阵。这种方法避免了使用固定标定板的繁琐过程，提高了标定的灵活性和实时性。

具体步骤：

畸变校正：
首先，利用已知的相机内参矩阵对两幅原始图像进行畸变校正。这是因为在相机成像过程中，由于镜头制造和安装误差等原因，图像会产生畸变，影响后续的特征点匹配和参数计算。
特征点提取与匹配：
使用特征点检测算法（如SIFT、SURF、ORB等）在两幅校正后的图像中分别提取特征点。
然后，利用特征点匹配算法（如FLANN、BFMatcher等）找出两幅图像中的匹配点对。这些匹配点对是两幅图像中同一物点在两个相机成像平面上的投影。
本质矩阵求解：
利用匹配点对和相机内参矩阵，通过数学方法求解本质矩阵（Essential Matrix）。本质矩阵描述了两个相机坐标系之间的旋转和平移关系，但不包含相机的内参信息。
在OpenCV中，可以使用cv::findEssentialMat函数来求解本质矩阵。该函数通常结合随机抽样一致性算法（RANSAC）来提高求解的鲁棒性，减少误匹配点对的影响。
外参矩阵分解：
通过对本质矩阵进行奇异值分解（SVD）等操作，可以分解出两个相机之间的旋转矩阵和平移向量，即外参矩阵。
在OpenCV中，可以使用cv::recoverPose函数从本质矩阵中恢复出相机的旋转矩阵和平移向量。该函数同样可以结合RANSAC算法来提高结果的准确性。

实践

外参自标定步骤：

第一步：使用左右相机的对应匹配点。

第二步：依据双目极线约束条件构造本征矩阵关于关键点位置的代价函数，将匹配关键点对带入代价函数，所求的代价函数之和称之为能量函数。

第三步：使用非线性优化方法优化本征矩阵的值，使得能量函数最小，此时估计的本征矩阵为最终结果，将得到的本征矩阵分解，即可得到双目相机的相对位姿。

左右相机的匹配点：
在这里插入图片描述

下图依次为原标定参数极线校正标定板视差图、恢复参数的标定板视差图：

标定板

下图依次为原标定参数极线校正人脸深度图、使用一段时间后校正深度图、恢复参数后校正深度图：

以下是著名的八点法求解本质矩阵：

# 读取左右图像  
imgL = cv2.imread('left.jpg',0) # 左图像  
imgR = cv2.imread('right.jpg',0) # 右图像  # 初始化SIFT检测器  
sift = cv2.xfeatures2d.SIFT_create()  # 检测并计算关键点和描述符  
kpL, desL = sift.detectAndCompute(imgL,None)  
kpR, desR = sift.detectAndCompute(imgR,None)  # 匹配描述符  
bf = cv2.BFMatcher()  
matches = bf.knnMatch(desL,desR,k=2)  # 选择好的匹配点  
good = []  
for m,n in matches:  if m.distance < 0.75*n.distance:  good.append(m)  # 如果匹配点数量不够，则退出程序  
if len(good)<4:  print("Not enough matches are found - {}/{}".format(len(good),4))  exit()  # 提取匹配点的坐标  
src_pts = np.float32([ kpL[m.queryIdx].pt for m in good ]).reshape(-1,1,2)  
dst_pts = np.float32([ kpR[m.trainIdx].pt for m in good ]).reshape(-1,1,2)  # 利用非线性优化求解本质矩阵和旋转矩阵  
E, mask = cv2.findEssentialMat(src_pts, dst_pts, None, method=cv2.RANSAC, prob=0.999, maxIters=1000)  
R, t, mask = cv2.recoverPose(E, src_pts, dst_pts)  
print("Rotation matrix: \n" + str(R))  
print("Translation vector: \n" + str(t))

但求解结果随缘，待优化。

以下是利用Scipy库进行非线性优化：

def skew_matrix(t):'''计算平移向量 T 的反对称矩阵'''return np.array([[0, -t[2], t[1]],[t[2], 0, -t[0]],[-t[1], t[0], 0]])def essential_matrix(r, t):R = cv2.Rodrigues(r)[0]skew_T = skew_matrix(t)E = np.dot(skew_T, R)return Edef error_function(params, left_pts, right_pts, mtx_left, mtx_right):r = params[:3]t = T#params[3:]#T#E = essential_matrix(r, t)error = []# constraint = 0for i in range(len(left_pts)):X1_normalized = np.dot(np.linalg.inv(mtx_left), np.array([left_pts[i][0], left_pts[i][1], 1]))X2_normalized = np.dot(np.linalg.inv(mtx_right), np.array([right_pts[i][0], right_pts[i][1], 1]))# 极线约束# constraint = abs(np.dot(np.dot(np.dot(np.dot(X2_normalized.T, np.linalg.inv(mtx_right).T), E), np.linalg.inv(mtx_left)), X1_normalized))constraint = np.dot(np.dot(X2_normalized.T, E), X1_normalized)print(constraint)# 添加到误差向量error.append(constraint)return np.array(error).flatten()

def nonlinear_optimization(left_pts, right_pts, mtx_left, mtx_right, initial_params):result = least_squares(error_function, initial_params, args=(left_pts, right_pts, mtx_left, mtx_right))# result = minimize(error_function, initial_params, args=(left_pts, right_pts, mtx_left, mtx_right))optimized_params = result.xr_optimized = optimized_params[:3]t_optimized = T#optimized_params[3:]print('optimized_params', optimized_params)R_optimized = cv2.Rodrigues(r_optimized)[0]E_optimized = essential_matrix(r_optimized, t_optimized)return R_optimized, E_optimized

	# 调用非线性优化函数R_optimized, E_optimized = nonlinear_optimization(left_pts, right_pts, mtx_left, mtx_right, initial_params)print("Optimized Rotation Matrix:")print(R_optimized)print('\nori R', R1)print("\nOptimized Essential Matrix:")print(E_optimized) print("\nori Essential Matrix:")# 矫正图像if 'dist' in src:dist_left = np.zeros((5, 1))dist_right = np.zeros((5, 1))R1, R2, P1, P2, Q, roi_left, roi_right = cv2.stereoRectify(mtx_left, dist_left, mtx_right, dist_right, image_size, R_optimized, T, flags=0, alpha=0.1) # cv2.CALIB_ZERO_DISPARITY# Undistortion and Rectification(计算畸变矫正和立体校正的映射变换)# map_x: The first map of y values; map_y: The second map of y valuesleft_map_re = cv2.initUndistortRectifyMap(mtx_left, dist_left, R1, P1, image_size, cv2.CV_32FC1)right_map_re = cv2.initUndistortRectifyMap(mtx_right, dist_right, R2, P2, image_size, cv2.CV_32FC1)leftrec, rightrec, leftrec_re, rightrec_re = show(left_map, right_map, left_map_re, right_map_re, src)# 构建StereoSGBM参数stereo_sgbm = cv2.StereoSGBM_create(minDisparity=0,numDisparities=16 * 2,  # 这应该是16的倍数blockSize=5,P1=8 * 3 * 5**2,P2=32 * 3 * 5**2,disp12MaxDiff=1,uniquenessRatio=15,speckleWindowSize=0,speckleRange=2,mode=cv2.StereoSGBM_MODE_SGBM_3WAY)# 计算深度图disparity_map = stereo_sgbm.compute(leftrec, rightrec)disparity_map_re = stereo_sgbm.compute(leftrec_re, rightrec_re)# 将视差图转换为深度图baseline = 0.1  # 相机基线（baseline）值，需要根据实际情况调整focal_length = mtx_left[0][0]  # 相机焦距，需要根据实际情况调整depth_map = (baseline * focal_length) / disparity_mapdepth_map_re = (baseline * focal_length) / disparity_map_re# 显示深度图cv2.imshow("Depth Map", depth_map)cv2.imshow("Depth Map recov", depth_map_re)cv2.waitKey(0)cv2.destroyAllWindows()

C++参考资料 ¹：

/* -*-c++-*- StereoV3D - Copyright (C) 2021.
* Author	: Ethan Li<ethan.li.whu@gmail.com>
* https://github.com/ethan-li-coding/StereoV3DCode
*/
#include "essential_solver.h"void sv3d::EssentialSolver::Solve(const Mat3X p1, const Mat3X p2, const Mat3 k1_mat, const Mat3 k2_mat, const SOLVE_TYPE& solver_type)
{assert(p1.cols() >= 8);assert(p1.rows() == p2.rows());assert(p1.cols() == p2.cols());// 通过内参矩阵k将p转换到x，x = k_inv*pMat3X x1(3,p1.cols()), x2(3,p2.cols());x1 = k1_mat.inverse() * p1;x2 = k2_mat.inverse() * p2;// 求解Solve(x1, x2, solver_type);
}void sv3d::EssentialSolver::Solve(const Mat3X x1, const Mat3X x2, const SOLVE_TYPE& solver_type)
{switch (solver_type) {case EIGHT_POINTS:Solve_EightPoints(x1, x2);default:break;}
}sv3d::Mat3 sv3d::EssentialSolver::Value()
{return data_;
}void sv3d::EssentialSolver::Solve_EightPoints(const Mat3X x1, const Mat3X x2)
{assert(x1.cols() >= 8);assert(x1.rows() == x2.rows());assert(x1.cols() == x2.cols());// 构建线性方程组的系数矩阵Aauto np = x1.cols();RMatX9 a_mat(np, 9);for (int n = 0; n < np; n++) {const auto x1_x = x1.data()[3 * n];const auto x1_y = x1.data()[3 * n + 1];const auto x2_x = x2.data()[3 * n];const auto x2_y = x2.data()[3 * n + 1];const auto dat = a_mat.data() + 9 * n;dat[0] = x1_x * x2_x; dat[1] = x2_x * x1_y; dat[2] = x2_x;dat[3] = x1_x * x2_y; dat[4] = x1_y * x2_y; dat[5] = x2_y;dat[6] = x1_x; dat[7] = x1_y; dat[8] = 1;}// 求解ATA的最小特征值对应的特征向量即矢量 eEigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9, 9>> solver(a_mat.transpose()*a_mat);const Vec9 e = solver.eigenvectors().col(0);// 矢量 e 构造本质矩阵 Edata_ = Eigen::Map<const RMat3>(e.data());// 调整 E 矩阵使满足内在性质：奇异值为[σ σ 0]Eigen::JacobiSVD<Mat3> usv(data_, Eigen::ComputeFullU | Eigen::ComputeFullV);auto s = usv.singularValues();const auto a = s[0];const auto b = s[1];s << (a + b) / 2.0, (a + b) / 2.0, 0.0;data_ = usv.matrixU() * s.asDiagonal() * usv.matrixV().transpose();
}