这一章由来：二郎之前研究matlab的双目立体视觉，已经得到了需要的信息，可是，二郎想要对代码进行修改使其更适宜自己的应用目的。
修改：标定不用修改……matlab内置的已经很强大了，而且没有必要用python和c++再做一遍，因此考虑的方法为——matlab标定后，参数导入到opencv中使用，这也是应用到python的一个原因。

一、matlab标定工具箱

matlab提供了两种方法：1.用自带的app进行标定（在双目立体视觉的文章中有详细讲解）；2.用matlab标定工具箱，像下面这样的东西

与其说它是一个工具箱，更确切地说它就是没有被matlab集成到内部，可以看到的m文件。
用法非常简单，下载在网上找一下，很多，或者找二郎要……吐槽一下，有的博主把这些东西挂到CSDN卖积分……感觉好不地道。
http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html（一篇英文教程，不想看英文的也可以接着文章往下看，二郎给大家做一遍）
1.下载的压缩包解压……到哪都行

这里需要注意……点进来之后，要把这个文件夹添加到执行目录，要不后续无法进行。
2.运行文件

3.选择进入程序

4.将图片目录添加到执行目录，且进入图片目录

5.以此运行，不用设置参数





点完后，去看matlab命令行窗口

上面二郎之前做过所以又记录，其实是需要输入棋盘格的尺寸的，也就是那个30,30

…………然后就一直持续标四个点，直至标完所有，二郎认为大家肯定会出现在标定中小手一抖，标跑偏的现象，没关系，后面能修改，先把四个点都标上再说。
表完之后点计算标定


你会发现我们想要求得值就出来了。然后用分析一下错误，可以看到我们哪张图错误率较大

我们可以用一下重投影，将我们求得内外参数代入公式，算出各点，带回到图像，计算误差（当然，这一步是计算机做的），重投影主要是为了recop.corners计算提高精度。再次确定角点时，以重投影的角点为起始角点，开始寻找角点。

做完上面一些步骤后我们的精度已经得到明显的提高
之后就可以针对单张图来进行操作了，比如再进行一下步骤3，这次不选所有图，而是单独找我们之前标错的图，进行重新的角点标注。在重新计算角点时，可以人为地设置窗口大小，之前默认为5*5，可以适当调大或者缩小。
多次进行角点检测的方法
第二次：Recomp.corners→[9,9]
第三次：Recomp.corners→[8,8]
第四次：Recomp.corners→[7,7]
逐渐缩小角点检测的窗口，避免产生窗口过小，造成的局部误差。

双目标定
当我们左右相机的单目标定完成后，将标定结果进行保存，建议保存名字为
(Calib_Result_left.mat和Calib_Result_right.mat)
运行工具箱里的stereo_gui.m

导入数据(Calib_Result_left.mat和Calib_Result_right.mat)，进行双目标定。

二、旋转矩阵和旋转向量

从上面的介绍中可以看到matlab已经集成的标定程序和标定工具箱各有各自的优点，弄哪一个都是可以的。在matlab已经集成的标定程序使用时，我们的2相机相对于1相机的旋转关系为旋转矩阵（9个参数），然而，在opencv和python中，多数程序使用的数据为旋转向量（3个参数）形式，这里涉及了一个转换定理——罗德里格斯（Rodrigues）旋转向量与矩阵的变换

图片截取于：https://wenku.baidu.com/view/b471efda6037ee06eff9aef8941ea76e58fa4acb.html?from=search

而具体如何实现旋转矩阵和旋转向量的转换呢？
下面列出了程序
python程序——程序不是二郎写的，来自github，方便大家看，因此黏贴过来。
https://github.com/blzq/tf_rodrigues/commit/338758887a18f0db4e4a38c949be120d7f5169e3#diff-c5ff043af7299aff712f4dc2fb35bba3

#!/usr/bin/python3
# -*- encoding: utf-8 -*-

import tensorflow as tf

