双目相机外参自标定方法
- 原理
- 实践
双目相机外参自标定方法是一种无需固定标定板，在拍摄实际场景的两张图像时，通过计算两幅图像之间的匹配特征点对，结合相机的内参矩阵，来实时求解两个相机之间相对位置（即外参）的方法。
原理
双目相机外参自标定基于双目立体视觉原理，通过匹配两幅图像中的对应点，利用这些匹配点对以及相机的内参矩阵，计算出两个相机之间的旋转矩阵和平移向量，即外参矩阵。这种方法避免了使用固定标定板的繁琐过程，提高了标定的灵活性和实时性。
具体步骤：
-
畸变校正：
首先，利用已知的相机内参矩阵对两幅原始图像进行畸变校正。这是因为在相机成像过程中，由于镜头制造和安装误差等原因，图像会产生畸变，影响后续的特征点匹配和参数计算。 -
特征点提取与匹配：
使用特征点检测算法（如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 E
def 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 = 0
for 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.x
r_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 values
left_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_map
depth_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++参考资料 1：
/* -*-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*p
Mat3X 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());
// 构建线性方程组的系数矩阵A
auto 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的最小特征值对应的特征向量即矢量 e
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9, 9>> solver(a_mat.transpose()*a_mat);
const Vec9 e = solver.eigenvectors().col(0);
// 矢量 e 构造本质矩阵 E
data_ = 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();
}
https://github.com/ethan-li-coding/StereoV3DCode ↩︎
![关于数字存储和byte[]数组的一些心得](https://i-blog.csdnimg.cn/direct/f14fb6d2546a424b9b1608bb2c6a159c.png)
