三维物体追踪笔记(1)-基于边缘的三维物体追踪——理论、公式推导与实现

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

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

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

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

令：x在垂直于轮廓方向且向内的相邻点为xpexpe，平行于轮廓的相邻点为xpaxpa。

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

E=12∑line∈contour∑x∈line(I(x)−I(xpa))2+(I(x)−I(xpe)−256)2E=12∑line∈contour∑x∈line(I(x)−I(xpa))2+(I(x)−I(xpe)−256)2

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

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

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

E=12∑line∈contour∑xi∈line((I(xi)−I(xipe)−256)2E=12∑line∈contour∑xi∈line((I(xi)−I(xipe)−256)2

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

Ei=12(I(x)−I(xppe)+256)2（白色）Ei=12(I(x)−I(xppe)+256)2（白色）

还是Ei=12(I(x)−I(xpe)−256)2（黑色）Ei=12(I(x)−I(xpe)−256)2（黑色），以便让像素差更接近与0.（我们以下假设是黑色）

2. 基于模型求导

2.1 链式分解

令fi=(I(xi)−I(xipe)−256)fi=(I(xi)−I(xipe)−256)，则E=12fTf=12[f1f2…fn]⎡⎣⎢⎢⎢⎢f1f2…fn⎤⎦⎥⎥⎥⎥E=12fTf=12[f1f2…fn][f1f2…fn]

E的最小化问题，转换为对无数个EiEi最小化问题求解。

已知：

fifi对李代数空间上的位姿ϕϕ求导：

∂fi∂ϕ=Ji=∂I(xpe)∂ϕ−∂I(x)∂ϕ∂fi∂ϕ=Ji=∂I(xpe)∂ϕ−∂I(x)∂ϕ

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

J=⎡⎣⎢⎢⎢⎢J1J2…Jn⎤⎦⎥⎥⎥⎥J=[J1J2…Jn]

那么根据高斯牛顿推理可以得到E的梯度

g=JTf=[J1J2…Jn]⎡⎣⎢⎢⎢⎢f1f2…fn⎤⎦⎥⎥⎥⎥=∑(Jifi)g=JTf=[J1J2…Jn][f1f2…fn]=∑(Jifi)

和E对应的海塞矩阵

H=JTJ=[J1J2…Jn]⎡⎣⎢⎢⎢⎢J1J2…Jn⎤⎦⎥⎥⎥⎥=∑JTiJiH=J<
bc30
/mi>TJ=[J1J2…Jn][J1J2…Jn]=∑JiTJi

2.2 子模块计算：

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

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

∂I(x)∂ϕ=∂I∂x∂x∂ϕ∂I(x)∂ϕ=∂I∂x∂x∂ϕ

∂I∂x∂I∂x表征此处的图像梯度，由sobel求导可得；

而∂x∂ϕ∂x∂ϕ表示三维点对李代数的求导：

Jx=∂x∂ϕ=⎡⎣⎢⎢⎢⎢−fxXYZ2−fx(1+Y2Z2) fx(1+X2Z2)fyXYZ2−fxYZfyXZfxZ00fyZ−fxXZ2−fyYZ2⎤⎦⎥⎥⎥⎥Jx=∂x∂ϕ=[−fxXYZ2fx(1+X2Z2)−fxYZfxZ0−fxXZ2−fx(1+Y2Z2)fyXYZ2fyXZ0fyZ−fyYZ2 ]

其中fx,fyfx,fy是内参数。

II. xpexpe处像素值对位姿求导

Jdxpe=∂I(xpe)∂ϕJdxpe=∂I(xpe)∂ϕ

因为xpe=x+dxpexpe=x+dxpe，而II中已经求出∂I(x)∂ϕ∂I(x)∂ϕ，所以只用求xpexpe的上加上对dxpedxpe对θθ的求导结果JdxpeJdxpe。

对于某一条边，设其两端顶点为x1,x2x1,x2，令

Jx1=∂I(x1)∂ϕ$,$Jx2=∂I(x2)∂ϕJx1=∂I(x1)∂ϕ$,$Jx2=∂I(x2)∂ϕ

Jdxpe=∂I(dxpe)∂ϕ=∂I(R2x2(π/2)dx)∂ϕ=∂(R2x2(π/2)dx)x1−x2|x1−x2|)∂θJdxpe=∂I(dxpe)∂ϕ=∂I(R2x2(π/2)dx)∂ϕ=∂(R2x2(π/2)dx)x1−x2|x1−x2|)∂θ

