视觉SLAM十四讲学习笔记——ch9后端1

9.1理论部分

推荐参考博文推导:

1. 视觉SLAM十四讲学习笔记——第九讲 后端优化（1）
2. slam十四讲-ch9（后端1）-卡尔曼滤波器公式推导及BA优化代码实现【注释】（应该是ch13前面最难的部分了）

9.2实践部分

9.2.1 利用ceres进行BA优化（多个相机和路标点）

代码及详细注释如下:

#include 
#include 
#include "common.h"
#include "SnavelyReprojectionError.h"

using namespace std;
//求解最小二乘的函数
void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]);//读入数据
    bal_problem.Normalize();//进行归一化处理
    bal_problem.Perturb(0.1, 0.5, 0.5);//利用Perturb加入噪声
    bal_problem.WriteToPLYFile("initial.ply");//将优化前的数据（相机和3d点） 保存在initial.ply文件中
    SolveBA(bal_problem);//求解最小二乘问题
    bal_problem.WriteToPLYFile("final.ply");//将优化后的数据（相机和3d点） 保存在final.ply文件中

    return 0;
}
//重点
void SolveBA(BALProblem &bal_problem) {
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
       //注意这里获得待优化系数首地址的时候要用mutable_points()和mutable_cameras()
    // 因为这两个函数指向的地址的内容是允许改变的（优化系数肯定要变的啦）
    double *points = bal_problem.mutable_points();//获得待优化系数3d点 points指向3d点的首地址
    double *cameras = bal_problem.mutable_cameras();//获得待优化系数相机 cameras指向相机的首地址

    // 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();//获得观测数据 observations指向观测数据的首地址
    ceres::Problem problem;
    //要用循环
    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);//核函数

        // Each observation corresponds to a pair of a camera and a point
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
         //bal_Problem.point_index()这返回的是一个地址指向索引号的首地址
        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);
    }
    /*
      cost_function,//代价函数
                loss_function,//核函数
                camera,//待优化的相机
                point//待优化的3d点
     */
    // 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::Solver::Options options;//这里有很多配置选项可以填
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;//消元
    options.minimizer_progress_to_stdout = true;//输出到cout
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}

结果如下:

Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points. 
Forming 83718 observations. 
Solving ceres BA ... 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.842900e+07    0.00e+00    2.04e+06   0.00e+00   0.00e+00  1.00e+04        0    9.82e-02    3.01e-01
   1  1.449093e+06    1.70e+07    1.75e+06   2.16e+03   1.84e+00  3.00e+04        1    2.54e-01    5.56e-01
   2  5.848543e+04    1.39e+06    1.30e+06   1.55e+03   1.87e+00  9.00e+04        1    1.55e-01    7.11e-01
   3  1.581483e+04    4.27e+04    4.98e+05   4.98e+02   1.29e+00  2.70e+05        1    1.56e-01    8.67e-01
   4  1.251823e+04    3.30e+03    4.64e+04   9.96e+01   1.11e+00  8.10e+05        1    1.53e-01    1.02e+00
   5  1.240936e+04    1.09e+02    9.78e+03   1.33e+01   1.42e+00  2.43e+06        1    1.62e-01    1.18e+00
   6  1.237699e+04    3.24e+01    3.91e+03   5.04e+00   1.70e+00  7.29e+06        1    1.65e-01    1.35e+00
   7  1.236187e+04    1.51e+01    1.96e+03   3.40e+00   1.75e+00  2.19e+07        1    1.52e-01    1.50e+00
   8  1.235405e+04    7.82e+00    1.03e+03   2.40e+00   1.76e+00  6.56e+07        1    1.53e-01    1.65e+00
   9  1.234934e+04    4.71e+00    5.04e+02   1.67e+00   1.87e+00  1.97e+08        1    1.49e-01    1.80e+00
  10  1.234610e+04    3.24e+00    4.31e+02   1.15e+00   1.88e+00  5.90e+08        1    1.49e-01    1.95e+00
  11  1.234386e+04    2.24e+00    3.27e+02   8.44e-01   1.90e+00  1.77e+09        1    1.54e-01    2.10e+00
  12  1.234232e+04    1.54e+00    3.44e+02   6.69e-01   1.82e+00  5.31e+09        1    1.57e-01    2.26e+00
  13  1.234126e+04    1.07e+00    2.21e+02   5.45e-01   1.91e+00  1.59e+10        1    1.55e-01    2.42e+00
  14  1.234047e+04    7.90e-01    1.12e+02   4.84e-01   1.87e+00  4.78e+10        1    1.62e-01    2.58e+00
  15  1.233986e+04    6.07e-01    1.02e+02   4.22e-01   1.95e+00  1.43e+11        1    1.57e-01    2.74e+00
  16  1.233934e+04    5.22e-01    1.03e+02   3.82e-01   1.97e+00  4.30e+11        1    1.60e-01    2.90e+00
  17  1.233891e+04    4.25e-01    1.07e+02   3.46e-01   1.93e+00  1.29e+12        1    1.52e-01    3.05e+00
  18  1.233855e+04    3.59e-01    1.04e+02   3.15e-01   1.96e+00  3.87e+12        1    1.53e-01    3.20e+00
  19  1.233825e+04    3.06e-01    9.27e+01   2.88e-01   1.98e+00  1.16e+13        1    1.51e-01    3.35e+00
  20  1.233799e+04    2.61e-01    1.17e+02   2.16e-01   1.97e+00  3.49e+13        1    1.50e-01    3.50e+00
  21  1.233777e+04    2.18e-01    1.22e+02   1.15e-01   1.97e+00  1.05e+14        1    1.47e-01    3.65e+00
  22  1.233760e+04    1.73e-01    1.10e+02   9.32e-02   1.89e+00  3.14e+14        1    1.50e-01    3.80e+00
  23  1.233746e+04    1.37e-01    1.14e+02   1.27e-01   1.98e+00  9.41e+14        1    1.46e-01    3.95e+00
  24  1.233735e+04    1.13e-01    1.17e+02   3.82e-01   1.96e+00  2.82e+15        1    1.53e-01    4.10e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0429 14:34:55.045905 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  25  1.233735e+04    0.00e+00    1.17e+02   0.00e+00   0.00e+00  1.41e+15        1    6.03e-02    4.16e+00
