原理.md

Tsai

Hall与Faugeras方法都是不考虑任何畸变的情况，进行相机内外参的求解，是直接线性分解方式。还有一种是只考虑径向畸变的方法为Tsai，要注意的是这个方法只考虑径向畸变，并且主点(光轴的中心)是默认在图像的正中心，这是Tsai方法的一个重要前提。

在之前不考虑畸变的时候，空间点$(X_w,Y_w,Z_w)$在相机坐标系下的成像点$P_d^C$与无畸变点$P_u^C$是一个点，因为不考虑畸变。在添加上畸变后，有：

$$ X_u^C=X_d^C+\delta _x,Y_u^C=Y_d^C+\delta _y $$

径向畸变是值在成像中心向外或者向内发生的畸变，如果一个成像点在图像坐标系下，与光轴中心的距离为$r=\sqrt{(X_d^{C})^2+(Y_d^{C})^2}$，那么:

$$ \delta_x = X_d^C(k_1r^2+k_2r^4+k_3t^6+......),\delta_y = Y_d^C(k_1r^2+k_2r^4+k_3t^6+......) $$

理论上，阶数越高，表达得越准确，但是一般来讲，使用一阶已经可以表示大多数相机的径向畸变，也就是$k_1\neq 0, k_i=0(i>1)$。

那么

$$ X_u^C=X_d^C(1+k_1r^2),Y_u^C=Y_d^C(1+k_1r^2) $$

在上图中，点$P_w$是3D点，通过射线投影在成像面上的点为$P_u$，畸变后在成像平面上的点为$P_d$，由于只考虑了进行畸变，所以$O_R$,$P_d$和$P_u$三个点是在同一条直线上的。首先我们还是考虑没有畸变的情况，也就是$k_1=0$,$P_u$和$P_d$是一个点。在相机坐标系下观察，可以认为$\vec{P_{oz}P_w}$与$\vec{O_RP_u}$是平行的，所以$\vec{P_{oz}P_w}\times\vec{O_RP_u}=0$。那么$x_u^cY_c-y_u^cX_c=0$。

其中，$x_u^c,y_u^c$分别表示成像点在相机坐标系下的位置坐标，$X_c,Y_c$表示三维标记点在相机坐标系下的位置坐标。主点就在图像的中心，那么有

$$ \begin{cases}

x_u^c=(x_i-u_0)*d_x\

y_u^c=(y_i-v_0)*d_y\

X_c=r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x\

Y_c=r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y

\end{cases}\tag{Tsai} $$

假设$\alpha=\frac{k_x}{k_y}=\frac{f/d_x}{f/d_y}=\frac{d_y}{d_x}$。那么:

$$ (x_i-u_0)(r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y) = (y_i-v_0)\alpha(r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x) $$

将$x_i-u_0$记为$x_i'$, $y_i-u_0$记为$y_i'$。将上式整理成为$\vec{A}\vec{X}=\vec{B}$的形式，其中

$$ \begin{cases}\vec{A}=[x_i'X_w,x_i'Y_w,x_i'Z_w,x_i',-y_i'X_W,-y_i'Y_W,-y_i'Z_W,-y_i']^T \

\vec{X}=[r_{21},r_{22},r_{23},T_y,\alpha r_{11},\alpha r_{12},\alpha r_{13},\alpha T_x]\

\vec{B}=0

\end{cases} $$

由于$\vec{B}=0$是齐次方程，所以对$A$进行SVD分解，得到的Vt的最后一行向量即为$X$的解。这里，应该也可以整理成为一个非齐次的形式，然后使用$X=(A^TA)^{-1}A^TB$来求解。

$$ \begin{cases}

