视觉SLAM十四讲——ch9实践（后端1）

0.实践前小知识介绍

0.1 数据集的使用

Ceres BA使用的是BAL数据集。在本例中，使用problem-16-22106-pre.txt文件。
BAL的数据集自身存在的特殊

BAL的相机内参模型由焦距f和畸变参数k1，k2给出。
因为BAL数据在投影时假设投影平面在相机光心之后，所以按照我们之前用的模型计算，需要在投影之后乘以系数-1。

1. 实践操作前的准备工作

1. 安装meshlab，因为会生成两个后缀名为.ply的文件，这两个文件需要用meshlab来查看。
   安装命令：

 sudo apt-get update
 sudo apt-get install meshlab

2. 在终端中进入ch9文件夹下，顺序执行以下命令进行编译。

mkdir build
cd build
cmake ..
//注意，j8还是其他主要看自己的电脑情况
make -j8

3. 在build文件中进行运行。
   注意： 在make过程中，会出现warning，但是对我们此实践的过程几乎没有影响。

2. 实践过程

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);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("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();
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_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();
    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.
        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);
    }

    // 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;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}

在build中执行语句：

 ./bundle_adjustment_ceres /home/fighter/slam/slambook2/ch9/problem-16-22106-pre.txt

运行结果：

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    1.84e-01    6.03e-01
   1  1.449093e+06    1.70e+07    1.75e+06   2.16e+03   1.84e+00  3.00e+04        1    2.79e-01    8.82e-01
   2  5.848543e+04    1.39e+06    1.30e+06   1.55e+03   1.87e+00  9.00e+04        1    1.37e-01    1.02e+00
   3  1.581483e+04    4.27e+04    4.98e+05   4.98e+02   1.29e+00  2.70e+05        1    1.28e-01    1.15e+00
   4  1.251823e+04    3.30e+03    4.64e+04   9.96e+01   1.11e+00  8.10e+05        1    1.24e-01    1.27e+00
   5  1.240936e+04    1.09e+02    9.78e+03   1.33e+01   1.42e+00  2.43e+06        1    1.27e-01    1.40e+00
   6  1.237699e+04    3.24e+01    3.91e+03   5.04e+00   1.70e+00  7.29e+06        1    1.29e-01    1.53e+00
   7  1.236187e+04    1.51e+01    1.96e+03   3.40e+00   1.75e+00  2.19e+07        1    1.26e-01    1.65e+00
   8  1.235405e+04    7.82e+00    1.03e+03   2.40e+00   1.76e+00  6.56e+07        1    1.24e-01    1.78e+00
   9  1.234934e+04    4.71e+00    5.04e+02   1.67e+00   1.87e+00  1.97e+08        1    1.26e-01    1.90e+00
  10  1.234610e+04    3.24e+00    4.31e+02   1.15e+00   1.88e+00  5.90e+08        1    1.29e-01    2.03e+00
  11  1.234386e+04    2.24e+00    3.27e+02   8.44e-01   1.90e+00  1.77e+09        1    1.28e-01    2.16e+00
  12  1.234232e+04    1.54e+00    3.44e+02   6.69e-01   1.82e+00  5.31e+09        1    1.26e-01    2.29e+00
  13  1.234126e+04    1.07e+00    2.21e+02   5.45e-01   1.91e+00  1.59e+10        1    1.24e-01    2.41e+00
  14  1.234047e+04    7.90e-01    1.12e+02   4.84e-01   1.87e+00  4.78e+10        1    1.25e-01    2.54e+00
  15  1.233986e+04    6.07e-01    1.02e+02   4.22e-01   1.95e+00  1.43e+11        1    1.28e-01    2.66e+00
  16  1.233934e+04    5.22e-01    1.03e+02   3.82e-01   1.97e+00  4.30e+11        1    1.30e-01    2.79e+00
  17  1.233891e+04    4.25e-01    1.07e+02   3.46e-01   1.93e+00  1.29e+12        1    1.22e-01    2.92e+00
  18  1.233855e+04    3.59e-01    1.04e+02   3.15e-01   1.96e+00  3.87e+12        1    1.23e-01    3.04e+00
  19  1.233825e+04    3.06e-01    9.27e+01   2.88e-01   1.98e+00  1.16e+13        1    1.21e-01    3.16e+00
  20  1.233799e+04    2.61e-01    1.17e+02   2.16e-01   1.97e+00  3.49e+13        1    1.22e-01    3.28e+00
  21  1.233777e+04    2.18e-01    1.22e+02   1.15e-01   1.97e+00  1.05e+14        1    1.20e-01    3.40e+00
  22  1.233760e+04    1.73e-01    1.10e+02   9.59e-02   1.89e+00  3.14e+14        1    1.22e-01    3.53e+00
  23  1.233746e+04    1.37e-01    1.14e+02   1.68e-01   1.98e+00  9.41e+14        1    1.24e-01    3.65e+00
  24  1.233735e+04    1.13e-01    1.17e+02   2.36e-01   1.96e+00  2.82e+15        1    1.28e-01    3.78e+00
  25  1.233725e+04    9.50e-02    1.18e+02   1.28e+00   1.99e+00  8.47e+15        1    1.23e-01    3.90e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0615 16:19:52.003427  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  26  1.233725e+04    0.00e+00    1.18e+02   0.00e+00   0.00e+00  4.24e+15        1    4.75e-02    3.95e+00