def rodrigues_batch(rvecs):
    """ 
    Convert a batch of axis-angle rotations in rotation vector form shaped
    (batch, 3) to a batch of rotation matrices shaped (batch, 3, 3).
    See
    https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
    https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
    --- second article is wrong?
    """
    batch_size = tf.shape(rvecs)[0]

    thetas = tf.norm(rvecs, axis=1, keepdims=True)
    u = rvecs / thetas

    # Each K is the cross product matrix of unit axis vectors
    # pyformat: disable
    zero = tf.zeros([batch_size])  # for broadcasting
    Ks_1 = tf.stack([  zero   , -u[:, 2],  u[:, 1] ], axis=1)  # row 1
    Ks_2 = tf.stack([  u[:, 2],  zero   , -u[:, 0] ], axis=1)  # row 2
    Ks_3 = tf.stack([ -u[:, 1],  u[:, 0],  zero    ], axis=1)  # row 3
    # pyformat: enable
    Ks = tf.stack([Ks_1, Ks_2, Ks_3], axis=2)                  # stack rows

    Rs = tf.eye(3, batch_shape=[batch_size]) + \
         tf.sin(thetas)[..., tf.newaxis] * Ks + \
         (1 - tf.cos(thetas)[..., tf.newaxis]) * tf.matmul(Ks, Ks)

    print(Rs.shape)
    return Rs

# For testing only
if __name__ == '__main__':
    import numpy as np
    import cv2
    rvecs = np.random.randn(4, 3).astype(np.float32)

    rvecs_tf = tf.constant(rvecs)
    Rs = rodrigues_batch(rvecs_tf)
    with tf.Session() as sess:
        print("TensorFlow: ")
        print(sess.run(Rs))

    print("\nOpenCV: ")
    for rvec in rvecs:
        mat, _ = cv2.Rodrigues(rvec)
        print(mat.T)
        print()

opencv的程序——来自网上，具体出处不明（输入旋转向量则转化为旋转矩阵，输入旋转矩阵，则转化为旋转向量）

#include <stdio.h>
#include <cv.h>

void main()
{
    int i;
    double r_vec[3]={-2.100418,-2.167796,0.273330};
    double R_matrix[9];
    CvMat pr_vec;
    CvMat pR_matrix;

    cvInitMatHeader(&pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
    cvInitMatHeader(&pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
    cvRodrigues2(&pr_vec, &pR_matrix,0);

    for(i=0; i<9; i++)
    {
        printf("%f\n",R_matrix[i]);
    }
}

同样，可以去opencv官网：https://docs.opencv.org/3.4.0/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac

Mat rvec1, tvec1;
solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);#求旋转矩阵
Mat rvec2, tvec2;
solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
Mat R1, R2;
Rodrigues(rvec1, R1);#实现旋转矩阵向旋转向量的转换
Rodrigues(rvec2, R2);

三、opencv所需标定结果格式（matlab）

Radial：径向畸变，光学透镜的特性使得成像存在着径向畸变（k1，k2，k3），k3可以有，也可以为0。
Tangen：切向畸变，装配方面的误差，传感器与光学镜头不在同一水平线，可由两个参数P1，P2确定。
这里提一下，畸变参数的排列（K1,K2,P1,P2,K3）（用在opencv中）

Mat cameraMatrixL = (Mat_<double>(3, 3) << 904.0589, 0, 656.6037,
    0, 869.2423, 308.9319,
    0, 0, 1);
//对应matlab里的左相机标定矩阵

Mat distCoeffL = (Mat_<double>(5, 1) << 0.3370, -0.1561, -0.0245, 4.2596e-4, 0.0000);
//对应Matlab所得左i相机畸变参数
        
Mat cameraMatrixR = (Mat_<double>(3, 3) << 903.1359, 0, 659.2396,
    0, 870.4047, 321.2040,
    0, 0, 1);
//对应matlab里的右相机标定矩阵

Mat distCoeffR = (Mat_<double>(5, 1) << 0.3338, -0.0631, -0.0105, -0.0022, 0.00000);
//对应Matlab所得右相机畸变参数

Mat T = (Mat_<double>(3, 1) << -118.9937, 2.5308, -1.8361);//T平移向量
                                                    //对应Matlab所得T参数
Mat rec = (Mat_<double>(3, 1) << XXXXX, XXXXXX,XXXXX);//rec旋转向量，对应matlab om参数，这里需要把旋转矩阵变为旋转向量
Mat R;//R 旋转矩阵

下面opencv的代码可以参照博文：https://blog.csdn.net/xiao__run/article/details/78900652

前言

由于摄像机标定中会使用到旋转矩阵以及旋转向量的知识，所以就整理了一下有关与这一部分基础知识的笔记，并进行详细的数学推导。

旋转矩阵

假设坐标系分别绕着$x$轴旋转$\varphi$角，绕$y$轴旋转$\theta$角，绕$z$轴旋转$\psi$角，这里旋转的角度就是我们常说的pitch, roll, yaw。设任意某点在旋转前的坐标系中的坐标是$(x,y,z)$，旋转后的坐标是$(x^{{}^{\prime }},y^{{}^{\prime }},z^{{}^{\prime }})$。

我们可以很容易地推导出这三种情况下的旋转矩阵：

• 绕X轴旋转φ角（PITCH）：

$$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]$$