\vec{A}=[x_i'X_w,x_i'Y_w,x_i'Z_w,-y_i'X_W,-y_i'Y_W,-y_i'Z_W,-y_i']^T\\

\vec{X}=[\frac{r_{21}}{T_y},\frac{r_{22}}{T_y},\frac{r_{23}}{T_y},\alpha \frac{r_{11}}{T_y},\alpha \frac{r_{12}}{T_y},\alpha \frac{r_{13}}{T_y},\alpha \frac{T_x}{T_y}]\\

\vec{B}=-x_i'

\end{cases} $$

通常认为$u_0$和$v_0$就是像素坐标系的中心点坐标。这里的矩阵方程有8个未知数，一个点对可以生成一个方程，所以至少需要八个点才能求解。

当得到X后，需要通过$v=[v_1,v_2,v_3]=[r_{21},r_{22},r_{23}]$的模长为1对$X$进行处理，得到的$X$才是正式的解。但是由于$[r_{21},r_{22},r_{23}]$的模长为1，只能归一化$X$的值，而没有符号。

假定有一个点$P_w(X_w,Y_w,Z_w)$与投影点$P_d(x_i,y_i)$，都在相机坐标系下观察。由于相机坐标系的原点在投影面中心的法向上，所以在相机坐标系下观察的话，$X_c$与$x_i$的符号是相同的，$Y_c$与$y_i$的符号也是相同的，即可确认$X$解的最终形式。

当求解到这一步的时候，还有$T_z$和$k_x,k_y$没有求解出来，由于$\alpha=\frac{k_x}{k_y}$，所以$k_x,k_y$只需要求出一个就可以了。

通过$(4)$可以得到：

$$ x_i-u_0 = k_x\frac{r_{11}X^w_i + r_{12}Y^w_i + r_{13}Z^w_i + T_x}{r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i + T_z}\

y_i-u_0 = k_y\frac{r_{21}X^w_i + r_{22}Y^w_i + r_{23}Z^w_i + T_y}{r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i + T_z} $$

同样，将$x_i-u_0$记为$x_i'$, $y_i-u_0$记为$y_i'$。单取第一行，整理成为：

$$ \begin{bmatrix}

r_{11}X^w_i + r_{12}Y^w_i + r_{13}Z^w_i + T_x & -x_i'

\end{bmatrix}

\begin{bmatrix}

k_y\T_z

\end{bmatrix}=x_i'(r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i) $$

或者取第二行，整理成为：

$$ \begin{bmatrix}

r_{21}X^w_i + r_{22}Y^w_i + r_{23}Z^w_i + T_y & -y_i'

\end{bmatrix}

\begin{bmatrix}

k_y\T_z

\end{bmatrix}=y_i'(r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i) $$

将所有的点坐标带入，构建超定方程进行最小二乘求解，得到$k_y,T_z$，通过$\alpha$得到$k_y$。

如下为进行相机校准的C++代码，其中的 m_ImageSize为图像的大小。

bool Tsai(std::vector<cv::Vec3f>& markers3DPoints, std::vector<cv::Vec2f>& image2DPoints)

{

    if (markers3DPoints.size() != image2DPoints.size())

    {

        return false;

    }

    int size = markers3DPoints.size();

    // compute r11,r12,r13,r21,r22,r23,Tx,Ty

    double offset_x = (m_ImageSize[0]-1.0) / 2.0;

    double offset_y = (m_ImageSize[1]-1.0) / 2.0;

    cv::Mat A = cv::Mat_<double>(size, 8);

    for (size_t i = 0; i < size; i++)

    {

        A.at<double>(i, 0) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][0];

        A.at<double>(i, 1) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][1];

        A.at<double>(i, 2) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][2];

        A.at<double>(i, 3) = (image2DPoints[i][0] - offset_x);

        A.at<double>(i, 4) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][0];

        A.at<double>(i, 5) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][1];

        A.at<double>(i, 6) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][2];

        A.at<double>(i, 7) = -(image2DPoints[i][1] - offset_y);

    }

    cv::Mat matD, matU, matVt;

    cv::SVD::compute(A, matD, matU, matVt);

    // normalize length and sign of V

    cv::Mat V = matVt.rowRange(7, 8);

    double gamma_abs = cv::norm(V.colRange(0, 3));

    std::cout << "gamma_abs = " << gamma_abs << std::endl;

    V = V / gamma_abs;

    double sign = (image2DPoints[0][1] - offset_y) * (

        V.at<double>(0, 0) * markers3DPoints[0][0] +

        V.at<double>(0, 1) * markers3DPoints[0][1] +

        V.at<double>(0, 2) * markers3DPoints[0][2] +

        V.at<double>(0, 3)

        );

    if (sign<0)

    {

        V = -V;

    }

    // compute alpha, Tx,Ty, R

    double alpha = cv::norm(V.colRange(4, 7));

    cv::Mat r2 = V.colRange(0, 3);

    cv::Mat r1 = V.colRange(4, 7) / alpha;

    cv::Mat r3 = r1.cross(r2);

    cv::Mat R;

    cv::vconcat(std::vector<cv::Mat>{r1, r2, r3}, R);

    double Ty = V.at<double>(0, 3);

    double Tx = V.at<double>(0, 7) / alpha;

    // compute fx,fy,Tz

    cv::Mat AA = cv::Mat_<double>(size, 2);

    cv::Mat B = cv::Mat_<double>(size, 1);

    for (size_t i = 0; i < size; i++)

    {

        cv::Mat matp3d = (cv::Mat_<double>(1, 3) << markers3DPoints[i][0],

            markers3DPoints[i][1], markers3DPoints[i][2]);

        double xi = image2DPoints[i][0] - offset_x;

        AA.at<double>(i, 0) = r1.dot(matp3d) + Tx;

        AA.at<double>(i, 1) = -xi;


        B.at<double>(i, 0) = xi * r3.dot(matp3d);

    }

    cv::Mat matKxTz = (AA.t() * AA).inv() * AA.t() * B;

    double kx = matKxTz.at<double>(0, 0);

    double Tz = matKxTz.at<double>(1, 0);

    double ky = kx / alpha;

    /*********Show the result**********/

    cv::Mat T = (cv::Mat_<double>(3, 1) << Tx, Ty, Tz);

    std::cout << "kx = " << kx << std::endl <<

        "ky = " << ky << std::endl <<

        "R = " << R << std::endl <<

        "T = " << T.t() << std::endl;

    std::cout << "camera pose = " << -R.t() * T << std::endl;

    return true;

}

