3D视觉(二)：单目摄像头的标定与校正

文章目录

3D视觉(二)：单目摄像头的标定与校正
一、相机模型
1、机器车坐标系到相机坐标系
2、相机坐标系到归一化平面坐标系
3、归一化平面坐标畸变
4、归一化平面坐标系到像素坐标系
二、单目摄像头标定
三、实验结果
四、源码
五、相关链接

相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素），这个过程可用针孔相机模型和透镜畸变模型来刻画。这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数。

一、相机模型

假设存在一个机器小车，以小车后轮为3D坐标原点可建立机器车坐标系。机器小车上携带有一个单目摄像头，以摄像头光心为3D坐标原点可建立相机坐标系。现已知某个物体在机器车坐标系下的3D坐标，如何计算出它在成像图像上像素点的2D索引位置？

1、机器车坐标系到相机坐标系

第1步：利用外参矩阵进行3D坐标变换。记旋转矩阵为R、平移向量为t，设物体在机器小车坐标系下的3D坐标为Pw，在相机坐标系下的3D坐标为P = (X, Y, Z)，则：

P = RPw + t = TPw

这里Pw和P的坐标单位都是米。

2、相机坐标系到归一化平面坐标系

第2步：利用针孔相机模型将3D相机坐标P = (X, Y, Z) 转化成2D归一化平面坐标 (x, y, 1)。

利用相似三角形原理，可以得到：z/f = x/X = y/Y。
一般我们取z = 1，得到映射后的归一化坐标为(x, y, z) = (X/f, Y/f, 1)，这里x、y的单位都是米。

归一化坐标可以看成相机前方z=1处平面上的一个点，这个z=1的平面也称为归一化平面。归一化平面再左乘内参矩阵，就可以得到像素坐标，所以我们可以把像素坐标(u, v)看成对归一化平面上的点进行量化测量的结果。

从这个模型也可以看出，如果对相机坐标同时乘以任何非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

3、归一化平面坐标畸变

第3步：对归一化坐标做畸变处理。为获得更好的成像效果，有时我们会在相机的前方加入透镜。透镜的加入会对成像过程中光线的传播产生新的影响。一是透镜自身的形状对光线传播存在影响；二是机械组装过程中透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像平面时的位置发生变化。

由透镜形状引起的畸变称为径向畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线，但在实际拍摄过程中，往往会出现真实环境中的一条直线在图片中变成了曲线，越靠近图像的边缘，这种现象越明显。畸变主要分为两类：桶形畸变、枕形畸变。桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变图像放大率随着与光轴之间的距离增加而增大。

由相机组装过程中不能使透镜和成像平面严格平行，由此引起的畸变成为切向畸变。

记归一化平面坐标为(x, y)，极坐标形式为(r, theta)，畸变坐标为(x_distorted, y_distorted)，它们之间的转换关系可用多项式进行描述：

径向畸变：

切向畸变：

综合以上两种畸变，得到畸变坐标：

这里x_distorted, y_distorted的单位是米。

4、归一化平面坐标系到像素坐标系

第4步：将畸变后的坐标(x_distorted, y_distorted)投影到像素平面，得到该点在图像上的位置。

像素坐标与归一化平面坐标之间，相差了一个缩放和一个原点的平移。我们设像素坐标在u轴上缩放了fx倍，在v轴缩放了fy倍，同时原点平移了(cx. cy)，则畸变后的坐标(x_distorted, y_distorted) 和像素坐标(u, v)的关系为：

u = fx * x_distorted + cx
v = fy * y_distorted + cy

有时我们不考虑畸变模型，可直接对归一化坐标x、y进行平移缩放，得到像素坐标u、v。这里x_distorted, y_distorted、x、y的单位都是米，u、v的单位是像素，fx、fy的单位是像素/米，cx、cy的单位是像素。

二、单目摄像头标定

通常认为，相机的内参矩阵在出厂后是固定的，不会在使用过程中发生变换。有些相机生产厂商会告诉你相机的内参，而有时需要我们自己手动确定相机的内参，也就是所谓的标定。此外，如果觉得标定过程过于麻烦且对精度要求不高，可采用如下方法近似内参矩阵和畸变系数向量：

