前端能给出短时间内的轨迹和地图，但由于不可避免的误差积累，这个地图在长时间内往往不正确。于是需要构建一个规模更大的优化问题，以考虑长时间内的最有轨迹和地图。

KF与EKF

KF：应用于线性系统

假设：k时刻的状态只与k-1时刻有关(马尔可夫性) 

关于KF的推导与理解：
英文版：https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
中文版：https://zhuanlan.zhihu.com/p/39912633 

总结步骤：

 其中：

 EKF：应用于非线性系统

思路是用一阶泰勒展开使之线性化，再使用KF

 BA与图优化

 

 

 由于H的稀疏性：

 

鲁棒核函数：使结果更加稳健

 代码实现：

 这里会用到BAL数据集，

 1. 计算误差：

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] : 旋转向量
    // [3-5] : 平移
    // [6-8] : 相机内参, [6] 焦距, [7-8] 畸变参数
    // 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);//畸变公式：1+k1*r^2+k2*r^4

        const T &focal = camera[6];
        predictions[0] = focal * distortion * xp;
        predictions[1] = focal * distortion * yp;//u'=uc*(1+k1*r^2+k2*r^4),u=f*u'

        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

 2. 

void SolveBA(BALProblem &bal_problem) {
    const int point_block_size = bal_problem.point_block_size();              //point数据的字节数
    const int camera_block_size = bal_problem.camera_block_size();            //pose数据的字节数
    double *points = bal_problem.mutable_points();                            //存储路标点point的首地址
    double *cameras = bal_problem.mutable_cameras();                          //存储相机位姿pose的首地址

    // Observations is 2 * num_observations long array observations    
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
    // and y position of the observation.
    const double *observations = bal_problem.observations();                   //存储观测值的首地址
    ceres::Problem problem;                                                    //构建ceres的最小二乘问题

    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        ceres::CostFunction *cost_function;                                     //代价函数   

        // Each Residual block takes a point and a camera as input
        // and outputs a 2 dimensional Residual
        cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
        /*
           在代价函数的计算模型中实现  用于添加误差项时的第一个参数 实现自动求导
        */
        // If enabled use Huber's loss function.
        ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);                     //鲁棒核函数

        //得到 与观测值匹配的相机位姿和观测点3d点
        double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];     
        double *point = points + point_block_size * bal_problem.point_index()[i];

        problem.AddResidualBlock(cost_function, loss_function, camera, point);         //添加误差项
        //参数 ：： 代价函数的对象  ， 核函数（不使用时nullptr） ，待估计的参数（可多个）
    }

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;

    std::cout << "Solving ceres BA ... " << endl;
    //配置ceres求解器
    ceres::Solver::Options options;                                               
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;    //采用schur消元求解增量方程
    options.minimizer_progress_to_stdout = true;                           //输出到cout
    ceres::Solver::Summary summary;                                        //优化信息
    ceres::Solve(options, &problem, &summary);                             //开始优化（参数：配置器 ，最小二乘问题。优化器）
    std::cout << summary.FullReport() << "\n";                             //输出结果
}

 结果：

 

                                 优化前                                                               优化后

课后习题

第一题

第二题：可能是计算的范数不同

第三题：感觉有点奇怪，书中256页说到Ceres中无法控制哪部分点云进行Schur消元，因为Ceres进行Schur消元是自动寻找的。

第四题：来自知乎@Philip.cjh

 第五题：

g2o实现了多个核函数,如Huber核、Cauchy核等。Ceres中的Loss function有多种核函数,如Huber、Cauchy等。其主要思路也是计算核函数的值以及其一阶和二阶导数,这是与g2o类似的地方。

第六题：

有时间再实现。