张氏标定Julia实现

张氏标定利用的是平板标定板，故在没有畸变的情况下，每一张标定板的图像都是一个平面单应。张氏标定法即为通过平面单应中内含的约束线性求解相机内参初值，再通过非线性优化进行完整求解。

线性投影几何

线性的投影几何可以记作：

s [\begin{matrix} u \\ v \\ 1 \end{matrix}] = [\begin{matrix} f_{x} & s & c_{x} & 0 \\ 0 & f_{y} & c_{y} & 0 \\ 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} R & t \\ 0^{T} & 1 \end{matrix}] [\begin{matrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{matrix}]

因为用的是平板标定板，所以世界坐标系的 $Z_{w} = 0$ ，所以上式可以简化为：

s [\begin{matrix} u \\ v \\ 1 \end{matrix}] = K [\begin{matrix} r_{1} & r_{2} & t \end{matrix}] [\begin{matrix} X_{w} \\ Y_{w} \\ 1 \end{matrix}] = [\begin{matrix} h_{1}^{T} \\ h_{2}^{T} \\ h_{3}^{T} \end{matrix}] X

上式为两个平面点集的映射关系，即平面单应，可以通过以下算法求解这一平面单应。这里的 $h_{i}$ 为 $H$ 的各行。

求解平面单应

整理上式可以得到：

h_{1}^{T} X - h_{3}^{T} X u = 0, h_{2}^{T} X - h_{3}^{T} X v = 0

写成矩阵形式有：

{[\begin{matrix} X^{T} & 0^{T} & - u X^{T} \\ 0^{T} & X^{T} & - v X^{T} \end{matrix}]}_{2 \times 9} {[\begin{matrix} h_{1} \\ h_{2} \\ h_{3} \end{matrix}]}_{9 \times 1} = 0

故若有n个标记点，将各个点的约束组装起来即可得到一个超定线性方程组:

A_{2 n \times 9} h_{9 \times 1} = 0

求解这一方程组即可在一缩放因子的意义下得到单应矩阵。
另外，由于图像点和实际标定板的单位不同，为了保证求解的稳定性，往往还需要对数据点进行归一化处理。

求解 $A P = 0$

由于常数因子的存在，可以为 $P$ 增加一个约束 $| | P | | = 1$ 而不影响结果，在此约束下求解齐次线性问题 $A P = 0$ 可退化为求其最小二乘解，即：

min | | A P | | s . t . | | P | | = 1

对 $A$ 进行SVD分解可以将问题转换为：

min | | A P | | = min | | U D V^{T} P | | s . t . | | P | | = 1

其中 $U$ 和 $V$ 均为正交阵，其并不改变向量的模，所以上式可以进一步转换为:

min | | U D V^{T} P | | = min | | D V^{T} P | | s . t . | | V^{T} P | | = 1

记 $Q = V^{T} P$ ，则问题转换为：

min | | D Q | | s . t . | | Q | | = 1

由于 $D$ 为对角矩阵，故其上式的解显而易见为 $D$ 中的最小元素，即 $A$ 最小的奇异值。
此时 $Q = [0, 0, . . ., 1]^{T}$ ， $P = V Q$ ，即为其最小奇异值对应的列向量。

# 归一化数据
function H_Normalize(Pxy)
    m=mean(Pxy[:,1:2],dims=1)                       # 计算均值
    s=std(Pxy[:,1:2],dims=1,corrected=false)        # 计算标准差
    N_p=(Pxy[:,1:2].-m)./s                          # 归一化
    N_p=[N_p ones(size(N_p,1))]                     # 齐次化
    T = [1/s[1] 0 -m[1]/s[1]; 0 1/s[2] -m[2]/s[2]; 0 0 1] # 变换T：Np=T*Pxy
    Inv_T = [s[1] 0 m[1]; 0 s[2] m[2]; 0 0 1]       # 逆变换 IT：Pxy=IT*Np
    return N_p,T,Inv_T
end

# 求解单应矩阵
function solve_H(Obj_p,Img_p)
    I_N,_,Inv_TI=H_Normalize(Img_p[:,1:2])          # 归一化
    O_N,TO,_=H_Normalize(Obj_p[:,1:2])
    Np=size(O_N,1)
    A=zeros(2*Np,9)
    A[1:2:2*Np,1:3].=O_N                            # 组装
    A[2:2:2*Np,4:6].=O_N
    A[1:2:2*Np,7:9].=-O_N.*I_N[:,1]
    A[2:2:2*Np,7:9].=-O_N.*I_N[:,2]
    F=svd(A,full=false)                             # 求解H
    h=F.Vt[end,:]
    h./=h[end]                                      # 令H33=1
    H_N=reshape(h,(3,3))                            # 按列存，reshape后需要转置
    H=Inv_TI * H_N' * TO
end

从单应求解内参矩阵 $K$

单应矩阵 $H$ 求解完成后，其有如下的结构：

K [\begin{matrix} r_{1} & r_{2} & t \end{matrix}] = [\begin{matrix} h_{1} & h_{2} & h_{3} \end{matrix}]

注意这里的 $h_{i}$ 为 $H$ 的各列。
考虑到旋转矩阵的正交性，即 $r_{1}^{T} r_{1} = 1, r_{1}^{T} r_{2} = 0$ ，可以得到两个约束：

h_{1}^{T} K^{- T} K^{- 1} h_{2} = 0, h_{1}^{T} K^{- T} K^{- 1} h_{1} = h_{2}^{T} K^{- T} K^{- 1} h_{2}

这里 $K$ 为一上三角矩阵，记 $B = K^{- T} K^{- 1}$ 则为一对称阵，其只有6个自由度。
记 $b = [B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}]$ ，则对 $B$ 的线性关系可以记作对 $b$ 的线性关系：

