双目立体成像（二）畸变矫正与立体校正

AAAAAJoker

已于 2023-10-25 14:28:35 修改

阅读量3.4k

点赞数 33

文章标签：人工智能

于 2023-09-26 21:20:06 首次发布

本文链接：https://blog.csdn.net/qq_62964142/article/details/133091664

版权

在双目立体成像（一）中，我们利用双目图片生成了点云，但是我们自己拍摄的图片的效果并不好。考虑给图片去畸变和立体校正试试。

一、原理

1.畸变矫正

1.1 为什么要去畸变

相机的畸变是因为我们在相机的前面加了透镜来获得更好的成像效果。而透镜的形状或者安装方式会使得进入相机的光线发生变化，这样到达相机感光原件的像素位置就发生了偏移，反映到图像上就是畸变。相机的畸变会导致后期摄像头成像不准确进而影响测距结果。

1.2 去畸变

相机标定完成后使用畸变系数对图像进行畸变矫正，以达到消除畸变的目的。

1.3 CODE

OpenCV中可以直接调用函数来进行畸变矫正和立体校正，但slam十四讲中有根据上述原理自己写代码，结合代码可以更好地理解原理。

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "./distorted.png";   // 请确保路径正确

int main(int argc, char **argv) {

  // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
  // 畸变参数
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
  // 内参
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

  cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;
      double r = sqrt(x * x + y * y);
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值 (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } else {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }

  // 画图去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::waitKey();
  return 0;
}

书上的取名是有点误导的，我最开始学的时候很迷惑，但是结合代码可以看到最后传给去畸变后的图像image_undistort的数据是(u_distorted, v_distorted)，所以x_distorted, y_distorted,u_distorted, v_distorted都是去畸变后的。

2.立体校正

2.1 为什么要进行立体校正

立体成像时，要通过两幅图像估计物点的深度信息，就必须在两幅图像中准确的匹配到同一物点，这样才能根据该物点在两幅图像中的位置关系，计算物体深度。给定左目图像上一个点 $P_{L}$ ，在右目图像上遍历所有像素点来找到物点对应的成像点 $P_{R}$ 的方法是一个二维搜索问题，计算量庞大。所幸可以通过对极约束（同一个点在两幅图像上的映射，已知左图映射点 $P_{L}$ ，那么右图映射点 $P_{R}$ 一定在相对于 $P_{L}$ 的极线上）将原先的二维搜索问题简化到一维搜索问题，减少待匹配的点数量，加快计算。

$P$ 为三维空间的真实物点
$O_{L}$ 、 $O_{R}$ 分别为左右相机的光心，光心的连线 $O_{L}O_{R}$ 称为基线
$P_{L}$ 、 $P_{R}$ 分别为物点 $P$ 在左右相机成像平面上形成的像点
$e_{L}$ 、 $e_{R}$ 分别为基线与成像平面的交点，称为极点（若图像中不存在极点，则说明两个摄像机不能拍到彼此）
像点与极点的连线称为极线，所有极线相交于极点

但大部分情况下极线是倾斜的，这导致图像匹配时往往效率不高。若能使两个摄像头的成像平面处于同一平面（即使极线与基线平行，此时基线与成像平面没有交点，极点处于无穷远），将大大降低搜索难度，提升搜索速度。但是，很难通过严格的摆放摄像头来达到这个目的，立体校正就是利用几何图形变换(Geometric Image Transformation)关系，使得原先不满足上述位置关系的两幅图像满足该条件。

2.2 立体校正原理

参考双目视觉之立体校正的过程是什么？ - 知乎

虚构两个完全相同的相机，他们的成像平面共面且与基线平行。

要将原先的图片变换到新的虚拟相机的平面(校正后平面)上，重点是求左右两边相机对应的单应变换。

即得到如下四个式子：

联立可得

进一步可得

$\begin{bmatrix}\widehat{u}_{L}\\ \widehat{v}_{L}\\ 1 \end{bmatrix}=\frac{\lambda _{L}}{\widehat{\lambda }_{L}}\widehat{K}\widehat{R}^{-1}R_{L}{K_{L}}^{-1}\begin{bmatrix}{u}_{L}\\ {v}_{L}\\ 1 \end{bmatrix}=H_{L}\begin{bmatrix}{u}_{L}\\ {v}_{L}\\ 1\end{bmatrix}$

$\begin{bmatrix}\widehat{u}_{R}\\ \widehat{v}_{R}\\ 1 \end{bmatrix}=\frac{\lambda _{R}}{\widehat{\lambda }_{R}}\widehat{K}\widehat{R}^{-1}R_{R}{K_{R}}^{-1}\begin{bmatrix}{u}_{R}\\ {v}_{R}\\ 1 \end{bmatrix}=H_{R}\begin{bmatrix}{u}_{R}\\ {v}_{R}\\ 1\end{bmatrix}$

