本文共 7154 字，大约阅读时间需要 23 分钟。

1. 基于边缘检测的三维跟踪建模

三维物体追踪是已知图像中某个物体在已知一系列空间三维点位置（或者是一个3D 模型面片集）的信息下，将这些点通过一个恰到好处的位姿(R,t)进行转换后投影到图像上。问题的求解目标是这个位姿(R,t)，难点是并不知道这些三维点对应图像上的哪些像素点。

假如已知这个3D模型在图像对应物体具有清晰轮廓，特别是物体与背景之间有较好的灰度值区分度的（比如白色桌子上放一本MVG书），就可以利用基于边缘检测的三维物体跟踪。

令：x是重投影回去的轮廓（以下简称轮廓）上的离散点，x表示二维点。此处为方便起见，先命名x对应的三维点的坐标为$(X,Y,Z)$。

令：x在垂直于轮廓方向且向内的相邻点为$x_{pe}$，平行于轮廓的相邻点为$x_{pa}$。

由于基于边缘检测的三维物体跟踪的最重要的特点是：垂直于边缘方向的梯度大，平行于边缘方向的梯度小，于是有：

$$E=\frac{1}{2}\sum _{line\in contour}\sum _{x\in line}(I(x)-I(x_{pa}){)}^2+(I(x)-I(x_{pe})-256{)}^2$$

其中$|I(x)-I(x_{pa})|$表示梯度。

后者$(256-I(x)-I(x_{pe}){)}^2$让候选点到边缘的梯度(即$I(x)-I(x_{pe})$）尽可能大（那样对应的E才会小）。灰度值最大值一般为256。

由于前者对结果的影响很小，因此我们考虑后者即可：

$$E=\frac{1}{2}\sum _{line\in contour}\sum _{x_i\in line}((I(x_i)-I(x_{i}pe)-256{)}^2$$

**注：**在实验室常用做法中使用的是根据该marker是黑色还是白色而决定使用:

$E_i=\frac{1}{2}(I(x)-I(x_{p_{pe}})+256{)}^2（白色）$

还是$E_i=\frac{1}{2}(I(x)-I(x_{pe})-256{)}^2（黑色）$，以便让像素差更接近与0.（我们以下假设是黑色）

2. 基于模型求导

2.1 链式分解

令$f_i=(I(x_i)-I(x_{i}pe)-256)$，则$$E=\frac{1}{2}{f}^{T}f=\frac{1}{2}\left[\begin{array}{cccc}{f}_1& f_2& \dots & f_n\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ \dots \\ f_n\end{array}\right]$$

那么$f$向量整体对李代数空间上的位姿$\varphi$求导得到该问题的雅克比矩阵：

2.2 子模块计算：

（3）式的这两项的计算分别如下：

I. x处像素值对位姿求导

$$\frac{\partial I(x)}{\partial \varphi }=\frac{\partial I}{\partial x}\frac{\partial x}{\partial \varphi }$$

$\frac{\partial I}{\partial x} $表征此处的图像梯度，由sobel求导可得；

而$\frac{\partial x}{\partial \varphi }$表示三维点对李代数的求导：

$$J_x=\frac{\partial x}{\partial \varphi }=\left[\begin{array}{cccccc}-\frac{f_{x}XY}{Z^2}& f_x(1+\frac{X^2}{Z^2})& -\frac{f_{x}Y}{Z}& \frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ -f_x(1+\frac{Y^2}{Z^2})& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}& 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\\ & & & & & \end{array}\right]$$

其中$f_x,f_y$是内参数。

**II. $x_{pe}$处像素值对位姿求导 **

$J_{dxpe} = \frac{ \partial I(x_{pe})}{\partial \phi} $

因为$x_{pe}=x+d_{x_{pe}}$，而II中已经求出$\frac{\partial I(x)}{\partial \varphi }$，所以只用对$d{x}_{pe}$求关于$\varphi$的求导结果$J_{dxpe}$。

对于某一条边，设其两端顶点为$x_1,x_2$，令

$$J_{dxpe}=\frac{\partial I(d{x}_{pe})}{\partial \varphi }=\frac{\partial I(R_{2x2}(-\pi /2)dx)}{\partial \varphi }=\frac{\partial (R_{2x2}(-\pi /2)dx)\frac{x1-x2}{|x1-x2|})}{\partial \varphi }$$

其中，$R_{2\times 2}(-\pi /2)dx)$是因为$x_{pe}$的变化相当于x的变化旋转90度。

逆时针还是顺时针旋转90度？取决于x点的位置。我们可以通过x与投影到像素平面的几何中心的连线的斜率得到它是顺时针还是逆时针。

我们规定所有点投影下来，所有点先后顺序按照逆时针连接，也就是连线总是：$x_1->x_2$

则当$x_2$的斜率大于$x_1$时，则令$z=x_1-x_2$

则$$J_{dxpe}=R_{2x2}(-\pi /2)\cdot \frac{\partial \frac{Z}{|Z|}}{\partial Z}\frac{\partial (x1-x2)}{\partial \varphi }$$

其中，旋转矩阵以及$$\frac{\partial (x1-x2)}{\partial \varphi }=J_{x1}-J_{x2}$$容易求得，只需要专心求第二项。

而$$f=(f_x,f_y{)}^T=\frac{\partial Z}{|Z|}$$表示一个分布在单位圆上的点。

如果Z为n维，则f表示落在n维“球”面上的点。

令fxx为$f_x对Z_x$的求导，以此类推fxy,fyy（易证fyx = fxy）；同时令$$n=\frac{1}{\sqrt{(x_1^2+x_2^2+...+x_n^2)}}$$

由于Z_x具有共同形式$$\frac{x_i}{\sqrt{(x_1^2+x_2^2+...+x_n^2}}$$：对于这种类型的求导：

f i i = 1 ∑ x i 2 + x i ( − 1 2 ) ∑ x i 2 3 ⋅ 2 x i = n − n 3 x i 2 fii = \frac{1}{\sqrt{\sum_{x_i^2}}}+ x_i (-\frac{1}{2}) \sqrt{

f i j = x i ( − 1 2 ) ∑ x i 2 3 ⋅ 2 x j = − n 3 ∗ x i ∗ x j fij = x_i (-\frac{1}{2}) \sqrt{

所以，对于之前的两维的$Z=[dx,dy{]}^T$，有：

$$\frac{\partial \frac{Z}{|Z|}}{\partial Z}=\left[\begin{array}{cc}n-n^3{d}_x^2& -n^{3}dxdy\\ -n^{3}dxdy& n-n^{3}d{y}^2\end{array}\right]$$

所以：$$P=R_{2\times 2}(-\pi /2)\cdot \frac{\partial \frac{Z}{|Z|}}{\partial Z}=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\cdot \frac{\partial \frac{Z}{|Z|}}{\partial Z}=\left[\begin{array}{cc}-n^{3}dxdy& -(n-n^{3}d{y}^2)\\ n-n^3{d}_x^2& n^{3}dxdy\end{array}\right]$$

而由于前边我们已经解释了：

这时就可以得到e对$\phi $的求导了：

$$J_{xpe}=J_x+J_{dxpe}$$

3. 工程实现需要留意的地方

3.1 实现优化算法

LM调参对追踪效果其实是有比较大的影响，比如LM的初始$\lambda$值（一般用海塞矩阵的对角元素的最大值$\tau $来作为初始值$\lambda_0 $），对结果影响比较大；另外一个需要调整的参数是收敛条件，比如当前error和上一个error的差相对于上一个error的比值。

转载地址：https://blog.csdn.net/Kevin_cc98/article/details/79582906 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！