h_{i}^{T} B h^{j} = v_{i j}^{T} b

上述的两个约束可以记作：

[\begin{matrix} v_{12}^{T} \\ {(v_{11} - v_{22})}^{T} \end{matrix}] b = 0

每一张标定图像能提供两个约束，则多张标定图可以构成一个齐次线性方程组 $V b = 0$ 。同样地，利用上述的SVD分解法，可以求解得到 $b$ 。 $K$ 中的各个参数可以参考张氏标定法的文献给出，不再赘述。

求解外部参数 $R$ 和 $t$

在得到相机内参矩阵 $K$ 后，可以得到：

[\begin{matrix} r_{1} & r_{2} & t \end{matrix}] = K^{- 1} [\begin{matrix} h_{1} & h_{2} & h_{3} \end{matrix}]

再利用旋转矩阵的正交性，可以得到尺度因子 $λ = 1 / | | K^{- 1} h_{1} | |$ ，即：

r_{1} = λ K^{- 1} h_{1}, r_{2} = λ K^{- 1} h_{2}, t = λ K^{- 1} h_{3}

旋转矩阵可以通过 $r_{1}$ 和 $r_{2}$ 得到：

r_{3} = r_{1} \times r_{2}

另外，这样得到的旋转矩阵 $R$ 并不能保证其正交性，所以为了得到正交阵，可以使用SVD分解，即：