其中 $H_{L}$ 和 $H_{R}$ 即为我们要求的将原图像变换到新的虚拟相机上的单应变换，从上式可以看出，求这两个单应矩阵只需求解虚拟相机的内参 $\widehat{K}$ 以及旋转矩阵 $\widehat{R}$ 。

对于内参矩阵，直接假设 $\widehat{K}=\frac{K_{L}+K_{R}}{2}$ 得到虚拟相机的内参。

对于旋转矩阵 $\widehat{R}=\left [ \widehat{r_{1}} ,\widehat{r_{2}},\widehat{r_{3}} \right ]$

其中 $\widehat{r_{1}}=\frac{C_{R}-C_{L}} {\begin{Vmatrix} C_{R}-C_{L} \end{Vmatrix}}$ ，即 $\widehat{x}$ 轴方向为左右两个相机光心的坐标相减，再归一化，可以保证基线baseline与虚拟相机的相机平面平行。
$\widehat{r_{2}}=r_{3}\times\widehat{r_{1}}$ ， $\widehat{y}$ 轴方向垂直于新的轴与原相机视线方向轴所构成的平面。
$\widehat{r_{3}}=\widehat{r_{1}}\times \widehat{r_{2}}$ ，保证与新的 $\widehat{x}$ 轴与 $\widehat{y}$ 轴构成的平面垂直即可。

二、代码

OpenCV中可以直接调用函数对图像进行畸变矫正与立体校正。

cv::undistort

该函数可以直接对图像进行畸变矫正。

void cv::undistort 
( InputArray  src,                        // 原始图像
        OutputArray  dst,                        // 矫正图像
        InputArray  cameraMatrix,               // 原相机内参矩阵
        InputArray  distCoeffs,                 // 相机畸变参数
        InputArray  newCameraMatrix = noArray() // 新相机内参矩阵
)

cv::`stereoRectify`

该函数为每个摄像头计算立体校正的映射矩阵，其运行结果是进行立体校正所需要的映射矩阵。

void cv::stereoRectify 	(   InputArray  cameraMatrix1,
                            InputArray  distCoeffs1,
                            InputArray  cameraMatrix2,
                            InputArray  distCoeffs2,
                            Size        imageSize,
                            InputArray  R,
                            InputArray  T,
                            OutputArray R1,
                            OutputArray R2,
                            OutputArray P1,
                            OutputArray P2,
                            OutputArray Q,
                            int         flags = CALIB_ZERO_DISPARITY,
                            double      alpha = -1,
                            Size        newImageSize = Size(),
                            Rect *      validPixROI1 = 0,
                            Rect *      validPixROI2 = 0 
	)

参数说明：
输入参数：

cameraMatrix1：左目相机内参矩阵
distCoeffs1：左目相机畸变参数
cameraMatrix2：右目相机内参矩阵
distCoeffs2：右目相机畸变参数
imageSize：图像大小
R：左目相机坐标系到右目相机坐标系的旋转变换
T：左目相机坐标系到右目相机坐标系的平移变换
flags：如果设置为 CALIB_ZERO_DISPARITY，函数会将两个相机的 principal point 设成一样。否则就会平移图像最大化有用的图像区域。
alpha：自由缩放参数。如果设置为 -1 或者不设置，函数执行默认缩放。否则参数应为 0-1 。0：矫正图像会放大和平移使得最终图像中只有有效像素；1：图像会缩小和平移使得原始图像中所有像素都可见。
newImageSize：矫正后的图像分辨率。默认（0，0），设置为原始图像大小。设置为高的分辨率可以保持原始图像的更多细节，特别是畸变较大的时候。
validPixROI1：一个最多地包含有效像素的长方形。（左目图像）
validPixROI2：一个最多地包含有效像素的长方形。（右目图像）

输出参数：

R1：矫正旋转矩阵。将第一个相机坐标系下未矫正的点变换到第一个相机矫正坐标系下
R2：矫正旋转矩阵。将第二个相机坐标系下未矫正的点变换到第二个相机矫正坐标系下
P1：3x4左相机投影矩阵。将左矫正坐标系下的点投影到左矫正坐标系图像平面坐标系。
P2：3x4右相机投影矩阵。将右矫正坐标系下的点投影到右矫正坐标系图像平面坐标系。
Q：4x4的视差深度映射矩阵。

对于水平双目相机（大部分的双目相机），其中 $P_{1}$ , $P_{2}$ , $Q$ 定义如下：

$P_{1}=\begin{pmatrix} f & 0& cx_{1}& T_{x}\cdot f\\ 0& f& cy& 0\\ 0& 0& 1& 0 \end{pmatrix}$

$P_{2}=\begin{pmatrix} f & 0& cx_{2}& T_{x}\cdot f\\ 0& f& cy& 0\\ 0& 0& 1& 0 \end{pmatrix}$

$Q=\begin{pmatrix} 1 & 0& 0& -cx_{1}\\ 0& 1& 0& -cy\\ 0 & 0& 0& f\\ 0 & 0& -\frac{1}{T_{x}}& \frac{cx_{1}-cx_{2}}{T_{x}} \end{pmatrix}$

