多视图几何--立体校正--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(); // 基线方向// 构造正交基e2Vector3d e2 = e1.cross(R.col(2));e2.normalize();// 构造正交基e3Vector3d e3 = e1.cross(e2);e3.normalize();// 新旋转矩阵 [e1, e2, e3]^TR_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法极线校正 - 知乎)