• 绕Y轴旋转θ角（ROLL）：

$$R_y=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

• 绕Z轴旋转ψ角（YAW）：

$$R_z=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

详细步骤我就不列出来了，我以前的博客中有详细的推导步骤，虽然使用场景不同，但是概念都是一样的。（四旋翼姿态解算——基础理论及推导)

如果已经上面的三个角度$\varphi$、$\theta$、$\psi$，，我们可以通过将每一步的旋转矩阵相乘得到整个旋转矩阵。

$$R=R_x\ast R_y\ast R_z$$

旋转向量

相机标定中表示旋转的方式，除了使用前面介绍的旋转矩阵之外，还可以使用旋转向量来表示。

我在网上搜索相关资料时，发现大多数都是直接给出了Rodrigue旋转向量的公式，而没有推导过程。如果只是拿来用，那肯定是没问题的，但是不把它的公式推导一遍，心里总是有点不踏实。（好吧，我知道我有点废话）下面我会给出我自己的证明步骤，在最后会给出公式。

---

补充概念：向量的拉格朗日公式

对于任意向量$\vec{a}(a_1,a_2,a_3)$,$\vec{b}(b_1,b_2,b_3)$，$\vec{c}(c_1,c_2,c_3)$，有如下公式成立：

$$\vec{a}\times (\vec{b}\times \vec{c})=(\vec{a}\cdot \vec{c})\vec{b}-(\vec{a}\cdot \vec{b})\vec{c}$$

证明如下：

设$\vec{d}=\vec{b}\times \vec{c}$，且可表示为$\vec{d}(d_1,d_2,d_3)$。

则有：$$\begin{gathered} \vec{d}=\vec{b}\times \vec{c}=\left[\begin{array}{ccc}\vec{i}& \vec{j}& \vec{k}\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right] \\ =\vec{i}(b_2{c}_3-b_3{c}_2)-\vec{j}(b_1{c}_3-b_3{c}_1)+\vec{k}(b_1{c}_2-b_2{c}_1) \end{gathered}$$

对应地就有：$d_1=b_2{c}_3-b_3{c}_2$、$d_2=b_3{c}_1-b_1{c}_3$、$d_3=b_1{c}_2-b_2{c}_1$。

再设$\vec{e}=\vec{a}\times \vec{d}=\vec{a}\times (\vec{b}\times \vec{c})$，且可表示为$\vec{e}(e_1,e_2,e_3)$。