cv::initUndistortRectifyMap

该函数计算原始图像和矫正图像之间的转换关系，将结果以映射的形式表达，映射关系存储在map1和map2中。

void cv::initUndistortRectifyMap	
(	InputArray 	cameraMatrix,     // 原相机内参矩阵
        InputArray 	distCoeffs,       // 原相机畸变参数
        InputArray 	R,                // 可选的修正变换矩阵 
        InputArray 	newCameraMatrix,  // 新相机内参矩阵
        Size 	        size,             // 去畸变后图像的尺寸
        int 	        m1type,           // 第一个输出的映射(map1)的类型，CV_32FC1 or CV_16SC2
        OutputArray 	map1,             // 第一个输出映射
        OutputArray 	map2              // 第二个输出映射
)

cv::remap()

该函数把原始图像中某位置的像素映射到矫正后的图像指定位置。这里的map1和map2就是上面cv::initUndistortRectifyMap()计算出来的结果。

void cv::remap
(       InputArray    src,                        // 原始图像
        OutputArray   dst,                        // 矫正图像           
        InputArray    map1,                       // 第一个映射          
        InputArray    map2,                       // 第二个映射      
        int           interpolation,              // 插值方式
        int           borderMode=BORDER_CONSTANT, // 边界模式           
        const Scalar& borderValue=Scalar()        // 边界颜色，默认Scalar()黑色   
)

具体应用如下：

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;


int main(int argc,char **argv) 
{
    if(argc!=3)
    {
        cout<<"ERROR:Please input left & right images!!!"<<endl;
    }
    // 读取左右两张图像
    cv::Mat left_image = cv::imread(argv[1],0);
    cv::Mat right_image = cv::imread(argv[2],0);

    ///相机参数部分
    // 创建左右相机的相机矩阵
    cv::Mat camera_matrix1 = (cv::Mat_<double>(3, 3) <<
        392.7540541781677, 0.0, 320.5167125964447,
        0.0, 395.9067564822596, 214.0481721219656,
        0.0, 0.0, 1.0);

    cv::Mat camera_matrix2 = (cv::Mat_<double>(3, 3) <<
        397.4929637584831, 0.0, 329.9205610646954,
        0.0, 401.2285495190174, 222.0059268953744,
        0.0, 0.0, 1.0);

    // 创建左右相机的畸变参数
    cv::Mat dist_coeffs1 = (cv::Mat_<double>(1, 4) <<
        0.13174253697491506, -0.08050774683828582, -0.004286096646724357, -0.005622260771377513);

    cv::Mat dist_coeffs2 = (cv::Mat_<double>(1, 4) <<
        0.17694175583550273, -0.16912625169087664, -0.008203563607649733, -0.009381171330041677);

    // 创建左右相机的旋转矩阵和平移矩阵
    cv::Mat R, T;
    R = (cv::Mat_<double>(3, 3) <<
        0.9999825939856568, 0.0008245221779233294, 0.0058422503279421025,
        -0.0008587443489828619, 0.9999824752786064, 0.005857618442135849,
        -0.005837318207817441, -0.005862533483799694, 0.99996577762306);

    T = (cv::Mat_<double>(3, 1) << -0.05877186259962565, -0.00017956560621059625, 0.010933999617181184);
    ///
  


    ///去畸变
    cv::Mat undistorted_left,undistorted_right;
    cv::undistort(left_image,undistorted_left,camera_matrix1,dist_coeffs1);
    cv::undistort(right_image,undistorted_right,camera_matrix2,dist_coeffs2);

    //保存去畸变后的图像
    cv::imwrite("./undistorted_left.jpg",undistorted_left);
    cv::imwrite("./undistorted_right.jpg",undistorted_right);
    ///
    


    ///立体校正
    //根据标定结果创建左右摄像头的投影矩阵
    cv::Mat R1,P1,R2,P2,Q;
    // 进行立体校正
    cv::stereoRectify(camera_matrix1, dist_coeffs1, camera_matrix2, dist_coeffs2, left_image.size(), R, T, R1, R2, P1, P2,Q);

    // 创建校正映射
    cv::Mat map1x, map1y, map2x, map2y;
    cv::initUndistortRectifyMap(camera_matrix1, dist_coeffs1, R1, P1, left_image.size(), CV_32FC1, map1x, map1y);
    cv::initUndistortRectifyMap(camera_matrix2, dist_coeffs2, R2, P2, left_image.size(), CV_32FC1, map2x, map2y);

    // 应用校正映射
    cv::Mat rectified_left, rectified_right;
    cv::remap(left_image, rectified_left, map1x, map1y, cv::INTER_LINEAR);
    cv::remap(right_image, rectified_right, map2x, map2y, cv::INTER_LINEAR);

    // 保存校正后的图像
    cv::imwrite("./rectified_left.jpg", rectified_left);
    cv::imwrite("./rectified_right.jpg", rectified_right);
    ///

    return 0;
}