相机模型求解

1. 1 问题描述

在已经有了相机模型和已知圆的3个特征点的世界坐标及其像素坐标的情况下，我们可以用数值求解的方式求解出相机的位姿。

假设世界坐标系的圆点为圆心

2. 2 约束条件

已知相机模型
$$Z_c\left[\begin{array}{l}uv1\end{array}\right]=\left[\begin{array}{ccccccc}\frac{f}{{\mu }_x}& 0& c_{x}0& \frac{f}{{\mu }_y}& c_{y}0& 0& 1\end{array}\right]\left[\begin{array}{cccccccccc}1& 0& 0& 00& 1& 0& 00& 0& 1& 0\end{array}\right]\left[\begin{array}{ccc}R& T\overrightarrow{0}& 1\end{array}\right]\left[\begin{array}{c}{X}_w{Y}_w{Z}_{w}1\end{array}\right]$$
其中
$$S=\left[\begin{array}{ccccccc}\frac{f}{{\mu }_x}& 0& c_{x}0& \frac{f}{{\mu }_y}& c_{y}0& 0& 1\end{array}\right]$$
是相机的内参矩阵，是一个固定的矩阵，因此可以放到等式左边，化简方程
$$Z_c{S}^{-1}\left[\begin{array}{l}uv1\end{array}\right]=\left[\begin{array}{cccccccccc}1& 0& 0& 00& 1& 0& 00& 0& 1& 0\end{array}\right]\left[\begin{array}{ccc}R& T\overrightarrow{0}& 1\end{array}\right]\left[\begin{array}{c}{X}_w{Y}_w{Z}_{w}1\end{array}\right]$$
其中
$$S^{-1}={\displaystyle \left[\begin{array}{ccccccc}\frac{\mu }{f}& 0& -\frac{C_x\mu }{f}\text{0}& \frac{\mu }{f}& -\frac{C_y\mu }{f}\text{0}& 0& 1\end{array}\right]}$$
（4）中的RT矩阵用四元数的表达方式可以写成
$${\displaystyle \left[\begin{array}{ccccccccccccc}-2y^2-2z^2+1& -2wz+2xy& 2wy+2xz& -T_x(-2y^2-2z^2+1)-T_y(-2wz+2xy)-T_z(2wy+2xz)\text{2}wz+2xy& -2x^2-2z^2+1& -2wx+2yz& -T_x(2wz+2xy)-T_y(-2x^2-2z^2+1)-T_z(-2wx+2yz)\text{-}2wy+2xz& 2wx+2yz& -2x^2-2y^2+1& -T_x(-2wy+2xz)-T_y(2wx+2yz)-T_z(-2x^2-2y^2+1)\text{0}& 0& 0& 1\end{array}\right]}$$
四元数的各个数值约束为
$$w^2+x^2+y^2+z^2=1$$

2.1. 2.1 圆心特征点

考虑世界坐标系圆心坐标 $P(0,0,0)$，像素坐标$p_0$

对应缩放
$$Zc={\displaystyle -T_x(-2wy+2xz)-T_y(2wx+2yz)-T_z(-2x^2-2y^2+1)}$$
像素坐标
$$Z_c{S}^{-1}{p}_0=\left[\begin{array}{l}-T_x(-2y^2-2z^2+1)-T_y(-2wz+2xy)-T_z(2wy+2xz)-T_x(2wz+2xy)-T_y(-2x^2-2z^2+1)-T_z(-2wx+2yz)\end{array}\right]$$

2.2. 2.2 X方向直径端点

考虑世界坐标系X方向直径端点 $P(L,0,0)$，像素坐标$p_x$