W0429 14:34:55.095809 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  26  1.233735e+04    0.00e+00    1.17e+02   0.00e+00   0.00e+00  3.53e+14        1    4.98e-02    4.21e+00
  27  1.233725e+04    9.50e-02    1.20e+02   2.02e-01   1.99e+00  1.06e+15        1    1.43e-01    4.35e+00
  28  1.233718e+04    6.92e-02    5.84e+01   4.77e-01   1.70e+00  3.18e+15        1    2.16e-01    4.57e+00
W0429 14:34:55.524346 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  29  1.233718e+04    0.00e+00    5.84e+01   0.00e+00   0.00e+00  1.59e+15        1    6.97e-02    4.64e+00
  30  1.233714e+04    3.65e-02    6.13e+01   7.39e-01   1.93e+00  4.77e+15        1    1.50e-01    4.79e+00
W0429 14:34:55.733259 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  31  1.233714e+04    0.00e+00    6.13e+01   0.00e+00   0.00e+00  2.38e+15        1    5.86e-02    4.85e+00
  32  1.233711e+04    3.32e-02    6.05e+01   5.42e-01   2.00e+00  7.15e+15        1    1.45e-01    4.99e+00
W0429 14:34:55.938537 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  33  1.233711e+04    0.00e+00    6.05e+01   0.00e+00   0.00e+00  3.57e+15        1    6.03e-02    5.05e+00
W0429 14:34:55.988092 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  34  1.233711e+04    0.00e+00    6.05e+01   0.00e+00   0.00e+00  8.94e+14        1    4.95e-02    5.10e+00
  35  1.233708e+04    3.14e-02    6.14e+01   2.30e-01   2.00e+00  2.68e+15        1    1.40e-01    5.24e+00
  36  1.233706e+04    2.41e-02    3.85e+02   6.15e+00   1.62e+00  8.04e+15        1    1.50e-01    5.39e+00
W0429 14:34:56.335259 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  37  1.233706e+04    0.00e+00    3.85e+02   0.00e+00   0.00e+00  4.02e+15        1    5.77e-02    5.45e+00
  38  1.233756e+04   -5.04e-01    3.85e+02   3.06e+01  -5.59e+01  1.01e+15        1    7.26e-02    5.52e+00
  39  1.233704e+04    1.68e-02    2.03e+01   3.41e-01   1.86e+00  3.02e+15        1    1.49e-01    5.67e+00
  40  1.234161e+04   -4.57e+00    2.03e+01   5.84e+01  -6.04e+02  1.51e+15        1    8.34e-02    5.75e+00
  41  1.233702e+04    1.51e-02    2.10e+01   2.66e-01   2.00e+00  4.52e+15        1    1.45e-01    5.90e+00
