1. 坐标系对齐与正交基构造
目标 ：构建新坐标系基向量 { e 1 , e 2 , e 3 } \{ \mathbf{e}_1, \mathbf{e}_2, \mathbf{e}_3 \} {e1,e2,e3}，使成像平面共面且极线水平对齐。
(1) 基线方向 e 1 \mathbf{e}_1 e1
-
基线向量由左右相机光心平移向量定义： b = C 2 − C 1 = T \mathbf{b} = C_2 - C_1 = \mathbf{T} b=C2−C1=T。
-
归一化基线方向 ：
e 1 = b ∥ b ∥ \mathbf{e}_1 = \frac{\mathbf{b}}{\| \mathbf{b} \|} e1=∥b∥b（确保 e 1 \mathbf{e}_1 e1与基线平行，为后续极线对齐奠定基础）。
(2) 正交基 e 2 \mathbf{e}_2 e2 构造
e 2 = e 1 × r ( 3 ) e_2 = e_1\times r(3) e2=e1×r(3)
r ( 3 ) r(3) r(3)为左相机的旋转矩阵的第三个列向量。
归一化 ：
e 2 = e 2 ∥ e 2 ∥ \mathbf{e}_2 = \frac{\mathbf{e}_2}{\| \mathbf{e}_2 \|} e2=∥e2∥e2
（避免直接使用原坐标系y轴，确保与 e 1 \mathbf{e}_1 e1正交）。
(3) 正交基 e 3 \mathbf{e}_3 e3 构造
- 通过叉乘生成右手坐标系：
e 3 = e 1 × e 2 \mathbf{e}_3 = \mathbf{e}_1 \times \mathbf{e}_2 e3=e1×e2
归一化 ：
e 3 = e 3 ∥ e 3 ∥ \mathbf{e}_3 = \frac{\mathbf{e}_3}{\| \mathbf{e}_3 \|} e3=∥e3∥e3
（确保 e 3 \mathbf{e}_3 e3垂直于成像平面，构成完整的正交基）。
(4) 新旋转矩阵 R new R_{\text{new}} Rnew
将基向量按列排列，构成新坐标系的外参旋转矩阵：
R new = [ e 1 e 2 e 3 ] ⊤ = [ e 1 x e 1 y e 1 z e 2 x e 2 y e 2 z e 3 x e 3 y e 3 z ] R_{\text{new}} = \begin{bmatrix} \mathbf{e}1 & \mathbf{e}2 & \mathbf{e}3 \end{bmatrix}^\top = \begin{bmatrix} e{1x} & e{1y} & e{1z} \\ e_{2x} & e_{2y} & e_{2z} \\ e_{3x} & e_{3y} & e_{3z} \end{bmatrix} Rnew=[e1e2e3]⊤= e1xe2xe3xe1ye2ye3ye1ze2ze3z
（新旋转矩阵使成像平面共面且极线水平）。
2. 投影矩阵更新
目标 ：构造左右相机的新投影矩阵 P 1 ′ P_1' P1′ 和 P 2 ′ P_2' P2′，确保共面成像。
(1) 左相机投影矩阵 P 1 ′ P_1' P1′
-
新外参 ：位置仍为原点，姿态由 R new R_{\text{new}} Rnew 定义。
-
投影矩阵 ：
P 1 ′ = K new ⋅ [ R new 0 ] P_1' = K_{\text{new}} \cdot \begin{bmatrix} R_{\text{new}} & \mathbf{0} \end{bmatrix} P1′=Knew⋅[Rnew0]其中 K new K_{\text{new}} Knew 为调整后的内参矩阵（通常取左右相机内参的平均值，倾斜因子设为0）。
(2) 右相机投影矩阵 P 2 ′ P_2' P2′
-
基线长度 ： B = ∥ b ∥ = ∥ T ∥ B = \| \mathbf{b} \| = \| \mathbf{T} \| B=∥b∥=∥T∥。
-
平移向量 ：沿新坐标系x轴平移： T new = [ B , 0 , 0 ] ⊤ \mathbf{T}_{\text{new}} = [B, 0, 0]^\top Tnew=[B,0,0]⊤。
-
投影矩阵 ：
P 2 ′ = K new ⋅ [ R new T new ] P_2' = K_{\text{new}} \cdot \begin{bmatrix} R_{\text{new}} & \mathbf{T}_{\text{new}} \end{bmatrix} P2′=Knew⋅[RnewTnew]（右相机仅沿新x轴平移，保证极线水平对齐）。
3. 单应性矩阵（Homography）推导
目标：将原始图像像素映射到校正后平面。
(1) 原相机投影模型
原始左相机的投影方程为：
x 1 = K 1 ⋅ [ R 0 ] ⋅ X \mathbf{x}_1 = K_1 \cdot \begin{bmatrix} R & \mathbf{0} \end{bmatrix} \cdot \mathbf{X} x1=K1⋅[R0]⋅X
（其中 R R R 为原相机的旋转矩阵）。
(2) 新相机投影模型
校正后的左相机投影为：
x 1 ′ = K new ⋅ [ R new 0 ] ⋅ X \mathbf{x}1' = K{\text{new}} \cdot \begin{bmatrix} R_{\text{new}} & \mathbf{0} \end{bmatrix} \cdot \mathbf{X} x1′=Knew⋅[Rnew0]⋅X
(3) 单应性变换关系
联立两式消去 X \mathbf{X} X，得到单应矩阵 H 1 H_1 H1：
x 1 ′ = K new R new R − 1 K 1 − 1 ⋅ x 1 \mathbf{x}1' = K{\text{new}} R_{\text{new}} R^{-1} K_1^{-1} \cdot \mathbf{x}_1 x1′=KnewRnewR−1K1−1⋅x1
即：
H 1 = K new R new R − 1 K 1 − 1 H_1 = K_{\text{new}} R_{\text{new}} R^{-1} K_1^{-1} H1=KnewRnewR−1K1−1
（通过变换矩阵 H 1 H_1 H1 将原图像像素映射到校正后平面）。
(4) 右相机单应矩阵
同理，右相机的单应矩阵为：
H 2 = K new R new R − 1 K 2 − 1 H_2 = K_{\text{new}} R_{\text{new}} R^{-1} K_2^{-1} H2=KnewRnewR−1K2−1
（校正后右相机的极线与左相机严格水平对齐）。
4. 极线约束验证
校正后极线满足水平对齐，对应点视差仅沿x轴：
x 2 ′ = x 1 ′ + [ d 0 0 ] \mathbf{x}_2' = \mathbf{x}_1' + \begin{bmatrix} d \\ 0 \\ 0 \end{bmatrix} x2′=x1′+ d00
其中视差 d d d 与深度 Z Z Z 的关系为：
Z = B ⋅ f d Z = \frac{B \cdot f}{d} Z=dB⋅f
（ f f f 为焦距，极线水平化大幅简化立体匹配搜索）。
code:
cpp
#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) - 知乎](https://zhuanlan.zhihu.com/p/466365225)) \[2\]([立体视觉入门指南（6）：对级约束与Fusiello法极线校正 - 知乎](https://zhuanlan.zhihu.com/p/466758105))