R = U D V^{T}, R^{'} = U V^{T}

此时 $R^{'}$ 为最终的结果，并利用Rodrigues’s Formula变为轴角表示，即仅用3个参数描述旋转，加上3个平移参数一起构成外部参数。

# 利用Rotations.jl
function R2Rv(R)
    RR=AngleAxis(R)
    angle=rotation_angle(RR)
    axis=rotation_axis(RR)
    axis.*angle
end

function calib_linear(Obj_p,Img_lis)
    Img_num=size(Img_lis,1)
    H_lis=zeros((Img_num,3,3))
    for (index,Img_p) in enumerate(Img_lis)
        H_lis[index,:,:]=solve_H(Obj_p,Img_p)    # 求解单应
    end
    V=zeros(Img_num*2,6)
    for i = 1:Img_num
        H=transpose(H_lis[i,:,:])                # 与文章保持一致
        v11=[H[1,1]^2, 2*H[1,2]*H[1,1], H[1,2]^2, 2*H[1,1]*H[1,3], 2*H[1,2]*H[1,3], H[1,3]^2]
        v12=[H[1,1]*H[2,1], H[1,2]*H[2,1]+H[1,1]*H[2,2], H[1,2]*H[2,2], H[1,1]*H[2,3]+H[1,3]*H[2,1], H[1,2]*H[2,3]+H[1,3]*H[2,2], H[1,3]*H[2,3]]
        v22=[H[2,1]^2, 2*H[2,2]*H[2,1], H[2,2]^2, 2*H[2,1]*H[2,3], 2*H[2,2]*H[2,3], H[2,3]^2]
        V[i*2-1,:].=v12                          # 组装V
        V[i*2,:].=v11-v22
    end
    F=svd(V,full=false)
    Bv=F.Vt[end,:]                               # 求解Bv
    Bv./=Bv[end]                                 
    v0=(Bv[2]*Bv[4]-Bv[1]*Bv[5])/(Bv[1]*Bv[3]-Bv[2]*Bv[2])
    lam=Bv[6]-(Bv[4]*Bv[4]+v0*(Bv[2]*Bv[4]-Bv[1]*Bv[5]))/Bv[1]
    fx=sqrt(lam/Bv[1])
    fy=sqrt(lam*Bv[1]/(Bv[1]*Bv[3]-Bv[2]*Bv[2]))
    s=-Bv[2]*fx*fx*fy/lam
    u0=s*v0/fy-Bv[4]*fx*fx/lam
    K=[fx s u0;0 fy v0;0 0 1]                    # 重构K
    RT_lis=zeros(6,Img_num)
    for i=1:Img_num
        H=H_lis[i,:,:]
        r12t = K\H
        r12t ./= norm(r12t[:,1])
        R=[r12t[:,1:2] cross(r12t[:,1],r12t[:,2])]
        RT_lis[1:3,i] .= R2Rv(R)
        RT_lis[4:6,i] .= r12t[:,3]
    end
    camera_p = [fx,fy,s,u0,v0,0,0]               # 2个0为畸变参数
    pose_p = vec(RT_lis)
    [camera_p;pose_p]
end

非线性优化

经过线性预估得到线性的标定初值后，可以利用非线性优化对畸变和变形参数进行进一步优化。这里利用了LsqFit.jl 来进行优化求解。相机的投影可以用如下函数来描述：

function project(obj_p,camera_p,pose_p)
    # x 为obj_p
    R=RotationVec(pose_p[1],pose_p[2],pose_p[3])      # 构建旋转矩阵
    points_proj = obj_p*R' .+ pose_p[4:6]'            # 点为行向量，需要转置R
    points_proj = points_proj[:,1:2]./points_proj[:,3:3] #齐次坐标

    fx,fy = camera_p[1:2]
    s = camera_p[3]
    u0,v0 = camera_p[4:6]
    k1,k2 = camera_p[6:7]

    n = sum(points_proj.^2,dims=2)                    # 添加畸变
    r = 1 .+ k1.*n + k2.* n.^2
    points_proj .*= r
    res = points_proj .* [fx fy] .+ [u0 v0]
    res[:,1] += points_proj[:,2] .* s 
    res
end

使用LsqFit.jl需要构建一个model，其仅包含 $x$ 和参数 $p$ ，去拟合目标数据 $y$ 。其中， $x$ 为标定板上的点， $p$ 为相机内外参， $y$ 为各图像点，且 $x, y, p$ 均为列向量。

function proj_model(x,p)
    camera_p=p[1:7]                          
    pose_p=reshape(p[8:end],6,:)
    pose_num=size(pose_p,2)
    res=zeros(pose_num*size(x,1),2)
    for i=1:pose_num
        res[1+(i-1)*size(x,1):i*size(x,1),:] .= project(x,camera_p,pose_p[:,i])
    end
    vec(res)
end

前述的线性标定函数返回了参数向量的初值，故也将其放到非线性标定中：

function calib_nonlinear(Obj_p,Img_lis,p0)
    y_data = vec(vcat(Img_lis...)[:,1:2])             # 目标函数列向量y
    curve_fit(proj_model,Obj_p,y_data,p0; autodiff=:finiteforward) # 调用LsqFit.jl
end

测试

数据为OpenCV的sample中的图像，已经预先提取好了标记点，利用上述的代码求解。参考值为：

#=
RMS: 0.19643790890334015
camera matrix:
 [[532.79536562   0.         342.45825163]
 [  0.         532.91928338 233.90060514]
 [  0.           0.           1.        ]]
distortion coefficients: [-2.81086258e-01  2.72581010e-02  1.21665908e-03 -1.34204274e-04 1.58514023e-01]
=#
# 数据准备，obj_p为54*3的矩阵，img_p为13*54*2的矩阵
obj_p=readdlm("TEST_obj_54_3.txt")
obj_p_num=size(obj_p,1)
img_p=readdlm("TEST_img_13_54_2.txt")
img_lis=[[img_p[1+(i-1)*obj_p_num:i*obj_p_num,:] ones(obj_p_num)] for i in 1:div(size(img_p,1),obj_p_num)]

p0=calib_linear(obj_p,img_lis)           # 线性求解
fit=calib_nonlinear(obj_p,img_lis,p0)    # 非线性求解
camera_p = fit.param[1:7]                # 相机内参
RMSE=sqrt(mean(fit.resid.^2))            # RMSE

求解结果如下，与参考值一致：

julia> @show camera_p;
camera_p = [533.544343906219, 533.9019017683516, 0.3925686228795032, 342.75451876656433, 233.3269485935073, -0.2912512982761789, 0.10544518845394096]

julia> @show RMSE;
RMSE = 0.1440068856754

线性投影几何

求解平面单应

求解AP=0

从单应求解内参矩阵K

求解外部参数R和t

非线性优化

测试

求解 $A P = 0$

从单应求解内参矩阵 $K$

求解外部参数 $R$ 和 $t$