对应缩放
$$Z_c={\displaystyle L(-2wy+2xz)-T_x(-2wy+2xz)-T_y(2wx+2yz)-T_z(-2x^2-2y^2+1)}$$
像素坐标
$$Z_c{S}^{-1}{p}_x=[\begin{array}{c}L(-2y^2-2z^2+1)-T_x(-2y^2-2z^2+1)-T_y(-2wz+2xy)-T_z(2wy+2xz)L(2wz+2xy)-T_x(2wz+2xy)-T_y(-2x^2-2z^2+1)-T_z(-2wx+2yz)\end{array}$$

###2.3 Y方向直径端点

考虑世界坐标系X方向直径端点$P(0,L,0)$，像素坐标$p_y$

对应缩放
$$Z_c={\displaystyle L(2wx+2yz)-T_x(-2wy+2xz)-T_y(2wx+2yz)-T_z(-2x^2-2y^2+1)}$$
像素坐标
$$Z_c{S}^{-1}{p}_y=\left[\begin{array}{c}L(-2wz+2xy)-T_x(-2y^2-2z^2+1)-T_y(-2wz+2xy)-T_z(2wy+2xz)L(-2x^2-2z^2+1)-T_x(2wz+2xy)-T_y(-2x^2-2z^2+1)-T_z(-2wx+2yz)\end{array}\right]$$

2.3. 2.4 总结

综上，(6)(8)(10)(12)中共有7个方程

已知的是$L,f,\mu ,p_0,p_x,p_y,C_x,C_y$

未知数$T_x,T_y,T_z,w,x,y,z$

共有7个方程，7个未知数，因此可以求解

3. 3 代码实现

主要是用了scipy中的fsolve函数进行数值求解

def func(xx):
T_x, T_y, T_z,  x, y, z, w = xx
p0x = -T_x*(-2*y**2 - 2*z**2 + 1) - T_y * \
(-2*w*z + 2*x*y) - T_z*(2*w*y + 2*x*z)
p0y = -T_x*(2*w*z + 2*x*y) - T_y*(-2*x**2 -
2*z**2 + 1) - T_z*(-2*w*x + 2*y*z)
Z_p0 = -T_x*(-2*w*y + 2*x*z) - T_y*(2*w*x + 2*y*z) - \
T_z*(-2*x**2 - 2*y**2 + 1)
pxx = -T_x*(-2*y**2 - 2*z**2 + 1) - T_y*(-2*w*z + 2*x*y) - \
T_z*(2*w*y + 2*x*z) - 2*y**2 - 2*z**2 + 1
pxy = -T_x*(2*w*z + 2*x*y) - T_y*(-2*x**2 - 2*z**2 + 1) - \
T_z*(-2*w*x + 2*y*z) + 2*w*z + 2*x*y
Z_px = -T_x*(-2*w*y + 2*x*z) - T_y*(2*w*x + 2*y*z) - \
T_z*(-2*x**2 - 2*y**2 + 1) - 2*w*y + 2*x*z
pyx = -T_x*(-2*y**2 - 2*z**2 + 1) - T_y*(-2*w*z + 2*x*y) - \
T_z*(2*w*y + 2*x*z) - 2*w*z + 2*x*y
pyy = -T_x*(2*w*z + 2*x*y) - T_y*(-2*x**2 - 2*z**2 + 1) - \
T_z*(-2*w*x + 2*y*z) - 2*x**2 - 2*z**2 + 1
Z_py = -T_x*(-2*w*y + 2*x*z) - T_y*(2*w*x + 2*y*z) - \
T_z*(-2*x**2 - 2*y**2 + 1) + 2*w*x + 2*y*z
return [p0x/Z_p0-pixels[0, 0],
p0y/Z_p0-pixels[0, 1],
pxx/Z_px-pixels[1, 0],
pxy/Z_px-pixels[1, 1],
pyx/Z_py-pixels[2, 0],
pyy/Z_py-pixels[2, 1],
w**2+x**2+y**2+z**2-1]

from scipy.optimize import fsolve
guess = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ans = fsolve(func, guess)

4. 4 实验结果

fsolve的参数包括一个函数和一个初始值，因为是数值求解，所以只能求出一个解，但实际上有多组解（其中只有一组解是真的）

每次计算在2ms左右(个人电脑)

这里设置多个初始值的（x,y），达到近似遍历的效果，在实际可能的范围内将答案求解

可以看到，解中包含了真实答案，但也有许多其他答案，需要后续进行继续分析排除