然后考虑有畸变的情况，也就是畸变系数$k_1\neq 0$,那么$(Tsai)$这个方程组不能这么写了，由于$(X_c,Y_c)$是空间坐标点$(X_w,Y_w,Z_w)$通过选择平移得到的，所以$(X_c,Y_c)$是没有任何畸变的，但是图像中检测得到的像素坐标点$(x_i,y_i)$是包含畸变的，$(x_i-u_0)*d_x$只能是$x_d^c$，而不是$x_u^c$，但是前面已经知道了$x_u^c = x_d^c(1+k_1r^2)$，畸变与非畸变的坐标存在一个系数关系$1+k_1r^2$，所以在图像的坐标上添加系数关系即可，虽然$r$中的值收到了$d_x,d_y$的影响，但是在计算中可以将$r$简化为$r^2=x_i'^2+y_i'^2$来计算。

所以$Tsai$可写成：

$$ \begin{cases}

x_d^c=(x_i-u_0)*d_x\

y_d^c=(y_i-v_0)*d_y\

r^2 = (x_d^c)^2+(y_d^c)^2\

x_u^c = x_d^c(1+k_1r^2)\

y_u^c = y_d^c(1+k_1r^2)\

X_c=r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x\

Y_c=r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y

\end{cases}\tag{Tsai with distortion} $$

简化迭代k1

通过迭代的方式计算$k_1$以及其他的相机参数：

1. 首先使用靠近中心的点通过忽略畸变($k_1=0$)的情况计算一次参数
2. 然后使用远离中心的点进行投影计算投影点
3. 使用投影点与图像上的点的距离信息估计参数$k_1$
4. 将参数$k_1$带入，重新计算所有点的有畸变的点坐标
5. 重复上面步骤，直到$k_1$收敛到稳定值

可以使用busyyang/tsai-calibration方法中的方式进行上述整个过程。

由于

$$ x_u^c = x_d^c(1+k_1r^2)\

y_u^c = y_d^c(1+k_1r^2) $$

关系的存在，当第一次忽略畸变计算出来内外参后，可以通过3D点的投影得到没有畸变的点$P_u^c(x_u^c,y_u^c)$，以及存在畸变在图像上检测得到的点$P_d^c(x_d^c,y_d^c)$，点坐标都表示是在相机坐标系下。

对上式进行平方相加：

$$ (x_u^c)^2+(y_u^c)^2 = [(x_d^c)^2+(y_d^c)^2][(1+k_1r^2)]^2 $$

令$u2=(x_u^c)^2+(y_u^c)^2$, $d2=(x_d^c)^2+(y_d^c)^2$, 另外我们知道$r^2=(x_d^c)^2+(y_d^c)^2=d2$

所以有：

$$ u2=d2(1+k\times d2)^2\

\sqrt{\frac{u2}{d2}}=1+k\times d2\

k=(1-\sqrt{\frac{u2}{d2}})/d2 $$

由于$u2,d2$分别表示畸变前后的点到图像中心距离的平方，两个数字的符号总是相同的，所以开根号的时候不需要考虑负号的问题。

def estimateKappaP(point):

    """

    x_u = x_d(1+k*r^2)

    y_u = y_d(1+k*r^2) where r^2 = (x_d)^2 + (y_d)^2


    (x_u)^2 + (y_u)^2 = [(x_d)^2 + (y_d)^2] [(1+k*r^2)^2]

    note u2 = (x_u)^2 + (y_u)^2, and d2 = (x_d)^2 + (y_d)^2

    so, u2 = d2[(1+k*d2)^2]

    then, k = (sqrt(u / d) - 1) / d2

    """

    u2 = (point.projectedSensor[0] * point.projectedSensor[0]) + (

            point.projectedSensor[1] * point.projectedSensor[1])

    d2 = (point.sensor[0] * point.sensor[0]) + (point.sensor[1] * point.sensor[1])


    d = math.sqrt(d2)

    u = math.sqrt(u2)

    k = (u / d - 1) / d2

    return k

而当我们已知投影点，想要计算畸变后的点坐标时候，我们已知了$k$,$u2$,需要计算出$d2$才能进行计算得出畸变后的坐标。

$$ u2=d2(1+k\times d2)^2\

x_d^c =\frac{x_u^c}{(1+k_1\times d2)}\

y_d^c =\frac{y_u^c}{(1+k_1\times d2)} $$

然而$d2$没办法直接获得，因为$P_d$的坐标还没有。但是观察可以发现，可以把$u2=d2(1+k\times d2)^2$写成$y=x(1+k\times x)^2$的形式，通过非线性优化的方法进行$d2$的求解，构建$F=||d2(1+k\times d2)^2-u2||^2$，求使得$F$最小化的$d2$: $\argmin{F(d2)}$，对于$d2$的初值，可以设置为$u2$。

import numpy as np

from scipy.optimize import minimize


k = 0.072227403232112464

u2 = 0.97219326705259235


obj = lambda x: (x * ((1 + k * x) ** 2) - u2) ** 2


res = minimize(obj, np.array([u2]), method='Nelder-Mead')

print(res.fun, '\n', res.success, '\n', res.x)

使用这种方式，首先使用中间畸变较小的点进行内外参求取，然后估计畸变系数$k_1$，后续将畸变考虑进去重复计算内外参与$k_1$。一开始的时候，距离中心点远的标记物的投影点与畸变后的点有12pixel的误差，经过多次迭代后投影点加上畸变与检测的得到的点的误差为4.7pixel的样子。