那么：$$\begin{gathered} \vec{e}=\vec{a}\times \vec{d}=\left[\begin{array}{ccc}\vec{i}& \vec{j}& \vec{k}\\ a_1& a_2& a_3\\ d_1& d_2& d_3\end{array}\right] \\ =\vec{i}(a_2{d}_3-a_3{d}_2)-\vec{j}(a_1{d}_3-a_3{d}_1)+\vec{k}(a_1{d}_2-a_2{d}_1) \end{gathered}$$

对应地得到得到：$e_1=a_2{d}_3-a_3{d}_2$、$e_2=a_1{d}_3-a_3{d}_1$、$e_3=a_1{d}_2-a_2{d}_1$

对于$e_1$，代入前面推导出来的$d_2$、$d_3$：

$$\begin{gathered} e_1=a_2{d}_3-a_3{d}_2=a_2(b_1{c}_2-b_2{c}_1)-a_3(b_3{c}_1-b_1{c}_3) \\ =b_1(a_2{c}_2+a_3{c}_3)-c_1(a_3{b}_3+a_2{b}_2) \\ =b_1(\vec{a}\cdot \vec{c}-a_1{c}_1)-c_1(\vec{a}\cdot \vec{b}-a_1{b}_1) \\ =b_1(\vec{a}\cdot \vec{c})-c_1(\vec{a}\cdot \vec{b}) \end{gathered}$$

同理可得：

$$e_2=b_2(\vec{a}\cdot \vec{c})-c_2(\vec{a}\cdot \vec{b})$$

$$e_3=b_3(\vec{a}\cdot \vec{c})-c_3(\vec{a}\cdot \vec{b})$$

把这三个式子合并，表示成向量形式：

$$\vec{e}=\vec{b}(\vec{a}\cdot \vec{c})-\vec{c}(\vec{a}\cdot \vec{b})$$

证明完毕，后面我们还会用到这个公式。

---

上图摘自维基百科。假设我们在任意空间中有任意一个向量$\vec{v}$，$\vec{k}$是某个单位向量。现在，我们将向量$\vec{v}$以单位向量$\vec{k}$为轴，旋转任意角度$\theta$，得到旋转后的向量$\overrightarrow{v_{rot}}$如图中所示。在图中，我们将$\vec{v}$关于$\vec{k}$做正交分解（高中物理），会得到两个新的向量：$\overrightarrow{v_{\perp }}$和$\overrightarrow{v_{\parallel }}$。

很明显：$\overrightarrow{v_{\perp }}$与$\vec{k}$相互垂直，$\overrightarrow{v_{\parallel }}$与$\vec{k}$共线（平行），且还有$\vec{v}=\overrightarrow{v_{\perp }}+\overrightarrow{v_{\parallel }}$。

因为$\overrightarrow{v_{\parallel }}$与$\vec{k}$共线，且$\vec{k}$是一个单位向量，所以：

$$\overrightarrow{v_{\parallel }}=\Vert \overrightarrow{v_{\parallel }}\Vert \cdot \vec{k}=\frac{\vec{v}\cdot \vec{k}}{\Vert \vec{k}\Vert }\cdot \vec{k}=(\vec{v}\cdot \vec{k})\cdot \vec{k}$$

接下来求$\overrightarrow{v_{\perp }}$，由我们前面推导的向量的拉格朗日定理：$\vec{a}\times (\vec{b}\times \vec{c})=(\vec{a}\cdot \vec{c})\vec{b}-(\vec{a}\cdot \vec{b})\vec{c}$,有：

$$\vec{k}\times (\vec{k}\times \vec{v})=(\vec{k}\cdot \vec{v})\cdot \vec{k}-(\vec{k}\cdot \vec{k})\cdot \vec{v}=(\vec{k}\cdot \vec{v})\cdot \vec{k}-\vec{v}$$

然后，我们就可以推出：

$$\overrightarrow{v_{\perp }}=\vec{v}-\overrightarrow{v_{\parallel }}=-\vec{k}\times (\vec{k}\times \vec{v})$$

