1. 1 问题描述
在已经有了相机模型和已知圆的3个特征点的世界坐标及其像素坐标的情况下，我们可以用数值求解的方式求解出相机的位姿。
假设世界坐标系的圆点为圆心
2. 2 约束条件
已知相机模型
$$
Z_{c}\left[\begin{array}{l}
u \
v \
1
\end{array}\right]=\left[\begin{array}{ccc}
\frac{f}{\mu_x} & 0 & c_{x} \
0 & \frac{f}{\mu_ y} & c_{y} \
0 & 0 & 1
\end{array}\right]\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0
\end{array}\right]\left[\begin{array}{cc}
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}{ccc}
\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}
u \
v \
1
\end{array}\right]=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0
\end{array}\right]\left[\begin{array}{cc}
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{matrix}\frac{\mu}{f} & 0 & - \frac{C_{x} \mu}{f}\0 & \frac{\mu}{f} & - \frac{C_{y} \mu}{f}\0 & 0 & 1\end{matrix}\right]
$$
（4）中的RT矩阵用四元数的表达方式可以写成
$$
\displaystyle \left[\begin{matrix}- 2 y^{2} - 2 z^{2} + 1 & - 2 w z + 2 x y & 2 w y + 2 x z & - T_{x} \left(- 2 y^{2} - 2 z^{2} + 1\right) - T_{y} \left(- 2 w z + 2 x y\right) - T_{z} \left(2 w y + 2 x z\right)\2 w z + 2 x y & - 2 x^{2} - 2 z^{2} + 1 & - 2 w x + 2 y z & - T_{x} \left(2 w z + 2 x y\right) - T_{y} \left(- 2 x^{2} - 2 z^{2} + 1\right) - T_{z} \left(- 2 w x + 2 y z\right)\- 2 w y + 2 x z & 2 w x + 2 y z & - 2 x^{2} - 2 y^{2} + 1 & - T_{x} \left(- 2 w y + 2 x z\right) - T_{y} \left(2 w x + 2 y z\right) - T_{z} \left(- 2 x^{2} - 2 y^{2} + 1\right)\0 & 0 & 0 & 1\end{matrix}\right]
$$
四元数的各个数值约束为
$$
w^2+x^2+y^2+z^2=1
$$
2.1. 2.1 圆心特征点
考虑世界坐标系圆心坐标 $P(0,0,0)$，像素坐标$p_0$
对应缩放
$$
Zc=\displaystyle - T_{x} \left(- 2 w y + 2 x z\right) - T_{y} \left(2 w x + 2 y z\right) - T_{z} \left(- 2 x^{2} - 2 y^{2} + 1\right)
$$
像素坐标
$$
Z_cS^{-1}p_0=\left[\begin{array}{l}
-T_{x}\left(-2 y^{2}-2 z^{2}+1\right)-T_{y}(-2 w z+2 x y)-T_{z}(2 w y+2 x z) \
-T_{x}(2 w z+2 x y)-T_{y}\left(-2 x^{2}-2 z^{2}+1\right)-T_{z}(-2 w x+2 y z)
\end{array}\right]
$$
2.2. 2.2 X方向直径端点
考虑世界坐标系X方向直径端点 $P(L,0,0)$，像素坐标$p_x$
对应缩放
$$
Z_c=\displaystyle L \left(- 2 w y + 2 x z\right) - T_{x} \left(- 2 w y + 2 x z\right) - T_{y} \left(2 w x + 2 y z\right) - T_{z} \left(- 2 x^{2} - 2 y^{2} + 1\right)
$$
像素坐标
$$
Z_cS^{-1}p_x=\left[\begin{array}{c}
L\left(-2 y^{2}-2 z^{2}+1\right)-T_{x}\left(-2 y^{2}-2 z^{2}+1\right)-T_{y}(-2 w z+2 x y)-T_{z}(2 w y+2 x z) \
L(2 w z+2 x y)-T_{x}(2 w z+2 x y)-T_{y}\left(-2 x^{2}-2 z^{2}+1\right)-T_{z}(-2 w x+2 y z)
\end{array}\right.
$$
###2.3 Y方向直径端点
考虑世界坐标系X方向直径端点$P(0,L,0)$，像素坐标$p_y$
对应缩放
$$
Z_c=\displaystyle L \left(2 w x + 2 y z\right) - T_{x} \left(- 2 w y + 2 x z\right) - T_{y} \left(2 w x + 2 y z\right) - T_{z} \left(- 2 x^{2} - 2 y^{2} + 1\right)
$$
像素坐标
$$
Z_cS^{-1}p_y=\left[\begin{array}{c}
L(-2 w z+2 x y)-T_{x}\left(-2 y^{2}-2 z^{2}+1\right)-T_{y}(-2 w z+2 x y)-T_{z}(2 w y+2 x z) \
L\left(-2 x^{2}-2 z^{2}+1\right)-T_{x}(2 w z+2 x y)-T_{y}\left(-2 x^{2}-2 z^{2}+1\right)-T_{z}(-2 w x+2 y z)
\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函数进行数值求解
1 | def func(xx): |
4. 4 实验结果
fsolve的参数包括一个函数和一个初始值，因为是数值求解，所以只能求出一个解，但实际上有多组解（其中只有一组解是真的）
每次计算在2ms左右(个人电脑)
这里设置多个初始值的（x,y），达到近似遍历的效果，在实际可能的范围内将答案求解
可以看到，解中包含了真实答案，但也有许多其他答案，需要后续进行继续分析排除