opencv单目标定

单目矩阵求解修改

• 归一化直接线性变换

1. 通过计算一个相似变换T，将质心转换到原点，并缩放到离原点的平均距离为$\sqrt{2}$，将点集合标准化$ui=[u_i^{\prime },v_i^{\prime }{]}^T$
2. 同样地，通过计算相似变换T’对点$u^{\prime }=[u_i^{\prime },v_i^{\prime }{]}^T$进行归一化
3. 对归一化点应用基本的DLT算法，得到单应矩阵H‘
4. 去归一化得到单应性:$H=T^{\prime -1}H’T$

• 初始$f_x,f_y$求解修改

opencv做了一些简化

内参矩阵 ： $A=\left[\begin{array}{ccc}\alpha & \gamma & u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$ 图像坐标和像素坐标系存在一个比例关系，设图像x方向每毫米有$\alpha$个像素，y方向每毫米有$\beta$个像素，则有$u=u_0+x\cdot \alpha$，$v=v_0+y\cdot \beta$

1. 假设 $\gamma =0$ （因为只需要初始解，不需要那么精确）
2. 不直接求解光心位置，而是假设其为图像的一半（将$u_0$，$v_0$假设为图片的中心）

见张正友求解内参矩阵部分

$$\begin{array}{rl}B=\lambda A^{-T}{A}^{-1}& =\lambda \left[\begin{array}{lll}{B}_{11}& B_{12}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{31}& B_{32}& B_{33}\end{array}\right]\\ & =\lambda \left[\begin{array}{ccc}\frac{1}{{\alpha }^2}& 0& \frac{-u_0}{{\alpha }^2}\\ 0& \frac{1}{{\beta }^2}& -\frac{v_0}{{\beta }^2}\\ \frac{-u_0}{{\alpha }^2}& -\frac{v_0}{{\beta }^2}& \frac{u_0^2}{{\alpha }^2}+\frac{v_0^2}{{\beta }^2}+1\end{array}\right]\end{array}$$
由于$u_0$，$v_0$是已知项，这样我们只需要求解两个参数就好
$$b=\left[\begin{array}{ll}{B}_{11}& B_{22}\end{array}\right]=\left[\begin{array}{ll}\frac{1}{{\alpha }^2}& \frac{1}{{\beta }^2}\end{array}\right]$$
我们知道，张正友标定法中单应矩阵对应
H = A [ r 1 r 2 t ] ↓ H = [ α 0 u 0 0 β v 0 0 0 1 ] [ r 11 r 21 t 1 r 12 r 22 t 2 r 13 r 23 t 3 ] ↓ H = [ α r 11 + u 0 r 13 α r 21 + u 0 r 23 α t 1 + u 0 t 3 β r 12 + v 0 r 13 β r 22 + v 0 r 23 β t 1 + v 0 t 3 r 13 r 23 t 3 ] \begin{gathered} H=A\left[r_{1} r_{2} t\right] \\ \downarrow \\ H=\left[\begin{array}{ccc} \alpha & 0 & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{lll} r_{11} & r_{21} & t_{1} \\ r_{12} & r_{22} & t_{2} \\ r_{13} & r_{23} & t_{3} \end{array}\right] \\ \downarrow \\ H=\left[\begin{array}{ccc} \alpha r_{11}+u_{0} r_{13} & \alpha r_{21}+u_{0} r_{23} & \alpha t_{1}+u_{0} t_{3} \\ \beta r_{12}+v_{0} r_{13} & \beta r_{22}+v_{0} r_{23} & \beta t_{1}+v_{0} t_{3} \\ r_{13} & r_{23} & t_{3} \end{array}\right] \end{gathered} H=A[r1​r2​t]↓H=⎣⎡​α00​0β0​u0​v0​1​⎦⎤​⎣⎡​r11​r12​r13​​r21​r22​r23​​t1​t2​t3​​⎦⎤​↓H=⎣⎡​αr11​+u0​r13​βr12​+v0​r13​r13​​αr21​+u0​r23​βr22​+v0​r23​r23​​αt1​+u0​t3​βt1​+v0​t3​t3​​⎦⎤​​
opencv 做了这样一个操作

 H[0] -= H[6]*a[2];  // H[6] = r13,a[2] = u0,H[0] = \alpha * r11
 H[1] -= H[7]*a[2]; 
 H[2] -= H[8]*a[2];
 H[3] -= H[6]*a[5];
 H[4] -= H[7]*a[5]; 
 H[5] -= H[8]*a[5];

这里$\begin{array}{rlr}& a[2]=u_0,& a[5]=v_0\end{array}$
经过上面的操作, $H$ 变为
$$H=\left[\begin{array}{ccc}\alpha r_{11}& \alpha r_{21}& \alpha t_1\\ \beta r_{12}& \beta r_{22}& \beta t_1\\ r_{13}& r_{23}& t_3\end{array}\right]$$
修改形式，有
$$H=[hvj]=\left[\begin{array}{lll}{h}_0& v_0& j_0\\ h_1& v_1& j_1\\ h_2& v_2& j_2\end{array}\right]=\left[\begin{array}{ccc}\alpha r_{11}& \alpha r_{21}& \alpha t_1\\ \beta r_{12}& \beta r_{22}& \beta t_1\\ r_{13}& r_{23}& t_3\end{array}\right]$$
$$d_1=\frac{h+v}{2}=\left[\frac{\alpha r_{11}+\alpha r_{21}}{2}\frac{\beta r_{12}+\beta r_{22}}{2}\frac{r_{13}+r_{23}}{2}\right]$$
$$d_2=\frac{h-v}{2}=\left[\frac{\alpha r_{11}-\alpha r_{21}}{2}\frac{\beta r_{12}-\beta r_{22}}{2}\frac{r_{13}-r_{23}}{2}\right]$$
由使用旋转矩阵的正交性(任意两列，任意两行的内积都为0)，构建方程组
$$\left[\begin{array}{cc}{h}_0{v}_0& h_1{v}_1\\ d_{10}{d}_{20}& d_{11}{d}_{21}\end{array}\right]\left[\begin{array}{c}\frac{1}{{\alpha }^2}\\ \frac{1}{{\beta }^2}\end{array}\right]=\left[\begin{array}{c}-h_2{v}_2\\ -d_{12}{d}_{22}\end{array}\right]$$
代入有（目的是构建一个${\alpha }^2$与${\beta }^2$相关的函数)
$$\left[\begin{array}{cc}{\alpha }^2{r}_{11}{r}_{12}& {\beta }^2{r}_{12}{r}_{22}\\ {\alpha }^2{r}_{11}^2-{\alpha }^2{r}_{21}^2& {\beta }^2{r}_{12}^2-{\beta }^2{r}_{22}^2\end{array}\right]\left[\begin{array}{c}\frac{1}{{\alpha }^2}\\ \frac{1}{{\beta }^2}\end{array}\right]=\left[\begin{array}{c}{r}_{11}{r}_{21}+r_{12}{r}_{22}\\ r_{11}^2-r_{21}^2+r_{12}^2-r_{22}^2\end{array}\right]=\left[\begin{array}{c}-r_{13}{r}_{23}\\ -(r_{13}^2-r_{23}^2\end{array}\right]$$
求解上述线性方程即可得对应的相机焦距

迭代去畸变

opencv 采用一种迭代的方式来去畸变，其更新公式如下
$$x_{n+1}=f\left(x_n\right)$$

DLT算法

不考虑相机畸变，有
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{A}\left[\begin{array}{ll}R& T\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_W\\ Z_W\\ 1\end{array}\right]=P_{3\times 4}\left[\begin{array}{c}{X}_w\\ Y_W\\ Z_W\\ 1\end{array}\right]=\left[\begin{array}{cccc}{p}_{11}& p_{12}& p_{13}& p_{14}\\ p_{21}& p_{22}& p_{23}& p_{24}\\ p_{31}& p_{32}& p_{33}& p_{34}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_W\\ Z_W\\ 1\end{array}\right]$$
同样，我们对上式展开，有
$$\{\begin{array}{l}s=p_{31}{X}_w+p_{32}{Y}_w+p_{33}{Z}_w+p_{34}\\ p_{11}{X}_w+p_{12}{Y}_w+p_{13}{Z}_w+p_{14}-p_{31}u{X}_w-p_{32}u{Y}_w-p_{33}u{Z}_w-p_{34}u=0\\ p_{21}{X}_w+p_{22}{Y}_w+p_{23}{Z}_w+p_{24}-p_{31}v{X}_w-p_{32}v{Y}_w-p_{33}v{Z}_w-p_{34}v=0\end{array}$$
写成矩阵形式有
$$\left[\begin{array}{cccccccccccc}X& Y& Z& 1& 0& 0& 0& 0& -uX& -uY& -uZ& -u\\ 0& 0& 0& 0& X& Y& Z& 1& -uX& -uY& -uZ& -u\end{array}\right]\left[\begin{array}{l}{p}_{11}\\ p_{12}\\ p_{13}\\ p_{14}\\ p_{21}\\ p_{22}\\ p_{23}\\ p_{24}\\ p_{31}\\ p_{32}\\ p_{33}\\ p_{34}\end{array}\right]=0$$
上式依然使用SVD分解求解即可。

畸变修改

我们之前定义的畸变形式如下
$$\begin{array}{rl}{x}_{\text{distorted }}& =x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ y_{\text{distorted }}& =y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$

1. 薄透镜畸变

Opencv 增加了薄透樈畸变，其定义如下
$$\begin{array}{rl}& \delta x=s_1{r}^2+s_2{r}^4\\ & \delta y=s_3{r}^2+s_4{r}^4\end{array}$$
则总畸变形式为
$$\begin{array}{rl}& x_{\text{distorted }}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)+s_1{r}^2+s_2{r}^4\\ & y_{\text{distorted }}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy+s_3{r}^2+s_4{r}^4\end{array}$$

2. Tilted Model

理想成像平面和实际成像平面的倾斜

为了描述安装平面和成像平面可能存在的倾斜, opencv 也增加了倾斜模型，其使用绕 $\mathrm{x}$ 、y轴的两个旋 转角 ${\theta }_x{\theta }_y$ 来描述理想图像坐标系到实际图像坐标系之间 (和像素坐标系贴合）投影的结果，作用在我们 上述的畸变之后，其定义如下

opencv使用了\taox，\taoy,而不是 ${\theta }_x{\theta }_y$。注意这里的正负号

$$\begin{array}{rl}\mathrm{mat}\mathrm{Rot}\mathrm{X}& =\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\theta }_x& \sin {\theta }_x\\ 0& -\sin {\theta }_x& \cos {\theta }_x\end{array}\right]\\ \mathrm{matRot}\mathrm{Y}& =\left[\begin{array}{ccc}\cos {\theta }_y& 0& -\sin {\theta }_y\\ 0& 1& 0\\ \sin {\theta }_y& 0& \cos {\theta }_y\end{array}\right]\end{array}$$
需要注意的是，这里的角度和我们常规定义是相反的，因为该模型定义是从畸变成像到理想像面的变换，因此我们投影时用的是其逆矩阵。按照先Y后X的顺序，我们有总的旋转矩阵
$$R=\left[\begin{array}{ccc}\cos {\theta }_y& \sin {\theta }_y\sin {\theta }_x& -\sin {\theta }_y\cos x\\ 0& \cos {\theta }_x& \sin {\theta }_x\\ \sin {\theta }_y& -\cos {\theta }_y\sin {\theta }_x& \cos {\theta }_x\cos {\theta }_y\end{array}\right]$$