W0429 14:34:56.842923 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  42  1.233702e+04    0.00e+00    2.10e+01   0.00e+00   0.00e+00  2.26e+15        1    5.78e-02    5.96e+00
W0429 14:34:56.895454 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  43  1.233702e+04    0.00e+00    2.10e+01   0.00e+00   0.00e+00  5.65e+14        1    5.25e-02    6.01e+00
  44  1.233701e+04    1.48e-02    2.08e+01   1.18e-01   1.99e+00  1.70e+15        1    1.41e-01    6.15e+00
W0429 14:34:57.093436 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  45  1.233701e+04    0.00e+00    2.08e+01   0.00e+00   0.00e+00  8.48e+14        1    5.71e-02    6.21e+00
  46  1.233700e+04    1.42e-02    2.08e+01   1.47e-01   1.99e+00  2.54e+15        1    1.49e-01    6.35e+00
W0429 14:34:57.300102 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  47  1.233700e+04    0.00e+00    2.08e+01   0.00e+00   0.00e+00  1.27e+15        1    5.78e-02    6.41e+00
  48  1.233698e+04    1.39e-02    2.19e+01   5.90e-01   2.00e+00  3.82e+15        1    1.62e-01    6.57e+00
W0429 14:34:57.515491 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  49  1.233698e+04    0.00e+00    2.19e+01   0.00e+00   0.00e+00  1.91e+15        1    5.29e-02    6.63e+00
W0429 14:34:57.563660 11724 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  50  1.233698e+04    0.00e+00    2.19e+01   0.00e+00   0.00e+00  4.77e+14        1    4.82e-02    6.68e+00

Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                        22122                    22122
Parameters                              66462                    66462
Residual blocks                         83718                    83718
Residuals                              167436                   167436

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                    SPARSE_SCHUR             SPARSE_SCHUR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                 22106,16
Schur structure                         2,3,9                    2,3,9

Cost:
Initial                          1.842900e+07
Final                            1.233698e+04
Change                           1.841667e+07

Minimizer iterations                       51
Successful steps                           36
Unsuccessful steps                         15

Time (in seconds):
Preprocessor                         0.203083

  Residual only evaluation           0.549531 (37)
  Jacobian & residual evaluation     2.340715 (36)
  Linear solver                      3.013938 (50)
Minimizer                            6.475674

Postprocessor                        0.005911
Total                                6.684669

Termination:                   NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)

9.2.2利用g2o进行BA优化（多个相机和路标点）

代码及详细注释如下:

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include "common.h"
#include "sophus/se3.hpp"

using namespace Sophus;
using namespace Eigen;
using namespace std;

/// 姿态和内参的结构,定义一个结构体表示相机 相机9维
struct PoseAndIntrinsics {
    PoseAndIntrinsics() {}
        //显示构造进行赋值，把数据集的内容赋值过去
    /// set from given data address
    explicit PoseAndIntrinsics(double *data_addr) {
        rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
        translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
        focal = data_addr[6];
        k1 = data_addr[7];
        k2 = data_addr[8];
    }

    /// 将估计值放入内存  //set_to函数是将优化的系数放进内存
    void set_to(double *data_addr) {
        auto r = rotation.log();
        for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
        for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
        data_addr[6] = focal;
        data_addr[7] = k1;
        data_addr[8] = k2;
    }

    SO3d rotation;  //李群 旋转
    Vector3d translation = Vector3d::Zero();    //平移
    double focal = 0;//焦距
    double k1 = 0, k2 = 0;  //畸变系数
};

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
//定义两个顶点 一个是 相机（PoseAndIntrinsics） 一个是路标点（3d点）
//顶点 ：相机（PoseAndIntrinsics）
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoseAndIntrinsics() {}
    //重置
    virtual void setToOriginImpl() override {
        _estimate = PoseAndIntrinsics();//给待优化系数赋上初始值
    }
    //更新
    virtual void oplusImpl(const double *update) override {
        _estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);
        _estimate.focal += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    /// 根据估计值投影一个点
    Vector2d project(const Vector3d &point) {
        Vector3d pc = _estimate.rotation * point + _estimate.translation;
        pc = -pc / pc[2];   //把相机坐标归一化
        double r2 = pc.squaredNorm();   //相机坐标归一化后的模的平方
        double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
        return Vector2d(_estimate.focal * distortion * pc[0],
                        _estimate.focal * distortion * pc[1]);
    }
    //存盘和读盘 ： 留空
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};
    //顶点 ：路标点（3d点）
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoint() {}

    //重置
    virtual void setToOriginImpl() override {
        _estimate = Vector3d(0, 0, 0);  //待优化系数的初始化
    }

    //更新
    virtual void oplusImpl(const double *update) override {
        _estimate += Vector3d(update[0], update[1], update[2]);
    }
     //存盘和读盘 ： 留空
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};