bool CarmPositionAlgo::Tsai(std::vector<cv::Vec3f>& markers3DPoints, std::vector<cv::Vec2f>& image2DPoints,double k1)

{

    if (markers3DPoints.size() != image2DPoints.size())

    {

        return false;

    }

    int size = markers3DPoints.size();

    // compute r11,r12,r13,r21,r22,r23,Tx,Ty

    double offset_x = (m_ImageSize[0] - 1.0) / 2.0;

    double offset_y = (m_ImageSize[1] - 1.0) / 2.0;

    cv::Mat A = cv::Mat_<double>(size, 8);

    for (size_t i = 0; i < size; i++)

    {

        double xi = (image2DPoints[i][0] - offset_x);

        double yi = (image2DPoints[i][1] - offset_y);

        double r2 = xi * m_ImageSpacing[0] * xi * m_ImageSpacing[0] + yi * yi * m_ImageSpacing[1] * m_ImageSpacing[1];

        xi = xi * (1 + k1 * r2);

        yi = yi * (1 + k1 * r2);

        A.at<double>(i, 0) = xi * markers3DPoints[i][0];

        A.at<double>(i, 1) = xi * markers3DPoints[i][1];

        A.at<double>(i, 2) = xi * markers3DPoints[i][2];

        A.at<double>(i, 3) = xi;

        A.at<double>(i, 4) = -yi * markers3DPoints[i][0];

        A.at<double>(i, 5) = -yi * markers3DPoints[i][1];

        A.at<double>(i, 6) = -yi * markers3DPoints[i][2];

        A.at<double>(i, 7) = -yi;

    }

    cv::Mat matD, matU, matVt;

    cv::SVD::compute(A, matD, matU, matVt);

    // normalize length and sign of V

    cv::Mat V = matVt.rowRange(7, 8);

    double gamma_abs = cv::norm(V.colRange(0, 3));

    V = V / gamma_abs;

    double sign = (image2DPoints[0][1] - offset_y) * (

        V.at<double>(0, 0) * markers3DPoints[0][0] +

        V.at<double>(0, 1) * markers3DPoints[0][1] +

        V.at<double>(0, 2) * markers3DPoints[0][2] +

        V.at<double>(0, 3)

        );

    if (sign<0)

    {

        V = -V;

    }

    // compute alpha, Tx,Ty, R

    double alpha = cv::norm(V.colRange(4, 7));

    cv::Mat r2 = V.colRange(0, 3);

    cv::Mat r1 = V.colRange(4, 7) / alpha;

    cv::Mat r3 = r1.cross(r2);

    cv::Mat R;

    cv::vconcat(std::vector<cv::Mat>{r1, r2, r3}, R);

    double Ty = V.at<double>(0, 3);

    double Tx = V.at<double>(0, 7) / alpha;

    // compute fx,fy,Tz

    cv::Mat AA = cv::Mat_<double>(size, 2);

    cv::Mat AAA = cv::Mat_<double>(size, 2);

    cv::Mat B = cv::Mat_<double>(size, 1);

    cv::Mat BB = cv::Mat_<double>(size, 1);

    for (size_t i = 0; i < size; i++)

    {

        cv::Mat matp3d = (cv::Mat_<double>(1, 3) << markers3DPoints[i][0],

            markers3DPoints[i][1], markers3DPoints[i][2]);

        double xi = image2DPoints[i][0] - offset_x;

        double yi = image2DPoints[i][1] - offset_y;

        double rs = xi * m_ImageSpacing[0] * xi * m_ImageSpacing[0] + yi * yi * m_ImageSpacing[1] * m_ImageSpacing[1];

        xi = xi * (1 + k1 * rs);

        yi = yi * (1 + k1 * rs);

        AA.at<double>(i, 0) = r1.dot(matp3d) + Tx;

        AA.at<double>(i, 1) = -xi;

        B.at<double>(i, 0) = xi * r3.dot(matp3d);


        AAA.at<double>(i, 0) = r2.dot(matp3d) + Ty;

        AAA.at<double>(i, 1) = -yi;

        BB.at<double>(i, 0) = yi * r3.dot(matp3d);


    }

    //cv::Mat matKxTz = (AA.t() * AA).inv() * (AA.t() * B);

    //double kx = matKxTz.at<double>(0, 0);

    //double Tz = matKxTz.at<double>(1, 0);

    //double ky = kx / alpha;

    //double f = ky * m_ImageSpacing[1];


    cv::Mat matKyTz = (AAA.t() * AAA).inv() * (AAA.t() * BB);

    double ky = matKyTz.at<double>(0, 0);

    double Tz = matKyTz.at<double>(1, 0);

    double kx = ky * alpha;

    double f = kx * m_ImageSpacing[0];



    /*********Show the result**********/

    cv::Mat T = (cv::Mat_<double>(3, 1) << Tx, Ty, Tz);

    std::cout << "kx = " << kx << std::endl <<

        "ky = " << ky << std::endl <<

        "R = " << R << std::endl <<

        "T = " << T.t() << std::endl;

    cv::Mat camera_pose = -R.t() * T;

    std::cout << "camera pose = " << -R.t() * T << std::endl;



    // TODO: 输出参数还没有完全输出

    m_CarmTransformParameters.f = f;

    m_CarmTransformParameters.kx = kx;

    m_CarmTransformParameters.ky = ky;

    m_CarmTransformParameters.u0 = offset_x;

    m_CarmTransformParameters.v0 = offset_y;

    m_CarmTransformParameters.rotation_matrix = R;

    m_CarmTransformParameters.translation_vector = T;

    m_CarmTransformParameters.camera_pose = camera_pose;

    m_CarmTransformParameters.image_spacing = cv::Vec3f(m_ImageSpacing[0], m_ImageSpacing[1], 1);


    return true;

}


