双目矫正及其应用

什么是双目矫正

双目矫正将两个相机的成像平面矫正到同一平面上，并伴随祛除畸变。

双目矫正之后两个相机的极线相互平行，极点在无穷远处。
OpenCV标定双目相机配置ORB-SLAM_!!opencv-matrix-CSDN博客

目的

这样的对齐是为了简化接下来的立体匹配过程，使得对应的特征点在同一水平线上，从而减少搜索匹配点时的复杂度。这也是双目slam做极线搜索的理论基础。

过程

1. 相机单独标定（内参K与畸变D）

首先，需要分别对左右相机进行单独的标定，以获得每个相机的内参矩阵和畸变系数。

这一步使用棋盘格或其他标定图案，通过拍摄多个不同视角的图片并分析这些图案来获取。

2. 立体相机标定（外参RT）

使用标定好的单个相机进行立体标定。

需要同时使用左右相机拍摄同一标定物体的图像，然后分析这些成对图像以确定两个相机之间的空间关系（即相对位置和姿态）。

这一步可以通过OpenCV中的 cv::stereoCalibrate 函数完成，它会输出两个相机之间的旋转矩阵R和平移向量T。

3. 立体校正（R_l/R_r与P_l/P_r）

在获取了相机之间的相对位置和姿态后，下一步是计算每个相机的校正旋转矩阵（R_l 和 R_r）和新的投影矩阵（P_l 和 P_r）。

这是为了使两个相机的成像平面在同一平面上，并且保证成像平面上的对应点在同一水平线上。

这一步使用OpenCV中的 cv::stereoRectify 函数完成。

注意：R 和t 表示 the first camera to the second camera，fist是左目时，R为左目到右目，和一般的标定结果相反。

cv::stereoRectify(
K_left, D_left, K_right, D_right, imageSize, R, T, //输入
R1, R2, P1, P2, //输出
Q, //输出，视差到深度映射矩阵，cv::Mat 4x4 矩阵，用于将图像的视差值转换成真实世界坐标系中的三维点。
cv::CALIB_ZERO_DISPARITY,//标志位，使得函数调整图像的主点位置，使左右相机的光学中心有相同的像素坐标，从而优化双目系统的对称性。这种设置意在减少视差搜索范围并简化视差计算。
1, //控制输出校正图像的可视区域大小的参数alpha。0 意味着只有当没有黑边时才能看到像素，alpha=1 意味着保留所有像素但图像中可能包含黑边。通过调整 alpha 值，可以在图像的覆盖区和黑边之间进行权衡
imageSize, //表示用于立体校正的图像的尺寸。这个尺寸应该与输入图像的实际尺寸相匹配，确保校正算法正确处理图像的所有区域。
&validRoi[0], &validRoi[1] // 这两个参数是输出参数，它们分别表示左右图像中有效的图像区域。这个区域内的像素在校正后保持有效，而图像的其他部分可能由于校正过程中的映射而不再有效（可能包括黑边）。这些矩形区域可以用来裁剪校正后的图像，以去除边缘的黑边。
);

双目slam中的应用

比如euroc数据集，直接给出双目矫正结果（R_l/R_r与P_l/P_r），而不是给出右到左目的RT，orbslam2对其直接应用，创建重映射图像。

1. 得到祛畸变重映射变换矩阵

// 左目
cv::initUndistortRectifyMap( //计算无畸变和修正转换映射
K_l, //输入：相机内参矩阵(标定得到)
D_l, //输入：去畸变参数(标定得到)
R_l, //输入（可选）：修正变换矩阵3*3, 可从cv::stereoRectify得来.如果这个矩阵为空矩阵,那么就将会被设置成为单位矩阵
P_l.rowRange(0,3).colRange(0,3), //输入：新相机内参矩阵
cv::Size(cols_l,rows_l), //输入：在去畸变之前的图像尺寸
CV_32F, //输入：输出映射的类型
M1l, M2l); //输出：映射矩阵1和2; M1l可以是（x，y）方向映射此时M2l为单位阵；也可以M1l与M2l分别代表x和y方向
// 右目
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

2. 双目祛畸变重映射重映射（把一幅图像中某位置的像素放置到另一个图片指定位置的过程）