我们可以根据前面给出的示意图来直观地理解这些式子：$\vec{k}\times \vec{v}$可以看作是$\overrightarrow{v_{\perp }}$绕着$\vec{k}$逆时针旋转90度得到的，即图中的$\vec{w}=\vec{k}\times \vec{v}$；对于$\vec{k}\times (\vec{k}\times \vec{v})=\vec{k}\times \vec{w}$，在图中观察发现，这个式子的结果其实就是$\overrightarrow{v_{\perp }}$绕着$\vec{k}$逆时针旋转180度得到的结果，共线且夹角为180度，正好对应推导出的结果：$\overrightarrow{v_{\perp }}=-\vec{k}\times (\vec{k}\times \vec{v})$。

还是根据这幅图来看，旋转过程中$\overrightarrow{v_{\parallel }}$的大小始终不变，因为它是$\vec{v}$在$\vec{k}$方向上的投影，就在旋转轴上，自然不会变。所以有：$\overrightarrow{v_{rot,\parallel }}=\overrightarrow{v_{\parallel }}$。

接下来看$\overrightarrow{v_{rot,\perp }}$的变化：

首先，可以肯定的是旋转过程中矢量的模长不变，只会改变方向，所以：$\Vert \overrightarrow{v_{rot,\perp }}\Vert =\Vert \overrightarrow{v_{\perp }}\Vert$。

我们可以由$\overrightarrow{v_{\perp }}$和$\vec{k}$方向的投影，组合得到$\overrightarrow{v_{rot,\perp }}$：

$$\overrightarrow{v_{rot,\perp }}=\overrightarrow{v_{\perp }}\cos \theta +\Vert \overrightarrow{v_{\perp }}\Vert \sin \theta \frac{\vec{w}}{\Vert \vec{w}\Vert }$$

前面有：$\vec{w}=\vec{k}\times \vec{v}$可以看作是$\overrightarrow{v_{\perp }}$绕着$\vec{k}$逆时针旋转90度得到的。那么有：$\Vert \overrightarrow{v_{\perp }}\Vert =\Vert \vec{w}\Vert$。

上式可以化简为：

$$\overrightarrow{v_{rot,\perp }}=\overrightarrow{v_{\perp }}\cos \theta +\sin \theta \vec{w}=\overrightarrow{v_{\perp }}\cos \theta +\sin \theta (\vec{k}\times \vec{v})$$

将$\overrightarrow{v_{rot,\perp }}$和$\overrightarrow{v_{rot,\parallel }}$相加，即可得到$\overrightarrow{v_{rot}}$：

$$\begin{array}{rl}\overrightarrow{v_{rot}}& =\overrightarrow{v_{rot,\perp }}+\overrightarrow{v_{rot,\parallel }}\\ & =\overrightarrow{v_{\parallel }}+\overrightarrow{v_{\perp }}\cos \theta +\sin \theta (\vec{k}\times \vec{v})\\ & =\overrightarrow{v_{\parallel }}+(\vec{v}-\overrightarrow{v_{\parallel }})\cos \theta +\sin \theta (\vec{k}\times \vec{v})\\ & =\vec{v}\cos \theta +(1-\cos \theta )\overrightarrow{v_{\parallel }}+\sin \theta (\vec{k}\times \vec{v})\end{array}$$

前面推出来了：$\overrightarrow{v_{\parallel }}=\Vert \overrightarrow{v_{\parallel }}\Vert \cdot \vec{k}=\frac{\vec{v}\cdot \vec{k}}{\Vert \vec{k}\Vert }\cdot \vec{k}=(\vec{v}\cdot \vec{k})\cdot \vec{k}$，直接把那个结论代入这里。

$$\begin{array}{rl}\overrightarrow{v_{rot}}& =\vec{v}\cos \theta +(1-\cos \theta )(\vec{v}\cdot \vec{k})\cdot \vec{k}+\sin \theta (\vec{k}\times \vec{v})\end{array}$$