bool CarmPositionAlgo::TsaiWithDistortion(std::vector<cv::Vec3f>& markers3DPointsInner, std::vector<cv::Vec2f>& image2DPointsInner,

    std::vector<cv::Vec3f>& markers3DPointsOutter, std::vector<cv::Vec2f>& image2DPointsOutter)

{

    const int iternum = 10;

  

    for (size_t i = 0; i < iternum; i++)

    {

        CarmTransformParameters param = GetCarmTransformParameters();

        Tsai(markers3DPointsInner, image2DPointsInner, param.k1);

        param = GetCarmTransformParameters();

        std::vector<double> kappa;

        for (size_t j = 0; j < markers3DPointsOutter.size(); j++)

        {

            auto point = markers3DPointsOutter[j];

            auto point_2d = image2DPointsOutter[j];

            cv::Mat p3d = (cv::Mat_<double>(3, 1) << point[0], point[1], point[2]);

            cv::Mat camera2plane = (cv::Mat_<double>(3, 3) << param.f, 0, 0, 0, param.f, 0, 0, 0, 1);

            cv::Mat project_point = camera2plane * (param.rotation_matrix * p3d + param.translation_vector);

            project_point /= project_point.at<double>(2, 0);


            cv::Mat distort_point = (cv::Mat_<double>(2, 1) <<

                (point_2d[0] - param.u0) * param.image_spacing[0],

                (point_2d[1] - param.v0) * param.image_spacing[1]);

            double u2 = std::pow(cv::norm(project_point.rowRange(0, 2)), 2);

            double d2 = std::pow(cv::norm(distort_point), 2);

            double u = std::sqrt(u2);

            double d = std::sqrt(d2);

            double k = (u / d - 1) / d2;

            kappa.push_back(k);

        }

        double k1 = std::accumulate(kappa.begin(), kappa.end(), 0.0) / kappa.size();

        std::cout << "Radial Distortion K1 = " << k1 << std::endl;

        m_CarmTransformParameters.k1 = k1;

    }

    return true;

}

非线性优化方法

在进行完一次不考虑畸变的Tsai方法后，由于$f$,$T_z$的值还不够准确，$k_1$还没有值，需要进一步优化。考虑畸变的时候，有：

$$ x_u^c=x_d^c(1+{\color{Red} k_1} r^2)=\frac{{\color{Red} f} (r_{11}X_w+r_{12}Y_w+r_{12}Z_w+Tx)}{r_{31}X_w+r_{32}Y_w+r_{32}Z_w+{\color{Red} Tz} } $$

其中，红色的元素是需要重新计算的，显然，由于$k_1$与$T_z$有乘积关系，不是一个线性问题，考虑当成一个非线性问题求解。构造：

$$ F=||\frac{{\color{Red} f} (r_{11}X_w+r_{12}Y_w+r_{12}Z_w+Tx)}{r_{31}X_w+r_{32}Y_w+r_{32}Z_w+{\color{Red} Tz} }-x_d^c(1+{\color{Red} k_1} r^2)||^2 $$

那么也就是求使得$F$的值最小的${f,k_1,T_z}$。

使用https://github.com/simonwan1980/Tsai-Camera-Calibration/blob/master/Tsai/Tsai.m 中的方法，在求出内外参后，使用fminsearch方法求$f,Tz,k_1$的准确值，这样只需要求一次内外参。fminsearch的实现可以参考https://github.com/Xtinc/matrix 这个repo中的实现，这个repo中的实现可以进行类似$y=2x_1x_2+x_1^2+x_1(x_2^2+5)^2$的最小化优化问题，由于我们的写出来的优化方程带观测参数，如$d=cx_1x_2+x_1^2+bx_1(x_2^2+a)^2$,不能直接使用Xtinc的方法，可以将所有观测值进行平方求和然后作为优化方程。

Xtinc/matrix实现：

非线性优化方法的数学实现比较复杂，直接使用了Xtinc/matrix实现的库，由于这个只需要头文件，非常轻量级。

#include "matrixs/optimization.hpp"// for non-linear optimization