//定义边 这里面边的定义比前几章的要简单很多
class EdgeProjection :
    public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     //计算残差
    virtual void computeError() override {
        auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
        auto v1 = (VertexPoint *) _vertices[1];
        auto proj = v0->project(v1->estimate());
        _error = proj - _measurement;
    }

    //存盘和读盘 ： 留空
    // use numeric derivatives
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

};

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

     BALProblem bal_problem(argv[1]);//传入数据
    bal_problem.Normalize();//对数据进行归一化
    bal_problem.Perturb(0.1,0.5,0.5);//给数据加上噪声（相机旋转、相机平移、路标点）
    bal_problem.WriteToPLYFile("initial_g2o.ply");
    SolveBA(bal_problem);//求解BA
    bal_problem.WriteToPLYFile("final_g2o.ply");

    return 0;
}

void SolveBA(BALProblem &bal_problem) {
    /获得 相机和点的维度
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
       //获得相机和点各自参数的首地址
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();
     //构建图优化
    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;//两个顶点的维度
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // use LM
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);//设置求解器
    optimizer.setVerbose(true);//打开调试输出
    /// build g2o problem
    //获得观测值的首地址
    const double *observations=bal_problem.observations();

    //加入顶点
    //因为顶点有很多个，所以需要容器
    //容器 vertex_pose_intrinsics 和 vertex_points存放两顶点的地址

    vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
    vector<VertexPoint *> vertex_points;
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
        double *camera = cameras + camera_block_size * i;//获得每个相机的首地址
        v->setId(i);//设置编号
        v->setEstimate(PoseAndIntrinsics(camera));//传入待优化的系数 此处为相机
        optimizer.addVertex(v);//加入顶点
        vertex_pose_intrinsics.push_back(v);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        VertexPoint *v = new VertexPoint();//获得每个路标点的首地址
        double *point = points + point_block_size * i;
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(Vector3d(point[0], point[1], point[2]));//传入待优化的系数 此处为路标点
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);//设置边缘化
        optimizer.addVertex(v);//加入顶点
        vertex_points.push_back(v);//将顶点一个一个放回到容器里面
    }

    // edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        EdgeProjection *edge = new EdgeProjection;
        // edge->setId(i);//设置编号
        edge->setVertex(0,vertex_pose_intrinsics[bal_problem.camera_index()[i]]);//加入顶点
        edge->setVertex(1,vertex_points[bal_problem.point_index()[i]]);//加入顶点
        edge->setMeasurement(Sophus::Vector2d(observations[2*i+0],observations[2*i+1]));//设置观测数据
        edge->setInformation(Eigen::Matrix2d::Identity());//设置信息矩阵
        edge->setRobustKernel(new g2o::RobustKernelHuber());//设置核函数
        optimizer.addEdge(edge);//加入边
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);
    //优化后在存到内从中去
    // set to bal problem
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
       double *camera =cameras + camera_block_size*i;
        auto vertex = vertex_pose_intrinsics[i];//把优化后的顶点地址给vertex
        auto estimate = vertex->estimate();//这样estimate就指向了优化后的相机结构体 此时estimate本质上指向了 相机结构体
        estimate.set_to(camera);//这样camera就指向了优化后的相机（利用了相机结构体的set_to()函数）
    }

    for (int i = 0; i < bal_problem.num_points(); ++i) {
        double *point = points + point_block_size * i;
        auto vertex = vertex_points[i];
        for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
    }
        //经过上面的两个循环后，原来待优化的系数就被优化完毕了，并且优化后的系数 还是存放在 bal_problem.mutable_cameras()和 bal_problem.mutable_points() 所对应的地址中

}

