别再只调OpenCV函数了！手撕一遍张正友标定C++代码，彻底搞懂内参、外参和畸变是咋算出来的
从零实现张正友标定：深入理解相机内参、外参与畸变校正的数学本质
当你第一次调用OpenCV的calibrateCamera函数并得到满意的重投影误差时，是否曾好奇过这个"黑盒子"内部究竟发生了什么？在工业检测、三维重建等高精度场景中，仅满足于API调用是远远不够的。本文将带你用C++从零实现经典张正友标定法，揭开相机参数估计的神秘面纱。
1. 标定原理与数学基础
相机标定的核心在于建立三维世界坐标与二维图像坐标之间的映射关系。张正友标定法通过平面棋盘格这种特殊结构，将问题简化为求解单应性矩阵的优化过程。
1.1 投影模型与坐标系转换
相机成像遵循小孔模型，其数学表示为：
s\begin{bmatrix}u\\v\\1\end{bmatrix} = K\begin{bmatrix}R & t\end{bmatrix} \begin{bmatrix}X_w\\Y_w\\Z_w\\1\end{bmatrix}其中：
(u,v)为像素坐标K为内参矩阵，包含焦距和主点信息[R|t]为外参矩阵，描述世界坐标系到相机坐标系的转换s为比例因子
当使用平面标定板（Z_w=0）时，投影方程简化为：
// 世界坐标到像素坐标的简化投影 Point2f project(const Mat &K, const Mat &R, const Mat &t, Point3f world) { Mat cam = R * (Mat_<double>(3,1) << world.x, world.y, 0) + t; Mat pixel = K * cam; return Point2f(pixel.at<double>(0)/pixel.at<double>(2), pixel.at<double>(1)/pixel.at<double>(2)); }1.2 单应性矩阵的几何意义
单应性矩阵H建立了标定板平面与图像平面之间的线性映射关系。对于平面标定场景，H可以分解为：
H = K\begin{bmatrix}r_1 & r_2 & t\end{bmatrix}其中r₁、r₂为旋转矩阵的前两列。这一关系将成为我们求解内参的关键桥梁。
2. 代码实现：从图像点到单应性矩阵
2.1 角点检测与数据预处理
使用OpenCV的棋盘格角点检测获取对应点对：
vector<Point2f> detectCorners(Mat image, Size patternSize) { vector<Point2f> corners; bool found = findChessboardCorners(image, patternSize, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE); if(found) { TermCriteria criteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.001); cornerSubPix(image, corners, Size(5,5), Size(-1,-1), criteria); } return corners; }2.2 归一化与稳健估计
数值计算稳定性对结果至关重要。我们采用数据归一化技术：
| 归一化步骤 | 数学操作 | 代码实现 |
|---|---|---|
| 平移中心 | 减去均值 | point.x -= mean_x |
| 尺度归一化 | 除以标准差 | point.x /= std_x |
void normalizePoints(vector<Point2f>& points, Mat& T) { Scalar mean, stddev; meanStdDev(points, mean, stddev); T = Mat::eye(3,3,CV_64F); T.at<double>(0,0) = 1.0/stddev[0]; T.at<double>(1,1) = 1.0/stddev[1]; T.at<double>(0,2) = -mean[0]/stddev[0]; T.at<double>(1,2) = -mean[1]/stddev[1]; for(auto& p : points) { p.x = (p.x - mean[0]) / stddev[0]; p.y = (p.y - mean[1]) / stddev[1]; } }2.3 单应性矩阵求解
通过SVD分解求解超定方程组：
Mat computeHomography(vector<Point2f>& objPoints, vector<Point2f>& imgPoints) { Mat A(2*objPoints.size(), 9, CV_64F); for(size_t i=0; i<objPoints.size(); ++i) { double x = objPoints[i].x, y = objPoints[i].y; double u = imgPoints[i].x, v = imgPoints[i].y; A.at<double>(2*i, 0) = -x; A.at<double>(2*i, 1) = -y; A.at<double>(2*i, 2) = -1; A.at<double>(2*i, 6) = x*u; A.at<double>(2*i, 7) = y*u; A.at<double>(2*i, 8) = u; A.at<double>(2*i+1, 3) = -x; A.at<double>(2*i+1, 4) = -y; A.at<double>(2*i+1, 5) = -1; A.at<double>(2*i+1, 6) = x*v; A.at<double>(2*i+1, 7) = y*v; A.at<double>(2*i+1, 8) = v; } Mat U, S, Vt; SVDecomp(A, S, U, Vt, SVD::MODIFY_A); Mat H = Vt.row(8).reshape(0, 3); return H; }3. 内参求解：从单应性到相机矩阵
3.1 闭式解推导
利用旋转矩阵的正交性约束，建立关于内参的方程：
\begin{bmatrix} h_1^TBh_2 \\ h_1^TBh_1 - h_2^TBh_2 \end{bmatrix} = 0其中B=K^{-T}K^{-1}。通过多幅图像堆叠方程，用SVD求解B矩阵：
Mat computeIntrinsics(vector<Mat>& homographies) { int n = homographies.size(); Mat V(2*n, 6, CV_64F); for(int i=0; i<n; ++i) { Mat H = homographies[i]; double h11 = H.at<double>(0,0), h12 = H.at<double>(0,1); double h21 = H.at<double>(1,0), h22 = H.at<double>(1,1); double h31 = H.at<double>(2,0), h32 = H.at<double>(2,1); V.at<double>(2*i, 0) = h11*h12; V.at<double>(2*i, 1) = h11*h22 + h12*h21; V.at<double>(2*i, 2) = h12*h22; V.at<double>(2*i, 3) = h31*h12 + h11*h32; V.at<double>(2*i, 4) = h31*h22 + h21*h32; V.at<double>(2*i, 5) = h32*h22; V.at<double>(2*i+1, 0) = h11*h11 - h12*h12; V.at<double>(2*i+1, 1) = 2*(h11*h21 - h12*h22); V.at<double>(2*i+1, 2) = h21*h21 - h22*h22; V.at<double>(2*i+1, 3) = 2*(h11*h31 - h12*h32); V.at<double>(2*i+1, 4) = 2*(h21*h31 - h22*h32); V.at<double>(2*i+1, 5) = h31*h31 - h32*h32; } Mat U, S, Vt; SVDecomp(V, S, U, Vt, SVD::MODIFY_A); Mat b = Vt.row(5); double B11 = b.at<double>(0), B12 = b.at<double>(1); double B13 = b.at<double>(3), B22 = b.at<double>(2); double B23 = b.at<double>(4), B33 = b.at<double>(5); double v0 = (B12*B13 - B11*B23)/(B11*B22 - B12*B12); double lambda = B33 - (B13*B13 + v0*(B12*B13 - B11*B23))/B11; double alpha = sqrt(lambda/B11); double beta = sqrt(lambda*B11/(B11*B22 - B12*B12)); double gamma = -B12*alpha*alpha*beta/lambda; double u0 = gamma*v0/beta - B13*alpha*alpha/lambda; Mat K = (Mat_<double>(3,3) << alpha, gamma, u0, 0, beta, v0, 0, 0, 1); return K; }3.2 参数物理意义解析
内参矩阵各元素的物理含义：
| 参数 | 数学符号 | 物理意义 | 典型值范围 |
|---|---|---|---|
| 焦距 | α, β | 像素单位的焦距 | 500-2000 |
| 主点 | u₀, v₀ | 图像中心偏移 | 图像宽高/2 |
| 倾斜系数 | γ | 图像轴不垂直度 | 接近0 |
4. 外参估计与畸变校正
4.1 外参矩阵计算
从已知的单应性矩阵和内参矩阵反推外参：
void computeExtrinsics(Mat &H, Mat &K, Mat &R, Mat &t) { Mat K_inv = K.inv(); Mat h1 = H.col(0), h2 = H.col(1), h3 = H.col(2); Mat r1 = K_inv * h1; Mat r2 = K_inv * h2; Mat r3 = r1.cross(r2); t = K_inv * h3 / norm(r1); Mat R_raw(3,3,CV_64F); r1.copyTo(R_raw.col(0)); r2.copyTo(R_raw.col(1)); r3.copyTo(R_raw.col(2)); // 正交化处理 Mat U, S, Vt; SVDecomp(R_raw, S, U, Vt); R = U * Vt; // 确保行列式为1 if(determinant(R) < 0) { R.col(2) *= -1; } }4.2 畸变模型与参数估计
张正友标定法主要考虑径向畸变：
\begin{cases} x_{distorted} = x(1 + k_1r^2 + k_2r^4) \\ y_{distorted} = y(1 + k_1r^2 + k_2r^4) \end{cases}通过最小二乘法求解畸变系数：
Mat estimateDistortion(vector<Point2f>& imgPoints, vector<Point3f>& objPoints, Mat &K, Mat &R, Mat &t) { int n = imgPoints.size(); Mat D(2*n, 2, CV_64F); Mat d(2*n, 1, CV_64F); for(int i=0; i<n; ++i) { Mat pt = (Mat_<double>(3,1) << objPoints[i].x, objPoints[i].y, 0); Mat cam = R * pt + t; double x = cam.at<double>(0)/cam.at<double>(2); double y = cam.at<double>(1)/cam.at<double>(2); double r2 = x*x + y*y; Mat ideal = K * cam; double u = ideal.at<double>(0)/ideal.at<double>(2); double v = ideal.at<double>(1)/ideal.at<double>(2); D.at<double>(2*i, 0) = (u - K.at<double>(0,2)) * r2; D.at<double>(2*i, 1) = (u - K.at<double>(0,2)) * r2 * r2; D.at<double>(2*i+1, 0) = (v - K.at<double>(1,2)) * r2; D.at<double>(2*i+1, 1) = (v - K.at<double>(1,2)) * r2 * r2; d.at<double>(2*i) = imgPoints[i].x - u; d.at<double>(2*i+1) = imgPoints[i].y - v; } Mat k = (D.t() * D).inv() * D.t() * d; return k; }5. 结果验证与精度提升
5.1 重投影误差分析
评估标定质量的关键指标：
double computeReprojectionError(vector<vector<Point3f>>& objectPoints, vector<vector<Point2f>>& imagePoints, Mat &K, vector<Mat>& Rs, vector<Mat>& ts, Mat &distCoeffs) { double totalError = 0; int totalPoints = 0; for(size_t i=0; i<objectPoints.size(); ++i) { vector<Point2f> projected; projectPoints(objectPoints[i], Rs[i], ts[i], K, distCoeffs, projected); double error = norm(Mat(imagePoints[i]), Mat(projected), NORM_L2); totalError += error * error; totalPoints += objectPoints[i].size(); } return sqrt(totalError/totalPoints); }5.2 非线性优化技巧
初始解通常需要进一步优化：
- Levenberg-Marquardt算法优化所有参数
- 考虑添加更多畸变参数（切向畸变）
- 多视角数据联合优化
void refineParameters(vector<vector<Point3f>>& objectPoints, vector<vector<Point2f>>& imagePoints, Mat &K, vector<Mat>& Rs, vector<Mat>& ts, Mat &distCoeffs) { // 将参数转换为优化变量 vector<double> params; params.push_back(K.at<double>(0,0)); // fx params.push_back(K.at<double>(1,1)); // fy params.push_back(K.at<double>(0,2)); // cx params.push_back(K.at<double>(1,2)); // cy params.push_back(distCoeffs.at<double>(0)); // k1 params.push_back(distCoeffs.at<double>(1)); // k2 for(auto& R : Rs) { Mat rvec; Rodrigues(R, rvec); params.push_back(rvec.at<double>(0)); params.push_back(rvec.at<double>(1)); params.push_back(rvec.at<double>(2)); } for(auto& t : ts) { params.push_back(t.at<double>(0)); params.push_back(t.at<double>(1)); params.push_back(t.at<double>(2)); } // 使用Ceres等优化库进行非线性优化 // ... }6. 工程实践中的关键细节
在实际项目中，以下几个细节往往决定标定的成败：
- 标定板质量：使用高精度棋盘格，确保平面度误差<0.1mm
- 图像采集策略：
- 覆盖图像不同区域
- 多种倾斜角度（建议15-70度）
- 避免镜头对焦变化
- 数值稳定性处理：
- 数据归一化
- 正交化后的旋转矩阵修正
- 异常值剔除
// 旋转矩阵正交化修正 Mat orthogonalizeRotation(Mat& R) { Mat U, S, Vt; SVDecomp(R, S, U, Vt); return U * Vt; }通过完整实现张正友标定算法，开发者不仅能深入理解相机模型的数学本质，更能针对特定应用场景（如远心镜头、鱼眼相机等）进行定制化改进。这种底层实现能力，正是区分普通调用者和真正计算机视觉工程师的关键所在。