#include "matrixs/matrixs.hpp"// for non-linear optimization

    ......

    Tsai(markers3DPointsInner, image2DPointsInner, 0); // 初始化畸变系数k1=0计算参数

    CarmTransformParameters param = GetCarmTransformParameters();

    markers3DPointsOutter.insert(markers3DPointsOutter.begin(), markers3DPointsInner.begin(), markers3DPointsInner.end());

    image2DPointsOutter.insert(image2DPointsOutter.begin(), image2DPointsInner.begin(), image2DPointsInner.end());

    std::vector<double> xc, yc; // 所有点的x,y坐标(在相机坐标系下)

    std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(xc),

        [=](auto p) {return (p[0] - param.u0) * param.image_spacing[0]; });

    std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(yc),

        [=](auto p) {return (p[1] - param.v0) * param.image_spacing[1]; });

    double tx = param.translation_vector.at<double>(0, 0);

    double ty = param.translation_vector.at<double>(1, 0);


    auto r1 = param.rotation_matrix.rowRange(0, 1);

    auto r2 = param.rotation_matrix.rowRange(1, 2);

    auto r3 = param.rotation_matrix.rowRange(2, 3);

    std::vector<double> r1p, r2p, r3p;// 预先计算r1,r2,r3与(X_w,Y_w,Z_w)的乘积，方便后续计算

    std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r1p),

        [=](auto p) {return r1.at<double>(0, 0) * p[0] + r1.at<double>(0, 1) * p[1] + r1.at<double>(0, 2) * p[2]; });

    std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r2p),

        [=](auto p) {return r2.at<double>(0, 0) * p[0] + r2.at<double>(0, 1) * p[1] + r2.at<double>(0, 2) * p[2]; });

    std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r3p),

        [=](auto p) {return r3.at<double>(0, 0) * p[0] + r3.at<double>(0, 1) * p[1] + r3.at<double>(0, 2) * p[2]; });


    // 优化目标方程，对所有的观测点，计算F的函数值，求和后返回。

    auto obj = [=](const ppx::MatrixS<3, 1>& x)

    {

        double k1 = x[0];

        double f = x[1];

        double tz = x[2];

        double value = 0, vx = 0, vy = 0;

        for (size_t i = 0; i < markers3DPointsOutter.size(); i++)

        {

            double r = xc[i] * xc[i] + yc[i] * yc[i];

            vx += std::pow(xc[i] * (1 + k1 * r) * (r3p[i] + tz) - f * (r1p[i] + tx), 2);

            vy += std::pow(yc[i] * (1 + k1 * r) * (r3p[i] + tz) - f * (r2p[i] + ty), 2);

        }

        value = vx + vy;


        std::cout << "Value = " << value;

        std::cout << "\tk1 = " << k1;

        std::cout << "\tf = " << f;

        std::cout << "\ttz = " << tz << std::endl;


        return value;

    };

    ppx::MatrixS<3, 1> x0{ 0.0,param.f,param.translation_vector.at<double>(2, 0) };// 使用Tsai的计算结果作为初始参数

    auto res = ppx::fminunc<ppx::Optimization::Powell>(obj, x0);// 进行非线性优化

    std::cout << res.x << std::endl;

    ......

ITK Powell实现：

这个实现的过程，也可以直接使用ITK库，实现可以参考PowellOptimizerTest. 首先需要通过 itk::SingleValuedCostFunction继承一个类出来写目标函数，类中的 GetValue函数中就写具体的实现，由于在计算中还需要使用到其他的信息，通过Set函数进行设置。由于打算使用Powell优化器，所以不需要实现 GetDerivative里面的内容，如果是使用梯度相关的优化器，还是需要实现这个里面的内容的。

#include <vector>

#include <itkSingleValuedCostFunction.h>

#include <opencv2/opencv.hpp>


class NonLinearFK1Tz :public itk::SingleValuedCostFunction

{

public:

    using Self = NonLinearFK1Tz;

    using Superclass = itk::SingleValuedCostFunction;

    using Pointer = itk::SmartPointer<Self>;

    using ConstPointer = itk::SmartPointer<const Self>;

    itkNewMacro(Self);

    itkTypeMacro(NonLinearFK1Tz, SingleValuedCostFunction);


    enum

    {

        SpaceDimension = 3

    };


    using ParametersType = Superclass::ParametersType;

    using DerivativeType = Superclass::DerivativeType;

    using MeasureType = Superclass::MeasureType;


    NonLinearFK1Tz() = default;



    void

        GetDerivative(const ParametersType&, DerivativeType&) const override

    {}


    MeasureType

    GetValue(const ParametersType& parameters) const override

    {

        double k1 = parameters[0];

        double f = parameters[1];

        double tz = parameters[2];


        MeasureType measure = 0;

        MeasureType vx = 0, vy = 0;

        for (size_t i = 0; i < m_Points3D.size(); i++)

        {

            double r = m_Xc[i] * m_Xc[i] + m_Yc[i] * m_Yc[i];

            double r1p = m_r1[0] * m_Points3D[i][0] + m_r1[1] * m_Points3D[i][1] + m_r1[2] * m_Points3D[i][2];

            double r2p = m_r2[0] * m_Points3D[i][0] + m_r2[1] * m_Points3D[i][1] + m_r2[2] * m_Points3D[i][2];

            double r3p = m_r3[0] * m_Points3D[i][0] + m_r3[1] * m_Points3D[i][1] + m_r3[2] * m_Points3D[i][2];

            vx += std::pow(m_Xc[i] * (1 + k1 * r) * (r3p + tz) - f * (r1p + m_Tx), 2);

            vy += std::pow(m_Yc[i] * (1 + k1 * r) * (r3p + tz) - f * (r2p + m_Ty), 2);

        }

        measure = vx + vy;


        std::cout << "Value = " << measure;

        std::cout << "\tk1 = " << k1;

        std::cout << "\tf = " << f;

        std::cout << "\ttz = " << tz << std::endl;


        return measure;

    }


