三维重建代码实现（二）

写在开头

最近在学习三维重建的相关知识，打算将三维重建SFM的整个过程用代码的形式梳理一下，本章节主要实现相机标定。
这里我们假定你有一定的三维重建相关的基本知识，作者在这里推荐高翔博士的《视觉SLAM十四讲：从理论到实践》，在B站上有高翔博士的讲解视频。

相机标定

我们首先做一个约定：
二维坐标点： $m=\left[ \begin{matrix} u\\v \end{matrix} \right]$ ，三维点坐标 $M =\left[ \begin{matrix} X\\Y\\Z \end{matrix} \right]$ 。
则他们的坐标对应的齐次形式是： $\tilde{m}=\left[ \begin{matrix} u\\v\\1 \end{matrix} \right]$ ，三维点坐标 $\tilde{M} = \left[ \begin{matrix} X\\Y\\Z\\1 \end{matrix} \right]$ 。
两者之间的关系是：
$s \cdot \tilde{m}=A\left[\begin{array}{ll} R & t \end{array}\right] \tilde{M}$

其中 $A=\left[\begin{array}{lll} \alpha & c & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1 \end{array} \right]$

$s$ 是一个比例参数， $R,t$ 是相机外参， $R$ 是旋转矩阵， $t$ 是平移。 $A$ 是相机内参， $(u_0,v_0)$ 是坐标的主点， $α$ 和 $β$ 是图像在 $u$ 轴和 $v$ 轴的比例因子， $c$ 是描述两个坐标轴亲倾斜角的参数（注：如果两个坐标轴相互垂直，则 $c$ =0，即默认情况下这个 $c$ 都是为0的）。

模型平面与图像之间的单应性关系

在计算机视觉中，平面的单应性被定义为一个平面到另外一个平面的投影映射。因此一个二维平面上的点映射到摄像机成像仪上的映射就是平面单应性的例子。
设 $R$ 的第 $i$ 列旋转矩阵为 $r_i$ ，那么 $R$ 可以表示为：

$R=\left[\begin{array}{lll} r_{1} & r_{2} & r_{3} \end{array}\right]$

带入到原方程中：
$s \cdot\left[ \begin{array}{l} u \\ v \\ 1 \end{array}\right]=A\left[\begin{array}{llll} r_{1} & r_{2} & r_{3} & t \end{array}\right]\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array} \right]$

假设模型平面在世界坐标系中 $Z$ 坐标为0，那么 $Z$ 坐标的值都为0：
$s \cdot\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]=A\left[\begin{array}{llll} r_{1} & r_{2} & r_{3} & t \end{array}\right]\left[\begin{array}{l} X \\ Y \\ 0 \\ 1 \end{array}\right]$
乘出来都还是0，省略掉这一部分：
$s \cdot\left[\begin{array}{l} u \\ v \end{array}\right]=A\left[\begin{array}{lll} r_{1} & r_{2} & t \end{array}\right]\left[\begin{array}{l} X \\ Y \\ 1 \end{array}\right]$

则此时的二维点坐标为 $m=\left[ \begin{matrix} u\\v \end{matrix} \right]$ ，三维点坐标为 $m=\left[ \begin{matrix} X\\Y\\1 \end{matrix} \right]$
因此点 $M$ 和它在图像上的映射点 $m$ 之间的关系可以使用单应矩阵 $H$ 来表示：
$s \cdot \tilde{m}=H \cdot \tilde{M}$ 其中：
$H=A\left[\begin{array}{lll} r_{1} & r_{2} & t \end{array}\right]$
很显然， $H$ 是一个 $3×3$ 的矩阵。 $A$ 对应着内参， $[r_1 r_2 t]$ 对应着外参。

内参的约束条件

令 $H=\left[\begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right]$ ， $h_1, h_2, h_3$ 都是 $3×1$ 的矩阵，那么则有：
$\left[ \begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right]=\lambda A\left[\begin{array}{lll} r_{1} & r_{2} & t \end{array}\right]$
式中 $\lambda$ 是任意的标量。
因为在旋转矩阵中，任意两列两两正交，则有：
$r_{1}^{T} r_{2}=0,\left\|r_{1}\right\|=\left\|r_{2}\right\|=1$
再根据 $\left[ \begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right]=\lambda A\left[\begin{array}{lll} r_{1} & r_{2} & t \end{array}\right]$ 得到方程组：
$\left\{\begin{array}{l} h_{1}=\lambda A r_{1} \\ h_{2}=\lambda A r_{2} \\ h_{3}=\lambda A t \end{array}\right.$
推出：
$\left\{\begin{array}{l} r_{1}=\lambda^{-1} A^{-1} h_{1} \\ r_{2}=\lambda^{-1} A^{-1} h_{2} \end{array}\right.$
代入前面的条件：
$r_{1}^{T} r_{2}=0,\left\|r_{1}\right\|=\left\|r_{2}\right\|=1$
得到：
$\begin{array}{c} r_{1}^{T} r_{2}=\lambda^{-T} h_{1}^{T} A^{-T} A^{-1} h_{2} \lambda^{-1}=\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{2}=0 \\ \quad\left\{\begin{array}{c} \left\|r_{1}\right\|^{2}=\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=1 \\ \left\|r_{2}\right\|^{2}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2}=1 \\ \Rightarrow \lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2} \end{array}\right. \end{array}$
综上所述，对于一个单应矩阵 $H$ ，对于内参有2个基本的约束条件：

$\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{2}=0$
$\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2}$

