SVO代码笔记（二）

SVO中FastDetector::detect()中再提取角点后，会使用shiTomasiScore()计算一下这个角点评分，这个得分越高，则特征越优先，那这个得分是怎么得到的呢？下面进行推导.

harris角点

在介绍Shi-Tomasi之前需要了解下harris角点，这里只给出关键的点以便于后面理解.
自相关函数:

c (x, y; Δ x, Δ y) = \sum_{(u, v) \in W (x, y)} w (u, v) (I (u, v) - I (u + Δ x, v + Δ y))^{2}

$c(x,y;\Delta x,\Delta y) = \sum_{(u,v)\in W(x,y)}w(u,v)(I(u,v) – I(u+\Delta x,v+\Delta y))^2$
注意这里的加权函数可以使用均值分布、高斯分布，然后根据泰勒展开，一阶近似，得到

c (x, y; Δ x, Δ y) \approx \sum_{w} (I_{x} (u, v) Δ x + I_{y} (u, v) Δ y)^{2} = [Δ x, Δ y] M (x, y) [\begin{matrix} Δ x \\ Δ y \end{matrix}]

$c(x,y;\Delta x,\Delta y)\approx \sum_w (I_x(u,v)\Delta x+I_y(u,v)\Delta y)^2=[\Delta x,\Delta y]M(x,y)\begin{bmatrix}\Delta x \\ \Delta y\end{bmatrix}$
其中

M (x, y) = \sum_{w} [\begin{matrix} I_{x} (x, y)^{2} & I_{x} (x, y) I_{y} (x, y) \\ I_{x} (x, y) I_{y} (x, y) & I_{y} (x, y)^{2} \end{matrix}] = [\begin{matrix} \sum_{w} I_{x} (x, y)^{2} & \sum_{w} I_{x} (x, y) I_{y} (x, y) \\ \sum_{w} I_{x} (x, y) I_{y} (x, y) & \sum_{w} I_{y} (x, y)^{2} \end{matrix}] = [\begin{matrix} A & C \\ C & B \end{matrix}]

$M(x,y)=\sum_w \begin{bmatrix}I_x(x,y)^2&I_x(x,y)I_y(x,y) \\ I_x(x,y)I_y(x,y)&I_y(x,y)^2\end{bmatrix} = \begin{bmatrix}\sum_w I_x(x,y)^2&\sum_w I_x(x,y)I_y(x,y) \\\sum_w I_x(x,y)I_y(x,y)&\sum_w I_y(x,y)^2\end{bmatrix}=\begin{bmatrix}A&C\\C&B\end{bmatrix}$
注意这里

M (x, y)

$M(x,y)$ 中省略了加权函数，自相关函数可以近似为二项函数：

c (x, y; Δ x, Δ y) \approx A Δ x^{2} + 2 C Δ x Δ y + B Δ y^{2}

$c(x,y;\Delta x,\Delta y)\approx A\Delta x^2+2C\Delta x\Delta y+B\Delta y^2$
二次项函数本质上就是一个椭圆函数（参考wiki: 椭圆的旋转和平移）。椭圆的扁率和尺寸是由

M (x, y)

$M(x,y)$ 的特征值

λ_{1}

$λ_1$ 、

λ_{2}

$λ_2$ 决定的，椭圆的方向是由

M (x, y)

$M(x,y)$ 的特征矢量决定的，为什么是这样，下面是我自己的理解：
因为

M

$M$ 矩阵是实对称矩阵，也就是正规矩阵，所以可以进行对角分解

M = R D R^{T}

$M=RDR^T$
其中

R

$R$ 是酉矩阵（这里是正交矩阵），其每一个行向量为特征值对应的标准化后特征向量，

D

$D$ 的对角线元素就是

M

$M$ 的特征值，接着

x^{T} M (x, y) x = x^{T} R D R^{T} x

$x^TM(x,y)x=x^TRDR^Tx$

R^{T} x

$R^Tx$ 相当于对

x

$x$ 进行了旋转变换（

R

$R$ 相当于 旋转矩阵），即解释了椭圆的方向是由

M (x, y)

$M(x,y)$ 的特征矢量决定.

M (x, y)

$M(x,y)$ 的特征值为

λ_{1}

$λ_1$ 、

λ_{2}

$λ_2$ ，也就是

D

$D$ 的对角线元素，如果把

R^{T} x

$R^Tx$ 看成一个整体上式不就构成一个标准椭圆了吗，

λ_{1}

$λ_1$ 、

λ_{2}

$λ_2$ 的倒数正好对应了长短轴. 如下图所示：
这里写图片描述