那么对于点 $[0,0,1]$ 其旋转后点为 $\left[R_{02}{R}_{12}{R}_{22}\right]$, 我们希望投影后其在新的坐标系下依然为 $[0,0,1]$, 因 此构建了一个投影矩阵
$$\mathrm{matProj}\mathbf{Z}=\left[\begin{array}{ccc}{R}_{22}& 0& -R_{02}\\ 0& R_{22}& -R_{12}\\ 0& 0& 1\end{array}\right]$$
那么总的投影矩阵为
$$P=\left[\begin{array}{ccc}\cos {\theta }_x& 0& 0\\ -\sin {\theta }_x\sin {\theta }_y& \cos {\theta }_y& 0\\ \sin {\theta }_y& -\cos {\theta }_y\sin {\theta }_x& \cos {\theta }_x\cos {\theta }_y\end{array}\right]$$

3. 罗德里格斯形式变换

已知R求罗德里格斯旋转表达形式
$$\sin \theta \left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]=\frac{R-R^T}{2}$$

// 旋转矩阵R——>罗德里格斯
 Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); 

多相机标定

• 单目相机标定流程
  1.不同角度拍摄棋盘格
  2.将坐标系固定在棋盘格上，计算3D坐标
  3.计算相机内参矩阵
  4.计算每个棋盘格和相机的相对旋转和平移
  5.优化每个棋盘格到相机的姿态以及相机内参

