Bouguet算法--源码解析

最近在看双目视觉极线校正相关问题，看了很多博客和论文，关于 Bouguet 算法，自己推导的时候发现逻辑总是对不上，于是找到了 Bouguet 的原版代码，从代码上学习 Bouguet 博士的相关思想和流程，记录如下。由于个人水平有限，若有错误请大家指出来。

这是 Bouguet 校准工具箱地址，matlab的版本， 作者提到 C 版本的代码被移植到 OpenCV 中，所以如果是想了解 OpenCV 关于极线校正的流程，这篇文章也应该有帮助。工具箱源码也可以直接在这里下载。

Camera Calibration Toolbox for Matlab

作者给了一些示例用于理解工具箱的使用，双目标定的实验参考，其中还包括一组测试数据，很贴心了。

Camera Calibration Toolbox for Matlabhttp://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html数学上的原理，本人无能为了，只能根据代码写一些自己的理解。

设左右相机的位姿关系满足：。 和 分别是右侧、左侧相机坐标系下的一点，R 和 T 是旋转和平移，极线校正只用到了 R 和 T。大部分文章中根本没写 R 和 T 是如何定义的，所以上来就感觉一头雾水这里的定义也是根据 Bouguet 自己的定义方式。

截图来自上面链接的网页：

 双目标定之后就有了 R 和 T，就可以开始构造变换矩阵了。在工具箱的  rectify_stereo_pair.m 中就是构造方法：

R = rodrigues(om);

% Bring the 2 cameras in the same orientation by rotating them "minimally": 
r_r = rodrigues(-om/2);
r_l = r_r';
t = r_r * T;

 第一行 R = rodrigues(om); 表示根据旋转向量 om 计算旋转矩阵，使用的是罗德里格斯公式。R的定义就是上面的公式，即 。第二句 r_r = rodrigues(-om/2);  这里传入了 -om/2 ,我们知道旋转向量的模表示旋转角度，1/2*om 自然就表示旋转一半角度，加上负号就表示反向旋转。r_r 也等于，矩阵分数次幂参考：矩阵的1/2次方_creator123123的博客-CSDN博客 

r_l=r_r'  ,也就是 r_l 是 r_r 的转置，因为是正交矩阵，所以也是逆。综上就有关系式：

然后最后一句 t = r_r * T;  这里 T 是平移向量，根据上面的定义，T表示右侧坐标系原点指向左侧坐标系原点的向量在右侧坐标系下的坐标，而 t 是用于计算接下来的变换矩阵，这和大部分博客中写的也不一样。我个人倾向于代码中是正确的，如果使用 T 构造矩阵也能得到好的结果，原因可能是 r_r 近似一个单位矩阵，因为两个相机摆放的时候也会使近似理想的双目视觉系统，所以最终 T 和 t 的差异不大。

% Rotate both cameras so as to bring the translation vector in alignment with the (1;0;0) axis:
if abs(t(1)) > abs(t(2)),
    type_stereo = 0;
    uu = [1;0;0]; % Horizontal epipolar lines
else
    type_stereo = 1;
    uu = [0;1;0]; % Vertical epipolar lines
end;
if dot(uu,t)<0,
    uu = -uu; % Swtich side of the vector 
end;
ww = cross(t,uu);
ww = ww/norm(ww);
ww = acos(abs(dot(t,uu))/(norm(t)*norm(uu)))*ww;
R2 = rodrigues(ww);

接下来开始构造变换矩阵，一般也叫 。

第一个 if abs(t(1)) > abs(t(2)), 是为了判断两个相机是水平左右放置还是上下放置，我们按水平左右放置来进行。uu =[1,0,0]，也就是 x 轴上的单位向量。

if dot(uu,t)<0,
    uu = -uu; % Swtich side of the vector 
end;

是判断 t 和x轴正方向之间的夹角，如果大于90度，uu 就取反，使得 uu 和 t 的夹角小于90度。因为上面说了 T 是右侧指向左侧的向量，R 近似单位矩阵，所以 t 也是指向x轴负方向，最终的

uu=[-1,0,0]，也就是x轴负方向。到这里就和大部分文章写的完全不一样了。

ww = cross(t,uu);
ww = ww/norm(ww);                % 旋转向量归一化

t 和 uu 叉积，得到的 ww 是旋转向量的方向，因为我们的目的是把 t 变换到x轴，所以旋转轴必然同时垂直与 t 和 uu。

ww = acos(abs(dot(t,uu))/(norm(t)*norm(uu)))*ww;