W0615 16:19:52.048473  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  27  1.233725e+04    0.00e+00    1.18e+02   0.00e+00   0.00e+00  1.06e+15        1    4.46e-02    3.99e+00
  28  1.233718e+04    6.92e-02    5.68e+01   3.52e-01   1.70e+00  3.18e+15        1    1.23e-01    4.12e+00
W0615 16:19:52.217936  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  29  1.233718e+04    0.00e+00    5.68e+01   0.00e+00   0.00e+00  1.59e+15        1    4.63e-02    4.16e+00
W0615 16:19:52.263574  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  30  1.233718e+04    0.00e+00    5.68e+01   0.00e+00   0.00e+00  3.97e+14        1    4.56e-02    4.21e+00
  31  1.233714e+04    3.65e-02    5.88e+01   9.90e-02   1.93e+00  1.19e+15        1    1.21e-01    4.33e+00
  32  1.233711e+04    3.32e-02    5.99e+01   2.59e-01   2.00e+00  3.57e+15        1    1.20e-01    4.45e+00
W0615 16:19:52.551789  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  33  1.233711e+04    0.00e+00    5.99e+01   0.00e+00   0.00e+00  1.79e+15        1    4.67e-02    4.50e+00
  34  1.233708e+04    3.14e-02    6.16e+01   1.08e+00   2.00e+00  5.36e+15        1    1.20e-01    4.62e+00
W0615 16:19:52.721449  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  35  1.233708e+04    0.00e+00    6.16e+01   0.00e+00   0.00e+00  2.68e+15        1    4.93e-02    4.67e+00
W0615 16:19:52.765900  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  36  1.233708e+04    0.00e+00    6.16e+01   0.00e+00   0.00e+00  6.70e+14        1    4.44e-02    4.71e+00
  37  1.233705e+04    2.50e-02    2.04e+01   9.75e-02   1.68e+00  2.01e+15        1    1.31e-01    4.84e+00
  38  1.233704e+04    1.58e-02    1.87e+01   7.15e-01   1.95e+00  6.03e+15        1    1.22e-01    4.96e+00
W0615 16:19:53.064455  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  39  1.233704e+04    0.00e+00    1.87e+01   0.00e+00   0.00e+00  3.02e+15        1    4.59e-02    5.01e+00
W0615 16:19:53.108860  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  40  1.233704e+04    0.00e+00    1.87e+01   0.00e+00   0.00e+00  7.54e+14        1    4.44e-02    5.05e+00
  41  1.233702e+04    1.51e-02    2.06e+01   1.12e-01   2.00e+00  2.26e+15        1    1.19e-01    5.17e+00
  42  1.233701e+04    1.48e-02    2.10e+01   8.72e-01   1.99e+00  6.79e+15        1    1.24e-01    5.30e+00