椭圆函数特征值与图像中的角点、直线（边缘）和平面之间的关系如下图所示。共可分为三种情况:
这里写图片描述

通过计算一个响应值 $R$ 来判别feature类型，这样做的好处是免去计算特征值麻烦，但是后面的Shi-Tomasi 算法却计算较小的那个特征值

R = d e t M - α (t r a c e M)^{2}

$R=det \boldsymbol{M} - \alpha(trace\boldsymbol{M})^2$
最后通过设定阈值，小于阈值的

R

$R$ 置为0即可.
为什么可以根据

λ_{1}

$λ_1$ 、

λ_{2}

$λ_2$ 对feature的类型进行这样的判断呢？下面进行讨论.

PCA降维

参考：http://blog.codinglabs.org/articles/pca-tutorial.html
PCA本质上是将方差最大的方向作为主要特征，并且在各个正交方向上将数据“离相关”，也就是让它们在不同正交方向上没有相关性.
内积与投影
其中关于内积的一种几何解释比较有意思，A、B的内积如下：

A \cdot B = | A | | B | c o s (a)

$A\cdot B=|A||B|cos(a)$
如果

B

$B$ 的模为1，即让

| B | = 1

$|B|=1$ ，那么就变成了:

A \cdot B = | A | c o s (a)

$A\cdot B=|A|cos(a)$
可以看出，当向量B的模为1，则A与B的内积值等于A向B所在直线投影的矢量长度！这就是说，如果B是一个基，那么A、B的内积就表示A这个向量在 新的基上的坐标，这对后面的理解会很有帮助.

PCA优化的目标
PCA优化的目标是：将一组N维向量降为K维（K大于0，小于N），其目标是选择K个单位（模为1）正交基，使得原始数据变换到这组基上后（借助上一步的推导理解这里想表达的意思），各字段两两间协方差为0（为什么要变成0？因为要减少不同维度的相关性），而字段的方差则尽可能大（为什么？因为要将维度降到更少的维度，就要求，数据在这个维度上要尽可能分散）.

假设我们有a和b两个字段，将它们按行组成矩阵X：

X = (\begin{matrix} a_{1} & a_{2} & \dots & a_{m} \\ b_{1} & b_{2} & \dots & b_{m} \end{matrix})

$X=\begin{pmatrix} a_1 & a_2 & \cdots & a_m \\ b_1 & b_2 & \cdots & b_m \end{pmatrix}$

\frac{1}{m} X X^{T} = (\begin{matrix} \frac{1}{m} \sum_{i = 1}^{m} a_{i}^{2} & \frac{1}{m} \sum_{i = 1}^{m} a_{i} b_{i} \\ \frac{1}{m} \sum_{i = 1}^{m} a_{i} b_{i} & \frac{1}{m} \sum_{i = 1}^{m} b_{i}^{2} \end{matrix})

$\frac{1}{m}XX^\mathsf{T}=\begin{pmatrix} \frac{1}{m}\sum_{i=1}^m{a_i^2} & \frac{1}{m}\sum_{i=1}^m{a_ib_i} \\ \frac{1}{m}\sum_{i=1}^m{a_ib_i} & \frac{1}{m}\sum_{i=1}^m{b_i^2} \end{pmatrix}$
这个矩阵对角线上的两个元素分别是两个字段的 方差，而其它元素是a和b的 协方差.

PCA降维
为了降维，我们要将协方差矩阵对角化，即除对角线外的其它元素化为0，并且在对角线上将元素按大小从上到下排列，这样我们就达到了优化目的. 因为协方差矩阵是实对称矩阵，所以必定可以进行对角化如下：

D = P C P^{T}

$D =PCP^\mathsf{T}$
优化目标变成了寻找一个矩阵

P

$P$ ，满足

P C P^{T}

$PCP^T$ 是一个对角矩阵，并且对角元素按 从大到小依次排列，那么P的 前K行就是要寻找的基，用P的前K行组成的矩阵乘以X就使得X从N维降到了K维并满足上述优化条件.

综上，如果我们对协方差矩阵M进行对角化，很明显，特征值就是主分量上的方差，如果存在两个主分量所对应的特征值都比较大，像素点的梯度分布比较散，梯度变化程度比较大，符合角点在窗口区域的特点；

Shi-Tomasi 算法
Shi-Tomasi 算法是Harris 算法的改进。Harris 算法最原始的定义是将矩阵 M 的行列式值与 M 的迹相减，再将差值同预先给定的阈值进行比较。后来Shi 和Tomasi 提出改进的方法，若两个特征值中较小的一个大于最小阈值，则会得到强角点。
Harris 角点检测的打分公式为：