其中，R2×2(−π/2)dx)R2×2(−π/2)dx)是因为xpexpe的变化相当于x的变化旋转90度。

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

我们规定所有点投影下来，所有点先后顺序按照逆时针连接，也就是连线总是：x1−>x2x1−>x2

则当x2x2的斜率大于x1x1时，则令z=x1−x2z=x1−x2

则Jdxpe=R2x2(−π/2)⋅∂Z|Z|∂Z∂(x1−x2)∂θJdxpe=R2x2(−π/2)⋅∂Z|Z|∂Z∂(x1−x2)∂θ

其中，旋转矩阵以及∂(x1−x2)∂θ(=Jx1−Jx2)∂(x1−x2)∂θ(=Jx1−Jx2)容易求得，只需要专心求第二项。

而f=(fx,fy)T=∂Z|Z|f=(fx,fy)T=∂Z|Z|表示一个分布在单位圆上的点。

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

令fxx为fx对Zxfx对Zx的求导，以此类推fxy,fyy（易证fyx = fxy）；同时令n=1(x21+x22+...+x2n)‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾√n=1(x12+x22+...+xn2)

由于Z_x具有共同形式xi(x21+x22+...+x2n‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾√xi(x12+x22+...+xn2：对于这种类型的求导：

fii=1∑x2i‾‾‾‾√+xi(−12)∑x2i‾‾‾√3⋅2xi=n−n3x2ifii=1∑xi2+xi(−12)∑xi23⋅2xi=n−n3xi2

fij=xi(−12)∑x2i‾‾‾√3⋅2xj=−n3∗xi∗xjfij=xi(−12)∑xi23⋅2xj=−n3∗xi∗xj

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

∂Z|Z|∂Z=[n−n3d2x−n3dxdy−n3dxdyn−n3dy2]∂Z|Z|∂Z=[n−n3dx2−n3dxdy−n3dxdyn−n3dy2]

所以：P=R2×2(−π/2)⋅∂Z|Z|∂Z=[0−110]⋅∂Z|Z|∂Z=[−n3dxdyn−n3d2x−(n−n3dy2)n3dxdy]P=R2×2(−π/2)⋅∂Z|Z|∂Z=[01−10]⋅∂Z|Z|∂Z=[−n3dxdy−(n−n3dy2)n−n3dx2n3dxdy]

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

Jdxpe=P⋅(Jx1−Jx2)Jdxpe=P⋅(Jx1−Jx2)

这时就可以得到e对θθ的求导了：

Jxpe=Jx+JdxpeJxpe=Jx+Jdxpe

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

3.1 自己实现优化算法的调参过程

在自己写LM算法进行总结时，发现LM调参对追踪效果其实是有比较大的影响，比如LM的初始λλ值（一般用海塞矩阵的对角元素的最大值ττ来作为初始值λ0λ0），对结果影响比较大；另外一个需要调整的参数是收敛条件，比如当前error和上一个error的差相对于上一个error的比值。

3.2 求导时需要注意的地方

在做优化问题时，我们容易陷入一种误区，如对上边的代价函数求导时，将所有JiJi直接相加，作为总体的JoJo。再利用H=JToJoH=JoTJo得到海塞矩阵。但这儿对ϕϕ求导的不是E，而是e(E,e区别参见上文)，代价函数不是(∑ei)T(∑ei)(∑ei)T(∑ei)，否则的话倒是应该那样求解的，同时结果之前要乘上2⋅∑ei2⋅∑ei。

这类似的形式让人想起了，当f为向量时，使用∂f2∂x=2f∂f∂x∂f2∂x=2f∂f∂x的前提是x是标量。