多视图几何--立体校正--Fusiello方法

1. 坐标系对齐与正交基构造

目标：构建新坐标系基向量 { e 1 , e 2 , e 3 } \{ \mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3 \} {e1​,e2​,e3​}，使成像平面共面且极线水平对齐。

(1) 基线方向 ${\mathbf{e}}_1$

• 基线向量由左右相机光心平移向量定义：$\mathbf{b}=C_2-C_1=\mathbf{T}$。

• 归一化基线方向：
  $${\mathbf{e}}_1=\frac{\mathbf{b}}{\Vert \mathbf{b}\Vert }$$
  （确保${\mathbf{e}}_1$与基线平行，为后续极线对齐奠定基础）。

(2) 正交基 ${\mathbf{e}}_2$ 构造

$$e_2=e_1\times r(3)$$

$r(3)$为左相机的旋转矩阵的第三个列向量。

归一化：
$${\mathbf{e}}_2=\frac{{\mathbf{e}}_2}{\Vert {\mathbf{e}}_2\Vert }$$
（避免直接使用原坐标系y轴，确保与${\mathbf{e}}_1$正交）。

(3) 正交基 ${\mathbf{e}}_3$ 构造

• 通过叉乘生成右手坐标系：
  $${\mathbf{e}}_3={\mathbf{e}}_1\times {\mathbf{e}}_2$$
  归一化：
  $${\mathbf{e}}_3=\frac{{\mathbf{e}}_3}{\Vert {\mathbf{e}}_3\Vert }$$
  （确保${\mathbf{e}}_3$垂直于成像平面，构成完整的正交基）。

(4) 新旋转矩阵 $R_{\text{new}}$

将基向量按列排列，构成新坐标系的外参旋转矩阵：
$$R_{\text{new}}={\left[\begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{e}}_3\end{array}\right]}^{\top }=\left[\begin{array}{ccc}{e}_{1x}& e_{1y}& e_{1z}\\ e_{2x}& e_{2y}& e_{2z}\\ e_{3x}& e_{3y}& e_{3z}\end{array}\right]$$
（新旋转矩阵使成像平面共面且极线水平）。

2. 投影矩阵更新

目标：构造左右相机的新投影矩阵 $P_1^{\prime }$ 和 $P_2^{\prime }$，确保共面成像。

(1) 左相机投影矩阵 $P_1^{\prime }$

• 新外参：位置仍为原点，姿态由 $R_{\text{new}}$ 定义。

• 投影矩阵：
  $$P_1^{\prime }=K_{\text{new}}\cdot \left[\begin{array}{cc}{R}_{\text{new}}& 0\end{array}\right]$$
  其中 $K_{\text{new}}$ 为调整后的内参矩阵（通常取左右相机内参的平均值，倾斜因子设为0）。

(2) 右相机投影矩阵 $P_2^{\prime }$

• 基线长度：$B=\Vert \mathbf{b}\Vert =\Vert \mathbf{T}\Vert$。

• 平移向量：沿新坐标系x轴平移：${\mathbf{T}}_{\text{new}}=[B,0,0{]}^{\top }$。

• 投影矩阵：
  $$P_2^{\prime }=K_{\text{new}}\cdot \left[\begin{array}{cc}{R}_{\text{new}}& {\mathbf{T}}_{\text{new}}\end{array}\right]$$
  （右相机仅沿新x轴平移，保证极线水平对齐）。

3. 单应性矩阵（Homography）推导

目标：将原始图像像素映射到校正后平面。

(1) 原相机投影模型

原始左相机的投影方程为：
$${\mathbf{x}}_1=K_1\cdot \left[\begin{array}{cc}R& 0\end{array}\right]\cdot \mathbf{X}$$
（其中 $R$ 为原相机的旋转矩阵）。

(2) 新相机投影模型

校正后的左相机投影为：
$${\mathbf{x}}_1^{\prime }=K_{\text{new}}\cdot \left[\begin{array}{cc}{R}_{\text{new}}& 0\end{array}\right]\cdot \mathbf{X}$$

(3) 单应性变换关系

联立两式消去 $\mathbf{X}$，得到单应矩阵 $H_1$：
$${\mathbf{x}}_1^{\prime }=K_{\text{new}}{R}_{\text{new}}{R}^{-1}{K}_1^{-1}\cdot {\mathbf{x}}_1$$
即：
$$H_1=K_{\text{new}}{R}_{\text{new}}{R}^{-1}{K}_1^{-1}$$
（通过变换矩阵 $H_1$ 将原图像像素映射到校正后平面）。

(4) 右相机单应矩阵

同理，右相机的单应矩阵为：
$$H_2=K_{\text{new}}{R}_{\text{new}}{R}^{-1}{K}_2^{-1}$$
（校正后右相机的极线与左相机严格水平对齐）。

4. 极线约束验证