    unsigned int

        GetNumberOfParameters() const override

    {

        return SpaceDimension;

    }


    void SetPoints3D(std::vector<cv::Vec3f> arg) { m_Points3D = arg; }

    void SetR1(std::vector<double> arg) { m_r1 = arg; }

    void SetR2(std::vector<double> arg) { m_r2 = arg; }

    void SetR3(std::vector<double> arg) { m_r3 = arg; }

    void SetXc(std::vector<double> arg) { m_Xc = arg; }

    void SetYc(std::vector<double> arg) { m_Yc = arg; }

    void SetTx(double arg) { m_Tx = arg; }

    void SetTy(double arg) { m_Ty = arg; }


private:

    std::vector<cv::Vec3f> m_Points3D;

    std::vector<cv::Vec2f> m_Points2D;

    std::vector<double> m_r1;

    std::vector<double> m_r2;

    std::vector<double> m_r3;

    std::vector<double> m_Xc;

    std::vector<double> m_Yc;

    double m_Tx;

    double m_Ty;


};

然后将 NonLinearFK1Tz作为CostFunction设置给 itk::PowellOptimizer优化器即可。

#include "OptimizeFK1Tz.h"

#include <itkPowellOptimizer.h>

    ......

    Tsai(markers3DPointsInner, image2DPointsInner, 0);

    CarmTransformParameters param = GetCarmTransformParameters();


    markers3DPointsOutter.insert(markers3DPointsOutter.begin(), markers3DPointsInner.begin(), markers3DPointsInner.end());

    image2DPointsOutter.insert(image2DPointsOutter.begin(), image2DPointsInner.begin(), image2DPointsInner.end());


    using OptimizerType = itk::PowellOptimizer;

    auto itkOptimizer = OptimizerType::New();

    auto costFunction = NonLinearFK1Tz::New();



    std::vector<double> xc, yc;

    std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(xc),

        [=](auto p) {return (p[0] - param.u0) * param.image_spacing[0]; });

    std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(yc),

        [=](auto p) {return (p[1] - param.v0) * param.image_spacing[1]; });

    auto r1 = param.rotation_matrix.rowRange(0, 1);

    auto r2 = param.rotation_matrix.rowRange(1, 2);

    auto r3 = param.rotation_matrix.rowRange(2, 3);



    costFunction->SetPoints3D(markers3DPointsOutter);

    costFunction->SetTx(param.translation_vector.at<double>(0, 0));

    costFunction->SetTy(param.translation_vector.at<double>(1, 0));

    costFunction->SetXc(xc);

    costFunction->SetYc(yc);

    costFunction->SetR1({ r1.at<double>(0,0),r1.at<double>(0,1) ,r1.at<double>(0,2) });

    costFunction->SetR2({ r2.at<double>(0,0),r2.at<double>(0,1) ,r2.at<double>(0,2) });

    costFunction->SetR3({ r3.at<double>(0,0),r3.at<double>(0,1) ,r3.at<double>(0,2) });



    itkOptimizer->SetCostFunction(costFunction);


    using ParameterType = NonLinearFK1Tz::ParametersType;

    const unsigned int spaceDimension = costFunction->GetNumberOfParameters();

    ParameterType initialPosition(spaceDimension);

    initialPosition[0] = 0;

    initialPosition[1] = param.f;

    initialPosition[2] = param.translation_vector.at<double>(2, 0);



    OptimizerType::ScalesType scales(spaceDimension);

    scales[0] = 1000;

    scales[1] = 1;

    scales[2] = 1;

    itkOptimizer->SetScales(scales);



    itkOptimizer->SetMaximize(false);

    itkOptimizer->SetMaximumIteration(100);

    itkOptimizer->SetMaximumLineIteration(5);


    itkOptimizer->SetInitialPosition(initialPosition);

    try

    {

        itkOptimizer->StartOptimization();

    }

    catch (const std::exception&)

    {

        std::cout << "Exception thrown ! " << std::endl;

    }

    ParameterType finalPosition = itkOptimizer->GetCurrentPosition();

    std::cout << "Solution        = (";

    std::cout << finalPosition[0] << ',';

    std::cout << finalPosition[1] << ',';

    std::cout << finalPosition[2] << ')' << std::endl;

    ......

Tsai with LM

这里的$k_1$与前面Tsai论文中不太一样，在Tsai的文章中，

$$ X_u=X_d+\delta_x\

Y_u=Y_d+\delta_y\

\delta_x=X_d(k_1r^2+k_2r^4+...)\

\delta_y=Y_d(k_1r^2+k_2r^4+...) $$

其中$(X_d,Y_d)$是畸变图像坐标。$(X_u,Y_u)$是没有畸变的图像坐标，$r=\sqrt{X_d^2+Y_d^2}$.

而下面的描述则是与OpenCV相同，

$$ X_d=X_u+\delta_x\

Y_d=Y_u+\delta_y\

\delta_x=X_u(k_1r^2+k_2r^4+...)\

\delta_y=Y_u(k_1r^2+k_2r^4+...) $$

其中$r=\sqrt{X_u^2+Y_u^2}$.

思路：

• Step1: 不考虑图像任何畸变的情况下，首先使用Tsai方法对参数进行求解。