// 左目
cv::remap(imLeft, //输入：图像
imLeftRect, //输出：图像
M1l, M2l, //映射矩阵，M1l可以是（x，y）方向映射此时M2l为单位阵；也可以M1l与M2l分别代表x和y方向
cv::INTER_LINEAR); // 插值方法： 双线性插值
// 右目
cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

已知旋转矩阵和投影矩阵，求RIGHT到LEFT的变换T

还是以euroc数据为例：

# 双目基线（米）乘以焦距（像素）
Camera.bf: 47.90639384423901

##########################LEFT###################
# 相机内参 K_l
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
# 在立体校正过程中，相机为实现共面过程中所需要进行的旋转 R_l
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
# 在立体校正过程后，相机在新坐标系下的投影矩阵 P_l
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]

##########################RIGHT###################
# K_r
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
# R_r
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
# P_r
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]

计算相对旋转矩阵：

R_l表示左目到理想平面的旋转，R表示右目到左目的旋转，那么R_l * R = R_r 即右目到理想平面的变换，推出R = R_l.T * R_r。

下面的代码是测试，将R_l与R_r转为R 再转回R_l与R_r的过程：

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

int main()
{
    // 数据来自euroc
    double bf = 47.90639384423901;
    cv::Mat D_left = (cv::Mat_<double>(5, 1) << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0);
    cv::Mat K_left = (cv::Mat_<double>(3, 3) << 458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0);
    cv::Mat R_left = (cv::Mat_<double>(3, 3) << 0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176);
    cv::Mat P_left = (cv::Mat_<double>(3, 4) << 435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0);

    cv::Mat D_right = (cv::Mat_<double>(5, 1) << -0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0);
    cv::Mat K_right = (cv::Mat_<double>(3, 3) << 457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1);
    cv::Mat R_right = (cv::Mat_<double>(3, 3) << 0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644);
    cv::Mat P_right = (cv::Mat_<double>(3, 4) << 435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0);

    cv::Mat R = R_left.t() * R_right; // 右目到左目
    double fx_left = K_left.at<double>(0, 0);
    cv::Mat T = (cv::Mat_<double>(3, 1) << (P_left.at<double>(0, 3) - P_right.at<double>(0, 3) / fx_left), 0, 0);
    std::cout << "R:\n"
              << R << "\n";
    std::cout << "T:\n"
              << T << "\n";

    cv::Size imageSize(752, 480);

    // 立体校正
    cv::Mat R1, R2, P1, P2, Q;
    cv::Rect validRoi[2];
    cv::stereoRectify(K_left, D_left, K_right, D_right, imageSize, R.t(), -T,
                      R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);

    std::cout << "R_left:\n" << R_left << "\n"; 
    std::cout << "R1:\n" << R1 << "\n";
    std::cout << "R_right:\n" << R_right << "\n";
    std::cout << "R2:\n" << R2 << "\n";

    std::cout << "P_left:\n" << P_left << "\n";
    std::cout << "P1:\n" << P1 << "\n";
    std::cout << "P_right:\n" << P_right << "\n";
    std::cout << "P2:\n" << P2 << "\n";
 
    // 转为旋转向量比较角度差
    cv::Mat rvec_left, rvec1;
    cv::Rodrigues(R_left, rvec_left);
    cv::Rodrigues(R1, rvec1);
    cv::Mat rvec_right, rvec2;
    cv::Rodrigues(R_right, rvec_right);
    cv::Rodrigues(R2, rvec2);
    double angle_diff1 = cv::norm(rvec_left - rvec1, cv::NORM_L2);
    double angle_diff2 = cv::norm(rvec_right - rvec2, cv::NORM_L2);
    std::cout << "Angle difference (radians): " << angle_diff1 << "\n";
    std::cout << "Angle difference (radians): " << angle_diff2 << "\n";

    return 0;
}

注意：stereoRectify 的第六和第七个参数表示the first camera to the second camera, 我们的First是Left，因此需要R转置，T加负号，代码中的R表示right到left。

ps：P_l 第一行第四列通常为0 ；P_r 第一行第四列通常为基线乘以左目x方向焦距