这一句是计算 t 和 uu 之间的夹角，并且把角度乘到旋转方向上，得到的就是包含旋转方向和旋转角的旋转向量ww。

R2 = rodrigues(ww);   

罗德里格斯公式，计算变换矩阵。代码中的 R2 就是变换矩阵 。

顺便提一下，这里构造出的   和使用下面的方法构造的矩阵只在符号上存在差异，数值上一样，原因未知。

 最终的变换矩阵如下：

% Global rotations to be applied to both views:
R_R = R2 * r_r;
R_L = R2 * r_l;

然后讲解极线校正的代码流程，大部分文章也缺少这一部分。

计算出变换矩阵之后，想要进行极线校正还是不够的，还需要生成两个相机的共同内参矩阵。代码中也表示：如果没有共同的内参矩阵，使用相机自身的内参矩阵也可以，这样做就是省事。双目视觉中要求两个相机的内参一样，但实际中显然是做不到的，所以在标定的时候会生成一个共同的内参矩阵。代码也在 rectify_stereo_pair.m  中，太长了，大家自己看。OpenCV 应该也是一样的流程。

校正实际是生成了一个映射表，而且映射表是一个逆过程，和畸变校正非常类似。对于M*N的空白图像，每个像素都能通过上面的极线校正矩阵映射到一个新的坐标，这个新的坐标通常是小数，所以需要通过原图像进行插值来填补空白图像的灰度，空白图像填补后就是我们极线校正之后的图像，也就是行对齐的图像。够绕的，如果做过反畸变，应该很容易理解。

工具箱中的 rect_index.m 函数就是用于生成查找表。

rect_index(I,R,f,c,k,alpha,KK_new); 函数入参的含义：

I 是空白图像，R是校正矩阵，如下：映射表有两个，右侧图像使用 R_R , 左侧使用 R_L。

% Global rotations to be applied to both views:
R_R = R2 * r_r;
R_L = R2 * r_l;

f,c,k,alpha,是相机内参和畸变。KK_new 就是两个相机共同的内参矩阵，是经过算法得到的。

可以发现极线校正需要的参数还是比较多的。没有 KK_new 使用相机自身的内参矩阵也可以。

[mx,my] = meshgrid(1:nc, 1:nr);
px = reshape(mx',nc*nr,1);
py = reshape(my',nc*nr,1);

rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))];


% Rotation: (or affine transformation):

rays2 = R'*rays;

x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)];

px和py 也就是标准模型中的像素坐标。

rays 是利用内参把像素坐标转到相机坐标系，并且每个像素点都被映射到归一化平面上，因为 rays 的第三个维度是1。

rays2 = R'*rays;

这一句是极线校正的核心代码，rays 是归一化坐标，R是变换矩阵。

但rays 乘的是 R 转置，也就是R的逆。因为我们是通过空白图像上的点的坐标反推该点在原图像上的坐标，所以是乘R的逆。

x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)];  依然是归一化。

% Add distortion:
xd = apply_distortion(x,k);


% Reconvert in pixels:

px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1);
py2 = f(2)*xd(2,:)+c(2);


% Interpolate between the closest pixels:

xd = apply_distortion(x,k); 是加畸变，目的是为了找到畸变后点在原图上的坐标，这样做就可以得到不含畸变且行对其的校正图像(行对其的图像一定是去畸变的图像，去畸变之前的图像直线不直，自然不可能行对齐)。

px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1);        % 相机坐标系到像素坐标系
py2 = f(2)*xd(2,:)+c(2);

这两句是将极线校正和加畸变之后的相机坐标系下的点映射到像素坐标系，得到坐标px2 、py2 .注意使用的是相机自己的内参，不是共同的内参。

映射之后得到的像素坐标大部分都不是整数，还有一些坐标会超过图像范围。所以接下来就是插值操作。作者使用的是双线性插值，双线性插值的好处是插值系数也可以提前算出来，无需每张图像都算插值系数。映射表提前算，插值系数也提前算，可以实现极致快速计算。

最后得到一个映射表，映射表由[px ，py] 和 [px2 ，py2]组成。比如现在有一张空白图像Img，图像行列和相机采集的图像一致，有一张真实图像Img2 ， 空白图像上的一个点坐标[px ，py] ，该点的灰度值由Img2上的 [px2 ，py2]处灰度来填充， [px2 ，py2] 是小数，所以通过Img2的插值来得到灰度。空白图像填充灰度之后的图像就是真实图像校正之后的图像，也就是行对齐的图像。左右相机各有一个查找表，需要各自单独操作。

总结：本文只是对代码进行解析，高深的数学推导无能为力了。转载注明出处