校正后极线满足水平对齐，对应点视差仅沿x轴：
$${\mathbf{x}}_2^{\prime }={\mathbf{x}}_1^{\prime }+\left[\begin{array}{c}d\\ 0\\ 0\end{array}\right]$$
其中视差 $d$ 与深度 $Z$ 的关系为：
$$Z=\frac{B\cdot f}{d}$$
（$f$ 为焦距，极线水平化大幅简化立体匹配搜索）。

code:

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

using namespace cv;
using namespace Eigen;
using namespace std;

// Eigen矩阵与OpenCV Mat互转
Mat eigen2mat(const MatrixXd& m) {
    Mat mat(m.rows(), m.cols(), CV_64F);
    for (int i = 0; i < m.rows(); ++i)
        for (int j = 0; j < m.cols(); ++j)
            mat.at<double>(i, j) = m(i, j);
    return mat;
}

MatrixXd mat2eigen(const Mat& m) {
    MatrixXd mat(m.rows, m.cols);
    for (int i = 0; i < m.rows; ++i)
        for (int j = 0; j < m.cols; ++j)
            mat(i, j) = m.at<double>(i, j);
    return mat;
}

// Fusiello立体校正核心函数（修正后）
void fusielloRectify(
    const Matrix3d& K1, const Matrix3d& K2,
    const Matrix3d& R, const Vector3d& T,
    Matrix3d& R_new, Matrix3d& H1, Matrix3d& H2) {

    // ---- 1. 坐标系对齐 ----
    Vector3d e1 = T.normalized(); // 基线方向
    
    
    // 构造正交基e2
    Vector3d e2 = e1.cross(R.col(2));
    e2.normalize();
    
    // 构造正交基e3
    Vector3d e3 = e1.cross(e2);
    e3.normalize();

    // 新旋转矩阵 [e1, e2, e3]^T
    R_new.col(0) = e1;
    R_new.col(1) = e2;
    R_new.col(2) = e3;

    // ---- 2. 计算单应矩阵 ----
    Matrix3d R_inv = R.inverse();
    Matrix3d K1_inv = K1.inverse();
    H1 = K1 * R_new * K1_inv * R_inv; // 左相机单应
    H2 = K2 * R_new * K1_inv * R_inv; // 右相机单应
}

// 生成重映射表（使用OpenCV）
void computeRemapMaps(
    const Matrix3d& H, const Size& size,
    Mat& mapx, Mat& mapy) {

    Mat H_inv = eigen2mat(H.inverse());
    mapx.create(size, CV_32FC1);
    mapy.create(size, CV_32FC1);

    for (int y = 0; y < size.height; ++y) {
        for (int x = 0; x < size.width; ++x) {
            Mat pt = (Mat_<double>(3,1) << x, y, 1);
            Mat pt_src = H_inv * pt;
            pt_src /= pt_src.at<double>(2); // 归一化齐次坐标
            
            mapx.at<float>(y, x) = pt_src.at<double>(0);
            mapy.at<float>(y, x) = pt_src.at<double>(1);
        }
    }
}

int main() {
    // ---- 标定参数（示例）----
    Matrix3d K1, K2, R;
    Vector3d T;
    K1 << 1000, 0, 320,
           0, 1000, 240,
           0, 0, 1;
    K2 = K1;
    R << 0.996, -0.087, 0.0,  // 假设左相机有轻微旋转
          0.087, 0.996, 0.0,
          0.0,   0.0,   1.0;
    T << 0.1, 0, 0; // 基线长度0.1米

    // ---- Fusiello校正 ----
    Matrix3d R_new, H1, H2;
    fusielloRectify(K1, K2, R, T, R_new, H1, H2);

    // ---- 生成重映射表 ----
    Size imgSize(640, 480);
    Mat mapx1, mapy1, mapx2, mapy2;
    computeRemapMaps(H1, imgSize, mapx1, mapy1);
    computeRemapMaps(H2, imgSize, mapx2, mapy2);

    // ---- 图像校正测试 ----
    Mat frame1 = imread("left.jpg");
    Mat frame2 = imread("right.jpg");
    Mat rect1, rect2;

    if (!frame1.empty() && !frame2.empty()) {
        remap(frame1, rect1, mapx1, mapy1, INTER_LINEAR);
        remap(frame2, rect2, mapx2, mapy2, INTER_LINEAR);
        
        // 绘制水平线验证极线对齐
        for (int y = 0; y < rect1.rows; y += 20) {
            line(rect1, Point(0, y), Point(rect1.cols, y), Scalar(0, 255, 0), 1);
            line(rect2, Point(0, y), Point(rect2.cols, y), Scalar(0, 255, 0), 1);
        }

        imshow("Left Rectified", rect1);
        imshow("Right Rectified", rect2);
        waitKey(0);
    } else {
        cerr << "Failed to load images!" << endl;
    }

    return 0;
}

参考：

《A Compact Algorithm for Rectification of Stereo Pairs》

[1](69. 三维重建4——立体校正(Recitification) - 知乎)

[2](立体视觉入门指南（6）：对级约束与Fusiello法极线校正 - 知乎)