对于 $H$ 矩阵来说，它是一个 $3×3$ 的矩阵，有9个参数，那么就有8个自由度。对应的外参有6个（注：旋转矩阵R有3个，平移向量t有3个）。到这一步，我们只能得到内参的约束条件，却没法解出来。

相机标定过程

利用约束条件求出内参矩阵

令 $B=A^{-T} A^{-1}$ ,而 $A=\left[\begin{array}{lll}\alpha & c & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{\alpha} & -\frac{c}{\alpha \beta} & \frac{v_{0} c-u_{0} \beta}{\alpha \beta} \\ 0 & \frac{1}{\beta} & -\frac{v_{0}}{\beta} \\ 0 & 0 & 1\end{array}\right]$ ，计算 $B$ 得到：
$B=\left[\begin{array}{ccc} \frac{1}{\alpha} & 0 & 0 \\ -\frac{c}{\alpha \beta} & \frac{1}{\beta} & 0 \\ \frac{v_{0} c-u_{0} \beta}{\alpha \beta} & -\frac{v_{0}}{\beta} & 1 \end{array}\right] \cdot\left[\begin{array}{ccc} \frac{1}{\alpha} & -\frac{c}{\alpha \beta} & \frac{v_{0} c-u_{0} \beta}{\alpha \beta} \\ 0 & \frac{1}{\beta} & -\frac{v_{0}}{\beta} \\ 0 & 0 & 1 \end{array}\right]$
$=\left[\begin{array}{ccc} \frac{1}{\alpha^{2}} & -\frac{c}{\alpha^{2} \beta} & \frac{v_{0} c-u_{0} \beta}{\alpha^{2} \beta} \\ -\frac{c}{\alpha^{2} \beta} & \frac{c^{2}}{\alpha^{2} \beta}+\frac{1}{\beta^{2}} & -\frac{c\left(v_{0} c-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{v_{0}}{\beta^{2}} \\ \frac{v_{0} c-u_{0} \beta}{\alpha^{2} \beta} & -\frac{c\left(v_{0} c-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{v_{0}}{\beta^{2}} & \frac{\left(v_{0} c-u_{0} \beta\right)^{2}}{\alpha^{2} \beta^{2}}+\frac{v_{0}^{2}}{\beta^{2}}+1 \end{array}\right]$

我们可以发现 $B$ 是一个对称向量，我们设：
$b=\left[\begin{array}{l} B_{11} \\ B_{12} \\ B_{22} \\ B_{13} \\ B_{23} \\ B_{33} \end{array}\right]$

则有：
$B=\left[\begin{array}{lll} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{array}\right]$
又有 $H=\left[\begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right]$ ，对于 $h^T_iBh_j$ :
$\begin{aligned} h_{i}^{T} B h_{j} &=\left[\begin{array}{lllll} h_{i 1} & h_{i_{2}} & h_{13} \end{array}\right] \cdot\left[\begin{array}{ccc} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{array}\right] \cdot\left[\begin{array}{c} h_{j 1} \\ h_{32} \\ h_{33} \end{array}\right] \\ &=\left(h_{11} B_{11}+h_{12} B_{12}+h_{13} B_{13}\right) h_{j 1}+\left(h_{11} B_{12}+h_{22} B_{22}+h_{13} B_{23}\right) h_{12}+\left(h_{13} B_{13}+h_{12} B_{23}+h_{13} B_{33}\right) h_{33} \\ &=h_{11} h_{j 1} B_{11}+\left(h_{12} h_{11}+h_{11} h_{12}\right) B_{12}+h_{12} h_{32} B_{22}+\left(h_{13} h_{j 1}+h_{11} h_{33}\right) B_{13}+\left(h_{13} h_{32}+h_{12} h_{33}\right) B_{23}+h_{33} h_{33} B_{33} \end{aligned}$
则有：
$h_{i}^{T} B h_{j}=v_{i j}^{T} b$
其中：
$v_{i j}=\left[\begin{array}{c} h_{i 1} h_{j 1} \\ \left(h_{i 2} h_{j 1}+h_{i 1} h_{j 2}\right) \\ h_{i 2} h_{j 2} \\ \left(h_{i 3} h_{j 1}+h_{i 1} h_{j 3}\right) \\ \left(h_{i 3} h_{j 2}+h_{i 2} h_{j 3}\right) \\ h_{i 3} h_{j 3} \end{array}\right]$
我们有约束条件：
$\left\{\begin{array}{l} \lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{2}=0 \\ \lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2} \end{array}\right.$
这两个约束条件可以改写为齐次形式：
$\left\{\begin{array}{l} v_{12}^{T} b=0 \\ \left(v_{11}-v_{22}\right)^{T} b=0 \end{array}\right.$
我们用一个新的矩阵 $V$ 来表示这两个式子：
$V=\left[ \begin{array}{c} v_{12}^{T} \\ \left(v_{11}-v_{22}\right)^{T} \end{array}\right]$
这里的 $V$ 是一个 $2×6$ 的矩阵。约束条件变为： $V⋅b=0$ 。
如果观察了 $n$ 张图片，那么可以得到n个方程 $V⋅b=0$ 。我们想要解出 $b$ ， $b$ 是一个6维向量，要求出唯一解，则至少需要6个方程。一个 $V⋅b=0$ 有2个约束条件，那么要求出唯一解，至少需要3个 $V⋅b=0$ ，即至少需要3张图片（n≥3）。
如果我们求出了唯一解b，那么就可以得到B，那就也可以求出相机内参A。
$\left\{\begin{array}{l} v_{0}=\left(B_{12} B_{13}-B_{11} B_{23}\right) /\left(B_{11} B_{22}-B_{12}^{2}\right) \\ \lambda=B_{33}-\left[B_{13}^{2}+v_{0}\left(B_{12} B_{13}-B_{11} B_{23}\right)\right] / B_{11} \\ \alpha=\sqrt{ \lambda / B_{11}} \\ \beta=\sqrt{ \lambda B_{11} /\left(B_{11} B_{22}-B_{12}^{2}\right)} \\ c=-B_{12} \alpha^{2} \beta / \lambda \\ u_{0}=c v_{0} / \alpha-B_{13} \alpha^{2} / \lambda \end{array}\right.$

利用内参矩阵 $A$ 求解外参

通过前面的方法，假设我们已经求到了内参矩阵A。
我们使用下面的式子表示点M和它在图像上的映射点m之间的关系：
$s \cdot \tilde{m}=H \cdot \tilde{M}$
如果我们已知图片中的点m的坐标，以及点M在三维空间中的坐标，s又只是个常数，那么我们可以求出单应性矩阵H。
$\left[\begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right]=\lambda A\left[\begin{array}{lll} r_{1} & r_{2} & t \end{array}\right]$
求解外参：
$\left\{\begin{array}{l} r_{1}=\lambda^{-1} A^{-1} h_{1} \\ r_{2}=\lambda^{-1} A^{-1} h_{2} \\ t=\lambda^{-1} A^{-1} h_{3} \end{array}\right.$
又有：
$\left\{\begin{array}{l} r_{3}=r_{1} \times r_{2} \\ \left\|r_{1}\right\|=\left\|r_{2}\right\|=1 \end{array}\right.$
得：
$\begin{array}{c} \left\{\begin{array}{l} r_{1}=\lambda^{-1} A^{-1} h_{1} \\ r_{2}=\lambda^{-1} A^{-1} h_{2} \\ r_{3}=r_{1} \times r_{2} \\ t=\lambda^{-1} A^{-1} h_{3} \end{array}\right. \\ \left\|\lambda^{-1} A^{-1} h_{1}\right\|=\left\|\lambda^{-1} A^{-1} h_{2}\right\|=1 \\ \Rightarrow \lambda=\left\|A^{-1} h_{1}\right\|=\left\|A^{-1} h_{2}\right\| \end{array}$
这样就求出了外参的各项参数。

极大似然估计

假设我们得到了模型平面的n幅图片，模型平面上有m个点，假设图像上像素点的噪声服从独立的同一分布，下面给出极大似然优化问题：
$\sum_{i=1}^{n} \sum_{j=1}^{m}\left\|m_{i j}-\hat{m}\left(A, R_{i}, t_{i}, M_{j}\right)\right\|$

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 216,843评论 6赞 502
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 92,538评论 3赞 392
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 163,187评论 0赞 353
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 58,264评论 1赞 292
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 67,289评论 6赞 390
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 51,231评论 1赞 299
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 40,116评论 3赞 418
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 38,945评论 0赞 275
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 45,367评论 1赞 313
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 37,581评论 2赞 333
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,754评论 1赞 348
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,458评论 5赞 344
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 41,068评论 3赞 327
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,692评论 0赞 22
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,842评论 1赞 269
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 47,797评论 2赞 369
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 44,654评论 2赞 354