• 双目相机标定流程
  1.不同角度拍摄棋盘格，需要保证两个相机拍到完整的棋盘格，以方便后续点对应
  2.每个相机单独标定，分别计算相机的内参以及每个棋盘格到相机的旋转和平移
  3.由于每个姿态的棋盘格在左右相机中是一一对应的，根据每个姿态每个棋盘格到相机的相对旋
  转，计算两个相机的相对旋转
  4.将优化目标设置为每个棋盘格到左相机的旋转矩阵，以及使用左相机到右相机的相对旋转和平
  移进行姿态传递，进行全局优化

MARKER

Random Pattern

这种标志物的思路来自特征匹配，我们知道特征点，比如经典的sift有128维的特征描述子，可以表征每个特征自己的性质。也就是说，对于一些特定的标志物，我们完全可以使用特征点作为其角点进行匹配。Opencv 提供了一种方式，就是Random Pattern。

chAruco/aprilgrid

apriltag 和 arUco 都是使用一定的编码方式来保证其检测鲁棒性的标志物，我们可以使用预先定义的marker来确定检测的角点具体是什么位置上的

opencv 本身集成了 ArUco 的实现，也在 contrib模块中。

• 生成ChArUco的代码

const char* keys =
"{@outfile |<none> | Output image }"
"{w | | Number of squares in X direction }"
"{h | | Number of squares in Y direction }"
"{sl | | Square side length (in pixels) }"
"{ml | | Marker side length (in pixels) }"
"{d | | dictionary: DICT_4X4_50=0, DICT_4X4_100=1,
DICT_4X4_250=2,"
"DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6,
DICT_5X5_1000=7, "
"DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10,
DICT_6X6_1000=11, DICT_7X7_50=12,"
"DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15,
DICT_ARUCO_ORIGINAL = 16}"
"{m | | Margins size (in pixels). Default is
(squareLength-markerLength) }"
"{bb | 1 | Number of bits in marker borders }"
"{si | false | show generated image }";
}
int main(int argc, char *argv[]) {
CommandLineParser parser(argc, argv, keys);
parser.about(about);
if(argc < 7) {
parser.printMessage();
return 0;
}
int squaresX = parser.get<int>("w");
int squaresY = parser.get<int>("h");
int squareLength = parser.get<int>("sl"); //黑色格子的边长
int markerLength = parser.get<int>("ml"); //白色格子中ArUco的边长
int dictionaryId = parser.get<int>("d");  // 字典族的名字
int margins = squareLength - markerLength;
if(parser.has("m")) {
margins = parser.get<int>("m");
}
int borderBits = parser.get<int>("bb");
bool showImage = parser.get<bool>("si");
String out = parser.get<String>(0);
if(!parser.check()) {
parser.printErrors();
return 0;
}
//生成图片的字典族
Ptr<aruco::Dictionary> dictionary =
aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictio
naryId));
Size imageSize;
imageSize.width = squaresX * squareLength + 2 * margins;
imageSize.height = squaresY * squareLength + 2 * margins;
//创建chAruco
Ptr<aruco::CharucoBoard> board =
aruco::CharucoBoard::create(squaresX, squaresY, (float)squareLength,
(float)markerLength, dictionary);
// show created board
Mat boardImage;
board->draw(imageSize, boardImage, margins, borderBits);
if(showImage) {
imshow("board", boardImage);
waitKey(0);
}
imwrite(out, boardImage);
return 0;
}