9.1理论部分
推荐参考博文推导:
9.2实践部分
9.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);//利用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 <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;
/// 姿态和内参的结构,定义一个结构体表示相机 相机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)
评论（0）
您还未登录，请登录后发表或查看评论