0.实践前小知识介绍
0.1 数据集的使用
Ceres BA使用的是BAL数据集。在本例中，使用problem-16-22106-pre.txt文件。
BAL的数据集自身存在的特殊
BAL的相机内参模型由焦距f和畸变参数k1，k2给出。
因为BAL数据在投影时假设投影平面在相机光心之后，所以按照我们之前用的模型计算，需要在投影之后乘以系数-1。
1. 实践操作前的准备工作
- 安装meshlab，因为会生成两个后缀名为.ply的文件，这两个文件需要用meshlab来查看。
安装命令：
sudo apt-get update
sudo apt-get install meshlab
- 在终端中进入ch9文件夹下，顺序执行以下命令进行编译。
mkdir build
cd build
cmake ..
//注意，j8还是其他主要看自己的电脑情况
make -j8
- 在build文件中进行运行。
注意： 在make过程中，会出现warning，但是对我们此实践的过程几乎没有影响。
2. 实践过程
2.1 Ceres BA
代码：
#include <iostream>
#include <ceres/ceres.h>
#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 <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>
#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
优化后：
图片中右下角的输出信息在终端也会输出文章来源：https://www.toymoban.com/news/detail-485549.html
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文件时报警告
- 在首次运行查看文件的命令时，终端出现警告：
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...
..........
解决办法：
在对应的文件所在的位置，再次运行命令即可。文章来源地址https://www.toymoban.com/news/detail-485549.html
到了这里，关于视觉SLAM十四讲——ch9实践（后端1）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