W0615 16:19:53.398123  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  43  1.233701e+04    0.00e+00    2.10e+01   0.00e+00   0.00e+00  3.39e+15        1    4.64e-02    5.34e+00
  44  1.233700e+04    1.42e-02    1.57e+01   1.28e+00   1.99e+00  1.00e+16        1    1.20e-01    5.46e+00
W0615 16:19:53.564965  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  45  1.233700e+04    0.00e+00    1.57e+01   0.00e+00   0.00e+00  5.00e+15        1    4.65e-02    5.51e+00
W0615 16:19:53.609803  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  46  1.233700e+04    0.00e+00    1.57e+01   0.00e+00   0.00e+00  1.25e+15        1    4.47e-02    5.55e+00
  47  1.233698e+04    1.39e-02    2.11e+01   1.94e-01   2.00e+00  3.75e+15        1    1.22e-01    5.68e+00
W0615 16:19:53.777860  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  48  1.233698e+04    0.00e+00    2.11e+01   0.00e+00   0.00e+00  1.88e+15        1    4.61e-02    5.72e+00
  49  1.233697e+04    1.36e-02    2.01e+01   7.07e-01   2.00e+00  5.62e+15        1    1.20e-01    5.84e+00
W0615 16:19:53.943998  5230 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
  50  1.233697e+04    0.00e+00    2.01e+01   0.00e+00   0.00e+00  2.81e+15        1    4.62e-02    5.89e+00

Solver Summary (v 2.0.0-eigen-(3.3.7)-lapack-suitesparse-(5.7.1)-cxsparse-(3.2.0)-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.233697e+04
Change                           1.841667e+07

Minimizer iterations                       51
Successful steps                           37
Unsuccessful steps                         14

Time (in seconds):
Preprocessor                         0.419711

  Residual only evaluation           0.517138 (36)
  Jacobian & residual evaluation     1.818814 (37)
  Linear solver                      2.616899 (50)
Minimizer                            5.472081

Postprocessor                        0.007762
Total                                5.899554

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

总体的误差应该随着迭代次数的增长不断下降；

运行结束后会输出两个文件，优化前的点云输出为initial.ply，优化后的点云输出为final.ply。
查看两个点云的命令如下：

meshlab initial.ply
meshlab final.ply

生成的图像如下所示：
初始图片：
图片中右下角的输出信息在终端也会输出

Current Plugins Dir is: /usr/lib/x86_64-linux-gnu/meshlab/plugins
Shader directory found '/usr/share/meshlab/shaders', and it contains 19 gdp files
LOG: 0 Opened mesh initial.ply in 453 msec
LOG: 0 All files opened in 454 msec

优化后：
图片中右下角的输出信息在终端也会输出

Current Plugins Dir is: /usr/lib/x86_64-linux-gnu/meshlab/plugins
Shader directory found '/usr/share/meshlab/shaders', and it contains 19 gdp files
LOG: 0 Opened mesh final.ply in 435 msec
LOG: 0 All files opened in 436 msec

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;

/// 姿态和内参的结构
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];
    }

    /// 将估计值放入内存
    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
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 {}
};

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.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("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();
    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
    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->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
        edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
        edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
        edge->setInformation(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];
        auto estimate = vertex->estimate();
        estimate.set_to(camera);
    }
    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];
    }
}

在build中执行语句：

 ./bundle_adjustment_g2o /home/fighter/slam/slambook2/ch9/problem-16-22106-pre.txt

运行结果：