现在我们已经推导出了旋转向量公示了，我们可以套用这个公式来表示任意旋转。

接下来要进一步整理成矩阵形式：

对于$\vec{k}\times \vec{v}$，用矩阵形式来表示：（无非就是向量叉乘，拆成三个轴上的分量来表示）

$$\left[\begin{array}{c}(\vec{k}\times \vec{v}{)}_x\\ (\vec{k}\times \vec{v}{)}_y\\ (\vec{k}\times \vec{v}{)}_z\end{array}\right]=\left[\begin{array}{c}{k}_y{v}_z-k_z{v}_y\\ k_z{v}_x-k_x{v}_z\\ k_x{v}_y-k_y{v}_x\end{array}\right]=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]\cdot \left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$

使用一个大写的$K$来表示：

$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$

那么有：$K\vec{v}=\vec{k}\times \vec{v}$

还有：$K(K\vec{v})=K^2\vec{v}=K(\vec{k}\times \vec{v})=\vec{k}\times (K\vec{v})=\vec{k}\times (\vec{k}\times \vec{v})$

公式可以进一步化简为：

$$\begin{array}{rl}\overrightarrow{v_{rot}}& =\vec{v}\cos \theta +(1-\cos \theta )(\vec{v}\cdot \vec{k})\cdot \vec{k}+\sin \theta (\vec{k}\times \vec{v})\\ & =\vec{v}\cos \theta +(1-\cos \theta )(\vec{v}+\vec{k}\times (\vec{k}\times \vec{v}))+\sin \theta (\vec{k}\times \vec{v})\\ & =\vec{v}+(1-\cos \theta )K^2\vec{v}+sin\theta K\vec{v}\end{array}$$

给出旋转矩阵$R$有：$\overrightarrow{v_{rot}}=R\vec{v}$

那么消去$\vec{v}$：$R = I + (1-\cos\theta)K^2 + sin\theta K $

Rodrigue旋转向量公式

已知单位向量$\vec{k}=(k_x,k_y,k_z)$，向量$\vec{v}$绕着它旋转$\theta$角。旋转后的向量$\overrightarrow{v_{rot}}$可以通过如下公式求得：

$$\begin{array}{rl}\overrightarrow{v_{rot}}& =\vec{v}\cos \theta +(1-\cos \theta )(\vec{v}\cdot \vec{k})\cdot \vec{k}+\sin \theta (\vec{k}\times \vec{v})\end{array}$$

我们可以把$\vec{v}$提取出来，就得到了：

$$\begin{array}{rl}\overrightarrow{v_{rot}}& =R\vec{v}\\ & =(I\cdot \cos \theta +(1-\cos \theta )\vec{k}\cdot \vec{k}+\sin \theta K)\vec{v}\end{array}$$

其中：

$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$

根据上面这个式子，我们可以给出Rodrigue旋转公式：

对于旋转向量$\vec{r}=(r_x,r_y,r_z)$：

$\theta$为向量$\vec{r}$的模长，而且要保证向量$\vec{r}$是一个单位向量。

反变换也可以很容易求出：

注：在opencv中有相关的实现（Rodrigues2函数）

当然还有一种写法：

$$\overrightarrow{v_{rot}}=R\vec{v}=I\cdot \vec{v}+(1-\cos \theta )K^2\cdot \vec{v}+sin\theta K\cdot \vec{v}$$

对应的旋转矩阵：（我们可以用这个公式来求旋转矩阵）

$$R=I+(1-\cos \theta )K^2+sin\theta K$$

其中：

$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$

参考链接：

1. http://blog.sina.com.cn/s/blog_5fb3f125010100hp.html

2. https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

3. Rodrigues旋转向量维基百科

4. http://blog.csdn.net/tl_tj/article/details/47006007

5. http://wiki.opencv.org.cn/index.php/Cv照相机定标和三维重建#Rodrigues2

   ​