直接法匹配

单层直接法

假设有两个帧，运动位置，但是有初始的估计$R,t$

假设左边这是第一帧，有了投影点$p_1$，按照初始的估计，得到第二帧上的投影为$p_2$。

投影关系为

已知的内容。注意在这里，第一幅图中的点不必是角点，随机的都可以

void DirectPoseEstimationSingleLayer(
        const gazebocv::Mat &img1, // 左边第一帧
        const cv::Mat &img2, // 右边第二帧
        const VecVector2d &px_ref, // 第一帧中的点
        const vector<double> depth_ref,//对应于第一幅图的深度图
        Sophus::SE3d &T21 //第一幅图点的相机坐标系到第二幅图的相机坐标系
)

首先求出在第二幅图中，对应于第一幅图的所有点。步骤

• 把img1的点转换到相机坐标系，我们知道像素坐标和相机坐标的转换公式关系有

  $$\begin{pmatrix} u\\v\\1 \end{pmatrix} =\frac{1}{Z} \begin{pmatrix} f_x & 0 & c_x\\ 0&f_y&c_y\\ 0&0&1 \end{pmatrix} \begin{pmatrix} X\\Y\\Z \end{pmatrix} =\frac{1}{Z}KP$$

  其中，$P$为点在相机坐标系下的坐标，$u,v$为像素坐标

• 利用变换矩阵进行位姿变换到img2

• 把img2的相机坐标系转到像素坐标

for (size_t i = 0; i < px_ref.size(); i++) {
    // 计算作图中的像素对应的相机坐标系
    Eigen::Vector3d point_ref = 
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
    // 计算点在第二图中的相机坐标
    Eigen::Vector3d cur = T21 * point_ref;
    // 如果深度小于0，则不需要
    if (cur[2] < 0)
        continue;
    // 计算第二帧所对应的像素坐标系
    double u = fx * cur[0] / cur[2] + cx, v = fy * cur[1] / cur[2] + cy;
    ...
}

我们依然取一个8x8的窗口进行计算

for (int x = -half_patch_size; x < half_patch_size; x++)
    for (int y = -half_patch_size; y < half_patch_size; y++) {
		...    
    }
}

下面开始计算误差，由于我们依然是基于灰度不变的假设，因此，我们依然可以最小化光度误差

$$e=I_1(p_1)-I_2(p_2)$$

那么对于每一个点来说

double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) - 
    GetPixelValue(img2, u + x, v + y);

这里，$p_1$是在第一帧上的点，$p_2$是第二帧上的点。那么整个相机的位姿估计问题可以写为

$$T^*=\mathop\min_T||e||^2=\mathop\min_T\sum_{i=1}^Ne_i^Te_i$$

其中$e_i=I_1(p_{1,i})-I_2(p_{2,i})$。

下面利用高斯牛顿方法求解此最小化问题，类似于光流法匹配一文中的做法，我们需要先求出导函数$J(x)$。我们可以知道$p_2=KP/Z,P=Tp_1$，$P=（X’,Y’,Z’）$为第二帧的相机坐标系的点，$p_1$为第一帧的相机坐标系的点。所以

$$\frac{\partial e}{\partial T}=-\frac{\partial I_2}{\partial p_2}\frac{\partial p_2}{\partial P}\frac{\partial P}{\partial \delta\phi}$$

参考SLAM十四讲P220，

$$\frac{\partial p_2}{\partial \delta\phi}= \begin{bmatrix} \displaystyle\frac{f_x}{Z'} & 0 & \displaystyle\frac{-f_xX'}{(Z')^2}& \displaystyle\frac{-f_xX'Y'}{(Z')^2}& f_x+\displaystyle\frac{f_x(X')^2}{(Z')^2}& \displaystyle\frac{-f_xY'}{Z'}\\ 0& \displaystyle\frac{f_y}{Z'}& \displaystyle\frac{-f_yY'}{(Z')^2}& -f_y-\displaystyle\frac{f_y(Y')^2}{(Z')^2}& \displaystyle\frac{f_xX'Y'}{(Z')^2}& \displaystyle\frac{f_yX'}{Z'} \end{bmatrix}$$

Matrix26d J_pixel_xi;
double iz2 = 1 / Z / Z;
J_pixel_xi << fx / Z, 0 , -fx * X * iz2, -fx * X * Y * iz2, fx + fx * X * X * iz2, - fx * Y / Z,
	0, fy / Z, -fy * Y * iz2, -fy - fy * Y * Y * iz2, fy * X * Y * iz2, fy * X / Z;

所以

$$\frac{\partial e}{\partial T}=-\frac{\partial I_2}{\partial p_2}\frac{\partial p_2}{\partial \delta\phi}$$

这里的${\partial I_2}/{\partial p_2}$可以通过导数计算的定义得到

Eigen::Vector2d J_img_pixel;
J_img_pixel << 0.5 * (GetPixelValue(img2, u + x + 1, v + y) - GetPixelValue(img2, u + x - 1, v + y)),
	0.5 * (GetPixelValue(img2, u + x, v + y + 1) - GetPixelValue(img2, u + x, v + y - 1));

最后计算$J$，累加每一个$H$和$b$。

多层直接法