Header: 16 22106 83718iteration= 0       chi2= 8894422.962194    time= 0.253426  cumTime= 0.253426       edges= 83718    schur= 1        lambda= 227.832660      levenbergIter= 1
iteration= 1     chi2= 1772145.543625    time= 0.21523   cumTime= 0.468656       edges= 83718    schur= 1        lambda= 75.944220       levenbergIter= 1
iteration= 2     chi2= 752585.321418     time= 0.212928  cumTime= 0.681584       edges= 83718    schur= 1        lambda= 25.314740       levenbergIter= 1
iteration= 3     chi2= 402814.285609     time= 0.210998  cumTime= 0.892582       edges= 83718    schur= 1        lambda= 8.438247        levenbergIter= 1
iteration= 4     chi2= 284879.389455     time= 0.232436  cumTime= 1.12502        edges= 83718    schur= 1        lambda= 2.812749        levenbergIter= 1
iteration= 5     chi2= 238356.210033     time= 0.243281  cumTime= 1.3683         edges= 83718    schur= 1        lambda= 0.937583        levenbergIter= 1
iteration= 6     chi2= 193550.729802     time= 0.228287  cumTime= 1.59659        edges= 83718    schur= 1        lambda= 0.312528        levenbergIter= 1
iteration= 7     chi2= 146861.192839     time= 0.212535  cumTime= 1.80912        edges= 83718    schur= 1        lambda= 0.104176        levenbergIter= 1
iteration= 8     chi2= 122873.392728     time= 0.214303  cumTime= 2.02342        edges= 83718    schur= 1        lambda= 0.069451        levenbergIter= 1
iteration= 9     chi2= 97812.478436      time= 0.213007  cumTime= 2.23643        edges= 83718    schur= 1        lambda= 0.046300        levenbergIter= 1
iteration= 10    chi2= 80336.316621      time= 0.210887  cumTime= 2.44732        edges= 83718    schur= 1        lambda= 0.030867        levenbergIter= 1
iteration= 11    chi2= 65654.850651      time= 0.210773  cumTime= 2.65809        edges= 83718    schur= 1        lambda= 0.020578        levenbergIter= 1
iteration= 12    chi2= 55967.141021      time= 0.209642  cumTime= 2.86773        edges= 83718    schur= 1        lambda= 0.013719        levenbergIter= 1
iteration= 13    chi2= 53270.115686      time= 0.210373  cumTime= 3.07811        edges= 83718    schur= 1        lambda= 0.009146        levenbergIter= 1
iteration= 14    chi2= 35981.369897      time= 0.266059  cumTime= 3.34416        edges= 83718    schur= 1        lambda= 0.006097        levenbergIter= 2
iteration= 15    chi2= 32092.173309      time= 0.316057  cumTime= 3.66022        edges= 83718    schur= 1        lambda= 0.016259        levenbergIter= 3
iteration= 16    chi2= 31154.877381      time= 0.261828  cumTime= 3.92205        edges= 83718    schur= 1        lambda= 0.021679        levenbergIter= 2
iteration= 17    chi2= 30773.690800      time= 0.211655  cumTime= 4.1337         edges= 83718    schur= 1        lambda= 0.014453        levenbergIter= 1
iteration= 18    chi2= 29079.971263      time= 0.266129  cumTime= 4.39983        edges= 83718    schur= 1        lambda= 0.012508        levenbergIter= 2
iteration= 19    chi2= 28481.944292      time= 0.26298   cumTime= 4.66281        edges= 83718    schur= 1        lambda= 0.016678        levenbergIter= 2
iteration= 20    chi2= 28439.938323      time= 0.210573  cumTime= 4.87339        edges= 83718    schur= 1        lambda= 0.011118        levenbergIter= 1
iteration= 21    chi2= 27171.835892      time= 0.264091  cumTime= 5.13748        edges= 83718    schur= 1        lambda= 0.011153        levenbergIter= 2
iteration= 22    chi2= 26749.623597      time= 0.265487  cumTime= 5.40296        edges= 83718    schur= 1        lambda= 0.014871        levenbergIter= 2
iteration= 23    chi2= 26674.555645      time= 0.209385  cumTime= 5.61235        edges= 83718    schur= 1        lambda= 0.009914        levenbergIter= 1
iteration= 24    chi2= 26089.998120      time= 0.262896  cumTime= 5.87524        edges= 83718    schur= 1        lambda= 0.010288        levenbergIter= 2
iteration= 25    chi2= 25877.861699      time= 0.264081  cumTime= 6.13933        edges= 83718    schur= 1        lambda= 0.013717        levenbergIter= 2
iteration= 26    chi2= 25834.638622      time= 0.213956  cumTime= 6.35328        edges= 83718    schur= 1        lambda= 0.009145        levenbergIter= 1
iteration= 27    chi2= 25570.298632      time= 0.264777  cumTime= 6.61806        edges= 83718    schur= 1        lambda= 0.011127        levenbergIter= 2
iteration= 28    chi2= 25457.520755      time= 0.26327   cumTime= 6.88133        edges= 83718    schur= 1        lambda= 0.011716        levenbergIter= 2
iteration= 29    chi2= 25380.650160      time= 0.26591   cumTime= 7.14724        edges= 83718    schur= 1        lambda= 0.012090        levenbergIter= 2
iteration= 30    chi2= 25362.215118      time= 0.211995  cumTime= 7.35923        edges= 83718    schur= 1        lambda= 0.008060        levenbergIter= 1
iteration= 31    chi2= 25202.452901      time= 0.263831  cumTime= 7.62306        edges= 83718    schur= 1        lambda= 0.008800        levenbergIter= 2
iteration= 32    chi2= 25122.596797      time= 0.263859  cumTime= 7.88692        edges= 83718    schur= 1        lambda= 0.009613        levenbergIter= 2
iteration= 33    chi2= 25115.107638      time= 0.214994  cumTime= 8.10192        edges= 83718    schur= 1        lambda= 0.006408        levenbergIter= 1
iteration= 34    chi2= 24967.189622      time= 0.269087  cumTime= 8.371  edges= 83718    schur= 1        lambda= 0.006268        levenbergIter= 2
iteration= 35    chi2= 24910.138884      time= 0.263436  cumTime= 8.63444        edges= 83718    schur= 1        lambda= 0.008358        levenbergIter= 2
iteration= 36    chi2= 24867.273980      time= 0.261406  cumTime= 8.89584        edges= 83718    schur= 1        lambda= 0.006277        levenbergIter= 2
iteration= 37    chi2= 24848.840518      time= 0.262723  cumTime= 9.15857        edges= 83718    schur= 1        lambda= 0.008370        levenbergIter= 2
iteration= 38    chi2= 24847.135479      time= 0.210439  cumTime= 9.36901        edges= 83718    schur= 1        lambda= 0.005580        levenbergIter= 1
iteration= 39    chi2= 24798.075555      time= 0.26268   cumTime= 9.63169        edges= 83718    schur= 1        lambda= 0.007249        levenbergIter= 2

同样的，我们可以查看优化前后两个点云的图像，查看结果如下所示：
优化前：

优化后：

初始图片：
图片中右下角的输出信息在终端也会输出

Current Plugins Dir is: /usr/lib/x86_64-linux-gnu/meshlab/plugins
Shader directory found '/usr/share/meshlab/shaders', and it contains 19 gdp files
LOG: 0 Opened mesh initial.ply in 534 msec
LOG: 0 All files opened in 535 msec

优化后：
图片中右下角的输出信息在终端也会输出

Current Plugins Dir is: /usr/lib/x86_64-linux-gnu/meshlab/plugins
Shader directory found '/usr/share/meshlab/shaders', and it contains 19 gdp files
LOG: 0 Opened mesh final.ply in 673 msec
LOG: 0 All files opened in 676 msec

3. 遇到的问题及解决办法

3.1 查看.ply文件时报警告

1. 在首次运行查看文件的命令时，终端出现警告：

Current Plugins Dir is: /usr/lib/x86_64-linux-gnu/meshlab/plugins
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
Warning! a default param was not found in the saved settings. This should happen only on the first run...
..........

解决办法：
在对应的文件所在的位置，再次运行命令即可。