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

yddcs

已于 2024-08-31 18:58:31 修改

阅读量258

点赞数 4

分类专栏：双目相机文章标签：数码相机计算机视觉立体匹配相机标定特征匹配

于 2024-08-31 18:30:35 首次发布

本文链接：https://blog.csdn.net/qq_35200351/article/details/141687955

版权

双目相机专栏收录该内容

5 篇文章 3 订阅

订阅专栏

双目相机外参自标定方法

原理
实践

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

原理

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

具体步骤：

畸变校正：
首先，利用已知的相机内参矩阵对两幅原始图像进行畸变校正。这是因为在相机成像过程中，由于镜头制造和安装误差等原因，图像会产生畸变，影响后续的特征点匹配和参数计算。
特征点提取与匹配：
使用特征点检测算法（如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++参考资料 ¹：

/* -*-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 ↩︎

yddcs

关注

4
点赞
踩
3

收藏

觉得还不错? 一键收藏
打赏
0
评论
【立体匹配】双目相机外参自标定方法介绍

双目相机外参自标定方法是一种无需固定标定板，在拍摄实际场景的两张图像时，通过计算两幅图像之间的匹配特征点对，结合相机的内参矩阵，来实时求解两个相机之间相对位置（即外参）的方法。双目相机外参自标定基于双目立体视觉原理，通过匹配两幅图像中的对应点，利用这些匹配点对以及相机的内参矩阵，计算出两个相机之间的旋转矩阵和平移向量，即外参矩阵。这种方法避免了使用固定标定板的繁琐过程，提高了标定的灵活性和实时性。畸变校正：首先，利用已知的相机内参矩阵对两幅原始图像进行畸变校正。这是因为在相机成像过程中不可避免的安装误差。
复制链接

扫一扫