记图像尺寸为 (h, w) = (size[0], size[1])，对于内参矩阵K= [[fx, 0, cx], [0, fy, cy], [0, 0, 1]，可近似 fx = fy = size[1]，cx = size[1]/2，cy = size[0]/2。对于畸变系数向量D，可近似 D=zeros(1, 5)。

“张氏标定”是张正友教授于1998年提出的单平面棋盘格的摄像机标定方法，张氏标定法已经作为工具箱或封装好的函数被广泛应用，原文为“A Flexible New Technique for Camera Calibration”。此文中所提到的方法，为相机标定提供了很大便利，并且具有很高的精度。从此标定可以不需要特殊的标定物，只需要一张打印出来的棋盘格。

张氏标定就是利用一张打印的棋盘格，然后对每个角点进行标记其在像素坐标系的像素点坐标，以及在世界坐标系的坐标，通过4组以上的点就可以求解出H矩阵的值。但为减少误差，具有更强的鲁棒性，我们一般会拍摄许多张照片，选取大量的角点进行标定。

我们假设标定棋盘位于世界坐标中zw=0平面，则可得到简化公式：

定义H矩阵为：
则原方程可化为：

借助OpenCV棋盘格内点检测函数，我们可得到u、v的观测值。由于棋盘格是按照一定顺序规律排列的，所以可以将对应的索引赋值成它们的3D坐标点，虽然和真实世界坐标具有尺寸差异，但这只会影响外参矩阵的计算结果，而不影响内参矩阵的求解。这样我们得到了(u, v, 1 )和(xw. yw, 1)的对应观测值，通过线性方程组求解即可解出H矩阵。再通过旋转矩阵、内参矩阵的特殊性质，可从H矩阵中还原出内参矩阵K、旋转矩阵R和平移向量t。

具体标定过程如下：
step1: 准备一张棋盘格图片，固定在墙上。
step2: 从不同角度拍摄棋盘格一系列照片，存储在文件夹内。
step3: 对于每张拍摄的棋盘图片，检测图片中所有棋盘格的特征点(u, v, 1 )。
step4: 对于每张拍摄的棋盘图片，将对应的索引赋值成它们的3D坐标点(xw. yw, 1)。
step5: 利用cv::calibrateCamera函数进行标定，求解参数优化问题。
step6: 利用cv::undistort函数，对原图像进行校正。

三、实验结果

从不同角度拍摄棋盘格一系列照片，如图所示：
对每张图片进行棋盘格内点检测：

标定得到的参数结果为：

原图和校正后的图像如下图所示，可以看到畸变被很大程度上消除。

四、源码

单目相机标定：

#include <opencv2/opencv.hpp>
// opencv.hpp中己经包含了OpenCV各模块的头文件，如高层GUI图形用户界面模块头文件highgui.hpp、图像处理模块头文件imgproc.hpp、2D特征模块头文件features2d.hpp等。
// 所以我们在编写应用程序时，原则上仅写上一句 #include <opencv2/opencv.hpp> 即可，这样可以精简优化代码
#include <opencv2/calib3d/calib3d.hpp>
// calib3d模块主要是相机校准和三维重建相关的内容：基本的多视角几何算法，单个立体摄像头标定，物体姿态估计，立体相似性算法，3D信息的重建等。
#include <opencv2/highgui/highgui.hpp>
// highgui模块，高层GUI图形用户界面，包含媒体的I/O输入输出、视频捕捉、图像和视频的编码解码、图形交互界面的接口等内容
#include <opencv2/imgproc/imgproc.hpp>
// imgproc模块，图像处理模块，包含：线性和非线性的图像滤波、图像的几何变换、特征检测等#include <iostream>
#include<unistd.h>
// unistd.h是用于linux/unix系统的调用，相当于windows下的windows.h，包含了许多UNIX系统服务的函数原型，例如read函数、write函数、sleep函数。
#include <chrono>
// chrono是C++11新加入的方便时间日期操作的标准库，它既是相应的头文件名称，也是std命名空间下的一个子命名空间，所有时间日期相关定义均在std::chrono命名空间下。
// 通过这个新的标准库，可以非常方便进行时间日期相关操作。 using namespace std;// 定义棋盘格维度，{6,4}代表行内点数为6，列内点数为4
int CHECKERBOARD[2]{6,4}; int main()
{// objpoints中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point3f数据结构// n * 54 * 3 * 1std::vector<std::vector<cv::Point3f> > objpoints;// imgpoints中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point2f数据结构// n * 54 * 2 * 1std::vector<std::vector<cv::Point2f> > imgpoints;// objp: 54 * 3 * 1, 记录单张棋盘格，54个内点的3d位置索引// 指定棋盘格坐标点时，按照先从上到下，后从左到右的顺序记录。每一行棋盘格的记录方式：(y索引, x索引， 0）std::vector<cv::Point3f> objp;//  [0, 0, 0;//  1, 0, 0;//  2, 0, 0;//  3, 0, 0;//  ... ...//  2, 8, 0;//  3, 8, 0;//  4, 8, 0;//  5, 8, 0]for(int i{0}; i<CHECKERBOARD[1]; i++){for(int j{0}; j<CHECKERBOARD[0]; j++)objp.push_back(cv::Point3f(j,i,0));}// images_path，存储所有棋盘格图片的存储路径std::vector<cv::String> images_path;std::string path = "../images2/*.jpg";cv::glob(path, images_path);std::string saved_path;cv::Mat frame, gray;// corner_pts，记录检测到的棋盘格54个内点的2D像素坐标 std::vector<cv::Point2f> corner_pts;// success，用于判断是否成功检测到棋盘格bool success;// 开始计时chrono::steady_clock::time_point t1 = chrono::steady_clock::now();for(int i{0}; i<images_path.size(); i++){ chrono::steady_clock::time_point t11 = chrono::steady_clock::now();// 图像大小 640 x 480frame = cv::imread(images_path[i]);std::cout << images_path[i] << std::endl;cv::cvtColor(frame,gray, cv::COLOR_BGR2GRAY);// OpenCV函数寻找棋盘格success = cv::findChessboardCorners(gray,cv::Size(CHECKERBOARD[0],CHECKERBOARD[1]), corner_pts, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);if(success){cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 30, 0.001);// 进一步refine检测到的网格内点的坐标精度// 这里cornerSubPix函数直接在原有corner_pts基础上进行覆盖，不会多创建一个新的变量再赋值cv::cornerSubPix(gray, corner_pts, cv::Size(11,11), cv::Size(-1,-1), criteria);// 作图，棋盘格检测结果cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0],CHECKERBOARD[1]), corner_pts, success);objpoints.push_back(objp);imgpoints.push_back(corner_pts);}//     cv::imshow("Image", frame);
//     cv::waitKey(10);saved_path = "../images1_demo/" + std::to_string(i) + ".jpg";cv::imwrite(saved_path, frame);chrono::steady_clock::time_point t22 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t22 - t11);cout << "每一张图片处理耗时: " << time_used.count() << " 秒. " << endl;}cv::destroyAllWindows();chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used1 = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "整体耗时: " << time_used1.count() << " 秒. " << endl;// 内参矩阵、畸变系数、旋转矩阵R、平移向量Tcv::Mat cameraMatrix, distCoeffs, R, T;chrono::steady_clock::time_point t111 = chrono::steady_clock::now();// 这里注意参数顺序，必须先cols后rowscv::calibrateCamera(objpoints, imgpoints, cv::Size(gray.cols,gray.rows), cameraMatrix, distCoeffs, R, T);chrono::steady_clock::time_point t222 = chrono::steady_clock::now();chrono::duration<double> time_used_cali = chrono::duration_cast<chrono::duration<double>>(t222 - t111);cout << "矫正耗时: " << time_used_cali.count() << " 秒. " << endl;std::cout << "cameraMatrix : " << cameraMatrix << std::endl;std::cout << "distCoeffs : " << distCoeffs << std::endl;
//   std::cout << "Rotation vector : " << R << std::endl;
//   std::cout << "Translation vector : " << T << std::endl;return 0;
}// 对于相机内参矩阵：[[fx, 0, cx], [0, fy, cy], [0, 0, 1]
//  一般都可近似 fx = fy = size[1], cx = size[1]/2, cy = size[0]/2// images2文件夹，内参标定结果：
// cameraMatrix : [845.5595871866724, 0, 1324.600361657917;
// 0, 850.5931334946969, 729.9380327446599;
// 0, 0, 1]
// distCoeffs : [-0.1129616696736557, 0.01545728211105597, -0.001661835061769386, -0.0001092622724212072, -0.001159949110844942]

单目相机校正：

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>
#include <iostream>using namespace std;int main()
{// images_path，存储所有棋盘格图片的存储路径std::vector<cv::String> images_path;std::string path = "../images2/*.jpg";cv::glob(path, images_path);// 根据计算得到的内参、畸变系数，对畸变图片进行矫正cv::Mat image;image = cv::imread(images_path[0]);cv::Mat dst, map1, map2, new_camera_matrix;cv::Size imageSize(cv::Size(image.cols, image.rows));// 内参矩阵float K[3][3] = {845.5595871866724, 0, 1324.600361657917, 0, 850.5931334946969, 729.9380327446599, 0, 0, 1};    // float类型cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, K);    // 畸变系数 float d[1][5] = {-0.1129616696736557, 0.01545728211105597, -0.001661835061769386, -0.0001092622724212072, -0.001159949110844942};   // float类型 cv::Mat distCoeffs = cv::Mat(1, 5, CV_32FC1, d);   // 将内参矩阵和畸变系数进行融合，得到新的矫正参数矩阵// 最后一个参数需要注意：最后一个参数默认是false，也就是相机光心不在默认的图像中心位置，可能导致去除畸变后的图像边缘仍存在畸变，因此需要改成truenew_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 0.01, imageSize, 0, true);for(int i{0}; i<images_path.size(); i++){  image = cv::imread(images_path[i]); // 第1种方法：OpenCV undistort函数，转换图像以补偿径向和切向镜头失真cv::undistort(image, dst, new_camera_matrix, distCoeffs, new_camera_matrix);// 第2种方法：OpenCV remap函数，计算联合不失真和整流变换，并以重映射的映射形式表示结果//   cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs,   imageSize, 1, imageSize, 0),imageSize, CV_16SC2, map1, map2);// //   cv::remap(image, dst, map1, map2, cv::INTER_LINEAR);cv::Mat resize_dst;resize(dst, resize_dst, cv::Size(256*2, 144*2), 0, 0, cv::INTER_LINEAR);cv::imshow("undistorted image", resize_dst);cv::waitKey(0);  std::string saved_path = "../images2_undist/" + std::to_string(i) + ".jpg";cv::imwrite(saved_path, dst);}return 0;
}

五、相关链接

如果代码跑不通，或者想直接使用我自己制作的数据集，可以去下载项目链接：
https://blog.csdn.net/Twilight737