【SLAM】BA和图优化的入门与实践

1.BA与三维投影

我们知道BA优化就是使用三维点进行投影找出最合适的相机位姿（旋转矩阵和平移矩阵），换句话，就是通过迭代调整光束，使其光束满足约束平面，这个迭代也是可以通过最小二乘法实现的。

那我们的转换步骤有哪些呢？

1. 通过旋转矩阵和平移矩阵将三维点从世界坐标系转换为相机坐标系

2. 归一化

3. 进行畸变校正

4. 由相机内参得出相机图像的二维矩阵。

当然，上述步骤所推算出的目标只是推测的二维坐标，我们希望这个推测的二维坐标与真实观测到的二维坐标一致，但是在实际的系统中，这往往是由差异的。通过这种差异我们进行最小化，就可以不断优化如此，这就是BA优化问题。

2.BA计算

一个图像是有很多特征点的，所以要进行ba优化不单单是针对一个特征点二十一批特征点，这就需要对目标函数求和。

我们希望上节介绍的误差最小，也希望当前状态对相机位姿的求导、当前状态对三维点的求导最小。

这里可以使用高斯牛顿法或者列文伯格法，构建我们的单应矩阵H矩阵。

对于H矩阵求逆是一个非常浪费计算量的过程，我们希望简化这过程，怎么简化呢？其实，H矩阵刚好是一个具有特殊结构的矩阵。

3.H矩阵的特殊性

上图我们可以看出h矩阵并不是与所有点都相关，因为对于雅可比矩阵来说，这不是每个项都会为非0值。

> 里面的空缺就是0，原因很显然，第一个相机关于第一个特征点产生的投影误差，不可能受第二个相机影响。上面的J矩阵，每一行例如第四行J14，就是第一个相机位置拍摄的图片中的第四个特征点，根据外参畸变内参计算到像素坐标的时候产生的误差对于相机的位姿和特征点产生的导数。

有块的地方就有关系，这是一种约束。

4.H矩阵特殊性推广

我们再考虑一般的情况，再很多个特征点下，这个矩阵就显得很大，但是很稀疏。

这就要消元或者边缘化了。

对于非0矩阵块，我们称之为共视关系。

5.鲁棒核函数

以上的计算都建立在特征点匹配正确的情况下，对于误匹配的特征点呢，这种错误会导致最小化的时候梯度变得很大。

这时就需要我们的核函数，他会限制梯度，让他不要发生很大的梯度变化，但是还是光滑可导的状态。

6.实践

class SnavelyReprojectionError {
public:
    SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
                                                                           observed_y(observation_y) {}

    template<typename T>
    bool operator()(const T *const camera,
                    const T *const point,
                    T *residuals) const {
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    // camera : 9 dims array
    // [0-2] : angle-axis rotation
    // [3-5] : translateion
    // [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
    // point : 3D location.
    // predictions : 2D predictions with center of the image plane.
    template<typename T>
    static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
        // Rodrigues' formula
        T p[3];
        AngleAxisRotatePoint(camera, point, p);
        // camera[3,4,5] are the translation
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // Compute the center fo distortion
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // Apply second and fourth order radial distortion
        const T &l1 = camera[7];
        const T &l2 = camera[8];

        T r2 = xp * xp + yp * yp;
        T distortion = T(1.0) + r2 * (l1 + l2 * r2);

        const T &focal = camera[6];
        predictions[0] = focal * distortion * xp;
        predictions[1] = focal * distortion * yp;

        return true;
    }

    static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
            new SnavelyReprojectionError(observed_x, observed_y)));
    }

private:
    double observed_x;
    double observed_y;
};

#endif // SnavelyReprojection.h

7.小结

这里，我就讲了几种优化方式，最后总结一下不同的后端方式
1.卡尔曼滤波器：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验；
2.扩展卡尔曼滤波器：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性。
3.BA优化：把一路上的所有坐标点与位姿整体放在一起作为自变量进行非线性优化