结果如下:

Header: 16 22106 83718iteration= 0	 chi2= 8894423.022949	 time= 0.324563 cumTime= 0.324563	 edges= 83718	 schur= 1	 lambda= 227.832660	 levenbergIter= 1
iteration= 1	 chi2= 1772145.050517	 time= 0.284179	 cumTime= 0.608742	 edges= 83718	 schur= 1	 lambda= 75.944220	 levenbergIter= 1
iteration= 2	 chi2= 752585.293391	 time= 0.30358	 cumTime= 0.912323	 edges= 83718	 schur= 1	 lambda= 25.314740	 levenbergIter= 1
iteration= 3	 chi2= 402814.243627	 time= 0.293005	 cumTime= 1.20533	 edges= 83718	 schur= 1	 lambda= 8.438247	 levenbergIter= 1
iteration= 4	 chi2= 284879.378894	 time= 0.289516	 cumTime= 1.49484	 edges= 83718	 schur= 1	 lambda= 2.812749	 levenbergIter= 1
iteration= 5	 chi2= 238356.214415	 time= 0.283434	 cumTime= 1.77828	 edges= 83718	 schur= 1	 lambda= 0.937583	 levenbergIter= 1
iteration= 6	 chi2= 193550.755079	 time= 0.286116	 cumTime= 2.06439	 edges= 83718	 schur= 1	 lambda= 0.312528	 levenbergIter= 1
iteration= 7	 chi2= 146859.909574	 time= 0.281158	 cumTime= 2.34555	 edges= 83718	 schur= 1	 lambda= 0.104176	 levenbergIter= 1
iteration= 8	 chi2= 122887.700218	 time= 0.272719	 cumTime= 2.61827	 edges= 83718	 schur= 1	 lambda= 0.069451	 levenbergIter= 1
iteration= 9	 chi2= 97810.139925	 time= 0.275721	 cumTime= 2.89399	 edges= 83718	 schur= 1	 lambda= 0.046300	 levenbergIter= 1
iteration= 10	 chi2= 80329.940265	 time= 0.267307	 cumTime= 3.1613	 edges= 83718	 schur= 1	 lambda= 0.030867	 levenbergIter= 1
iteration= 11	 chi2= 65663.994405	 time= 0.321365	 cumTime= 3.48266	 edges= 83718	 schur= 1	 lambda= 0.020578	 levenbergIter= 1
iteration= 12	 chi2= 55960.726637	 time= 0.29458	 cumTime= 3.77724	 edges= 83718	 schur= 1	 lambda= 0.013719	 levenbergIter= 1
iteration= 13	 chi2= 53275.547797	 time= 0.282115	 cumTime= 4.05936	 edges= 83718	 schur= 1	 lambda= 0.009146	 levenbergIter= 1
iteration= 14	 chi2= 35983.312124	 time= 0.395526	 cumTime= 4.45488	 edges= 83718	 schur= 1	 lambda= 0.006097	 levenbergIter= 2
iteration= 15	 chi2= 32091.891518	 time= 0.57566	 cumTime= 5.03054	 edges= 83718	 schur= 1	 lambda= 0.016259	 levenbergIter= 3
iteration= 16	 chi2= 31156.262647	 time= 0.383858	 cumTime= 5.4144	 edges= 83718	 schur= 1	 lambda= 0.021679	 levenbergIter= 2
iteration= 17	 chi2= 30773.139623	 time= 0.311891	 cumTime= 5.72629	 edges= 83718	 schur= 1	 lambda= 0.014453	 levenbergIter= 1
iteration= 18	 chi2= 29079.563460	 time= 0.381047	 cumTime= 6.10734	 edges= 83718	 schur= 1	 lambda= 0.012488	 levenbergIter= 2
iteration= 19	 chi2= 28484.154313	 time= 0.420185	 cumTime= 6.52752	 edges= 83718	 schur= 1	 lambda= 0.016651	 levenbergIter= 2
iteration= 20	 chi2= 28445.405201	 time= 0.316267	 cumTime= 6.84379	 edges= 83718	 schur= 1	 lambda= 0.011101	 levenbergIter= 1
iteration= 21	 chi2= 27170.592543	 time= 0.334324	 cumTime= 7.17811	 edges= 83718	 schur= 1	 lambda= 0.011118	 levenbergIter= 2
iteration= 22	 chi2= 26748.191194	 time= 0.333374	 cumTime= 7.51149	 edges= 83718	 schur= 1	 lambda= 0.014824	 levenbergIter= 2
iteration= 23	 chi2= 26675.118188	 time= 0.26431	 cumTime= 7.7758	 edges= 83718	 schur= 1	 lambda= 0.009883	 levenbergIter= 1
iteration= 24	 chi2= 26087.985781	 time= 0.338879	 cumTime= 8.11468	 edges= 83718	 schur= 1	 lambda= 0.010281	 levenbergIter= 2
iteration= 25	 chi2= 25875.818536	 time= 0.410122	 cumTime= 8.5248	 edges= 83718	 schur= 1	 lambda= 0.013708	 levenbergIter= 2
iteration= 26	 chi2= 25831.564925	 time= 0.285147	 cumTime= 8.80994	 edges= 83718	 schur= 1	 lambda= 0.009139	 levenbergIter= 1
iteration= 27	 chi2= 25568.344873	 time= 0.341496	 cumTime= 9.15144	 edges= 83718	 schur= 1	 lambda= 0.011118	 levenbergIter= 2
iteration= 28	 chi2= 25455.865005	 time= 0.34611	 cumTime= 9.49755	 edges= 83718	 schur= 1	 lambda= 0.011781	 levenbergIter= 2
iteration= 29	 chi2= 25454.942053	 time= 0.331617	 cumTime= 9.82917	 edges= 83718	 schur= 1	 lambda= 0.007854	 levenbergIter= 1
iteration= 30	 chi2= 25260.709796	 time= 0.356176	 cumTime= 10.1853	 edges= 83718	 schur= 1	 lambda= 0.009148	 levenbergIter= 2
iteration= 31	 chi2= 25171.392636	 time= 0.345109	 cumTime= 10.5305	 edges= 83718	 schur= 1	 lambda= 0.009425	 levenbergIter= 2
iteration= 32	 chi2= 25104.160294	 time= 0.420982	 cumTime= 10.9514	 edges= 83718	 schur= 1	 lambda= 0.008637	 levenbergIter= 2
iteration= 33	 chi2= 25042.986799	 time= 0.406712	 cumTime= 11.3581	 edges= 83718	 schur= 1	 lambda= 0.008765	 levenbergIter= 2
iteration= 34	 chi2= 24984.677998	 time= 0.43978	 cumTime= 11.7979	 edges= 83718	 schur= 1	 lambda= 0.005949	 levenbergIter= 2
iteration= 35	 chi2= 24943.879912	 time= 0.419768	 cumTime= 12.2177	 edges= 83718	 schur= 1	 lambda= 0.007933	 levenbergIter= 2
iteration= 36	 chi2= 24886.075504	 time= 0.426047	 cumTime= 12.6437	 edges= 83718	 schur= 1	 lambda= 0.005674	 levenbergIter= 2
iteration= 37	 chi2= 24868.088225	 time= 0.414774	 cumTime= 13.0585	 edges= 83718	 schur= 1	 lambda= 0.007565	 levenbergIter= 2
iteration= 38	 chi2= 24833.053138	 time= 0.431343	 cumTime= 13.4899	 edges= 83718	 schur= 1	 lambda= 0.008448	 levenbergIter= 2
iteration= 39	 chi2= 24815.047826	 time= 0.440435	 cumTime= 13.9303	 edges= 83718	 schur= 1	 lambda= 0.009766	 levenbergIter= 2

利用MeshLab显示点云文件,final.py.initial.py

调试遇到问题bug

本章调试遇到bug和第8章基本一致,此外还遇到fmt报错问题,都可以通过修改CmakeList调试通过
修改如下:set(CMAKE_CXX_FLAGS "-O3 -std=c++11")改为set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4"),在每一个target_link_libraries末尾加上 fmt.

cmake_minimum_required(VERSION 2.8)

project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS "-O3 -std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

Find_Package(G2O REQUIRED)
Find_Package(Eigen3 REQUIRED)
Find_Package(Ceres REQUIRED)
Find_Package(Sophus REQUIRED)
Find_Package(CSparse REQUIRED)

SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)

include_directories(${PROJECT_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})

add_library(bal_common common.cpp)
add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp)
add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)

target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common fmt)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common fmt)

参考博客

slam十四讲-ch9（后端1）-卡尔曼滤波器公式推导及BA优化代码实现【注释】（应该是ch13前面最难的部分了）