• Step2: 优化$d_x,d_y,T_z,u_0,v_0,k_1,k_2$, 由于我们考虑$f$是固定的，内参系数中$k_x=f/d_x,k_y=f/d_y$只能得到$k_x,k_y$, 经测试，我们固定$f$求$d_x,d_y$更加符合Carm的结果，在其他文献中多认为$d_x,d_y$中一个是固定的，优化$f$和$d_x/d_y$的比例值。

  数据点在相机坐标系下的表示可以写为：

  $\begin{bmatrix}

  X_c\Y_c\Z_c

  \end{bmatrix} = R

  \begin{bmatrix}

  X_w\Y_w\Z_w

  \end{bmatrix} +

  \begin{bmatrix}

  T_x\T_y\T_z

  \end{bmatrix}$

  把数据点投影到图像上，由于图像距离为$f$:

  $\begin{cases}

  X_u= \frac{f}{Z_c}X_c=f\frac{R_1X_w+T_x}{R_3Z_w+T_z}\

  Y_u= \frac{f}{Z_c}Y_c=f\frac{R_2Y_w+T_y}{R_3Z_w+T_z}

  \end{cases}$

  在仅考虑径向畸变的情况下，有：

  $\begin{cases}

  X_d=X_u+\delta_x=X_u(1+k_1r^2+k_2r^4+...)\

  Y_d=Y_u+\delta_y=Y_u(1+k_1r^2+k_2r^4+...)\

  \text{where: } r^2=X_u^2+Y_u^2\end{cases}$

  此时，$(X_d,Y_d)$是畸变图像坐标。$(X_u,Y_u)$是没有畸变的图像坐标，均在物理空间下，单位均是 mm. 将存在畸变的$(X_i,Y_i)$转换到图像坐标时：

  $

  \begin{cases}

  X_d = (X_i-u_0)d_x=X_u+\delta_x=X_u(1+k_1r^2+k_2r^4+...)\

  Y_d = (Y_i-v_0)d_y=Y_u+\delta_y=Y_u(1+k_1r^2+k_2r^4+...)

  \end{cases}$

  带入所有中间变量：

  $

  \begin{cases}

  (X_i-{\color{red}{u_0}}){\color{red}{d_x}} = f\frac{R_1X_w+T_x}{R_3Z_w+{\color{red}{T_z}}}(1+{\color{red}{k_1}}r^2+{\color{red}{k_2}}r^4+...)\

  (Y_i-{\color{red}{v_0}}){\color{red}{d_y}} = f\frac{R_2X_w+T_y}{R_3Z_w+{\color{red}{T_z}}}(1+{\color{red}{k_1}}r^2+{\color{red}{k_2}}r^4+...)\

  \text{where: }r^2=X_u^2+Y_u^2=(f\frac{R_1X_w+T_x}{R_3Z_w+{\color{red}{T_z}}})^2+(f\frac{R_2Y_w+T_y}{R_3Z_w+{\color{red}{T_z}}})^2

  \end{cases}$

  红色的符号表示是需要优化的内容，显然这是一个非线性的问题，如果把上式写成$A=B$和$C=D$的形式，就可以写出优化的目标函数为：

  $J=||A-B||^2+||C-D||^2$

  其中，$d_x,d_y,T_z$的初始值使用Tsai方法获得的结果，$u_0,v_0$的初始值使用图像大小的一半即可，$k_1,k_2$的初始值为0。

• Step3: 优化外参与畸变系数：

  外参主要是$R,T$形成的，畸变系数按照OpenCV的写法有：$R,T,k_1{\color{red}(,k_2)},p_1,p_2,s_1{\color{red}(,s_2)},s_3{\color{red}(,s_4)}$，其中红色括号内的可忽略置零。

  $\begin{cases}

  x_u=\frac{R_{11}X_w+R_{12}Y_w+R_{13}Z_w+T_x}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z},y_u=\frac{R_{21}X_w+R_{22}Y_w+R_{23}Z_w+T_y}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z}\

  x^{''}=x_u(1+k_1r^2+k_2r^4)+2p_1x_uy_u+p_2(r^2+2x_u^2)+s_1r^2+s_2r^4\

  y^{''}=y_u(1+k_1r^2+k_2r^4)+p_1(r^2+2y_u^2)+2p_2x_uy_u+s_3r^2+s_4r^4\

  u \rightarrow \frac{f}{d_x}x^{''}+c_x,v \rightarrow \frac{f}{d_y}y^{''}+c_y\

  ......\

  J=\sqrt{||u-(\frac{f}{d_x}x^{''}+c_x)||^2+||v-(\frac{f}{d_y}y^{''}+c_y)||^2}\

  \text{where: } r^2=x_u^2+y_u^2

  \end{cases}$

  对于$R$矩阵，可以将$R$矩阵分解为三个角度，对角度进行优化，然后通过角度与矩阵的相互关系进行转换。

• Step4: 优化内参

  内参主要是$k_x,k_y,u_0,v_0$四个参数，考虑到$k_x = f/d_x, k_y=f/d_y$,对$d_x,d_y,u_0,v_0$四个参数进行迭代优化，使用的公式还是如Step3中一样。

• Step5: 循环迭代

  重复步骤3-4直到收敛到目标。