R = λ_{1} λ_{2} - k (λ_{1} + λ_{2})^{2}

$R = λ_1λ_2 − k (λ_1 + λ_2)^2$
但 Shi-Tomasi 使用的打分函数为：

R = m i n (λ_{1}, λ_{2})

$R = min (λ_1, λ_2)$
如果打分超过阈值，我们就认为它是一个角点.

SVO计算fast得分

绕了一大圈终于回来了，现在再看这个问题已经很简单了，SVO中使用的不是harris，而是fast（更快），SVO在提取fast角点时，为了保证角点分布均匀，进行了栅格化，即在每一个栅格内只提取一个得分最高的角点（这就是所谓的非极大值抑制），这个得分就是使用的shiTomasi计算的，得分越高就表示角点越可靠.
代码最后一行计算是两个特征值中较小的那个，因为这个矩阵是二维的，所以计算也很简单:

\begin{aligned} [\begin{matrix} d x & d x y \\ d x y & d y \end{matrix}] ξ = λ ξ \\ [\begin{matrix} d x - λ & d x y \\ d x y & d y - λ \end{matrix}] ξ = 0 \\ | \begin{matrix} d x - λ & d x y \\ d x y & d y - λ \end{matrix} | = 0 \\ (d x - λ) (d y - λ) - d x y^{2} \\ λ^{2} - (d x + d y) λ + d x d y - d x y^{2} = 0 \\ λ_{1, 2} = \frac{(d x + d y) \pm \sqrt{(d x + d y)^{2} - 4 (d x d y - d x y^{2})}}{2} \end{aligned}

$\begin{equation} \begin{split} &\begin{bmatrix} dx & dxy \\ dxy & dy \end{bmatrix}\xi=\lambda\xi \\ &\\ &\begin{bmatrix} dx-\lambda & dxy \\ dxy & dy-\lambda \end{bmatrix}\xi=0 \\ &\\ &\begin{vmatrix} dx-\lambda & dxy \\ dxy & dy-\lambda \end{vmatrix}=0 \\ &\\ &(dx-\lambda)(dy-\lambda)-dxy^2 \\ &\\ &\lambda^2-(dx+dy)\lambda+dxdy-dxy^2=0 \\ &\\ &\lambda_{1,2}=\frac{(dx+dy)\pm\sqrt{(dx+dy)^2-4(dxdy-dxy^2)}}{2} \end{split} \nonumber \end{equation}$
由上可得，较小的特征值为

\frac{(d x + d y) - \sqrt{(d x + d y)^{2} - 4 (d x d y - d x y^{2})}}{2}

$\frac{(dx+dy)-\sqrt{(dx+dy)^2-4(dxdy-dxy^2)}}{2}$ .

float shiTomasiScore(const cv::Mat& img, int u, int v)
{
  assert(img.type() == CV_8UC1);

  float dXX = 0.0;
  float dYY = 0.0;
  float dXY = 0.0;
  const int halfbox_size = 4;
  const int box_size = 2*halfbox_size;
  const int box_area = box_size*box_size;
  const int x_min = u-halfbox_size;
  const int x_max = u+halfbox_size;
  const int y_min = v-halfbox_size;
  const int y_max = v+halfbox_size;

  if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1)
    return 0.0; // patch is too close to the boundary

  const int stride = img.step.p[0];
  for( int y=y_min; y<y_max; ++y )
  {
    const uint8_t* ptr_left   = img.data + stride*y + x_min - 1;
    const uint8_t* ptr_right  = img.data + stride*y + x_min + 1;
    const uint8_t* ptr_top    = img.data + stride*(y-1) + x_min;
    const uint8_t* ptr_bottom = img.data + stride*(y+1) + x_min;
    for(int x = 0; x < box_size; ++x, ++ptr_left, ++ptr_right, ++ptr_top, ++ptr_bottom)
    {
      float dx = *ptr_right - *ptr_left;
      float dy = *ptr_bottom - *ptr_top;
      dXX += dx*dx;
      dYY += dy*dy;
      dXY += dx*dy;
    }
  }

  // Find and return smaller eigenvalue:
  dXX = dXX / (2.0 * box_area);
  dYY = dYY / (2.0 * box_area);
  dXY = dXY / (2.0 * box_area);
  // 上面的步骤就是harris算法计算一个窗口内的M矩阵

  // 求较小的特征值
  return 0.5 * (dXX + dYY - sqrt( (dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY) ));
}

<完>
@leatherwang