一、KF以及非线性、EKF公式推导（卡尔曼滤波和线性近似卡尔曼滤波）
二、利用ceres进行BA优化（多个相机和路标点）
完整代码注释可以访问我的github链接：slambook2-ch9
注： 程序的演示使用了meshlab显示点云 ，没有安装的朋友，请自行 sudo apt-get install meshalb
下面对 ceres_ba.cpp 和 SnavelyReprojectionError.h 的源码进行了详细的解释
1、ceres_ba.cpp 源码如下：
//
// Created by nnz on 2020/11/10.
//
#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<<"usge : ./ceres_ba problem-16-22106-pre.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指向相机的首地址
const double *observations= bal_Problem.observations();//获得观测数据 observations指向观测数据的首地址
ceres::Problem problem;
//要用循环
for(int i=0;i<bal_Problem.num_observations();i++)
{
ceres::CostFunction *cost_function;
cost_function=SnavelyReprojectionError::Create(observations[2*i],observations[2*i+1]);
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);//核函数
//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(
// cons_function ( new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>
// (
// new SnavelyReprojectionError( observed_x,observed_y ))
// );
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";
}
2、SnavelyReprojectionError.h 源码如下：
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"
//这是一个算投影残差的类 这里就像是 p138页的struct CURVE_FITTING_COST
class SnavelyReprojectionError {
public:
//初始化 用有参构造获取观测数据
SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
observed_y(observation_y) {}
//重载() //残差的计算
template<typename T>
bool operator()(const T *const camera,
const T *const point,
T *residuals) const {
// camera[0,1,2] are the angle-axis rotation
T predictions[2];//存放预测的像素点
CamProjectionWithDistortion(camera, point, predictions);//得到去畸变后的像素，这个像素是预测值，也就是估计值 优化相机和 point
//构造残差
residuals[0] = predictions[0] - T(observed_x);
residuals[1] = predictions[1] - T(observed_y);
return true;
}
// camera : 9 dims array
// [0-2] : angle-axis rotation 旋转
// [3-5] : translateion 平移
// [6] focal length 焦距
// [7-8] second and forth order radial distortion 畸变系数
//这里是认为 fx=fy 然后不考虑 cx cy
// point : 3D location.世界坐标下的3d点
// predictions : 2D predictions with center of the image plane.
//去畸变的后的3d点在相机下的投影
template<typename T>
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);//得到旋转后的点 p
// camera[3,4,5] are the translation
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
//p经过了旋转和平移 就是相机下的坐标
//见书p102页 畸变部分
// Compute the center fo distortion 不懂为什么这里有负号
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
const T &l1 = camera[7];//second order radial distortion
const T &l2 = camera[8];// forth order radial distortion
T r2 = xp * xp + yp * yp;
//下面的式子展开就是p102 式5.12
// 这里认为 x方向的畸变和 y方向的畸变参数是一样的
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
const T &focal = camera[6];
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;
return true;
}
//可以参考p139页的代码
//将cost_function封装在类里面
//这里是创建cost_function
static ceres::CostFunction *Create(const double observed_x, const double observed_y)
{
//这里面残差是二维 待优化的是相机以及3d点 所以自动求导模板中的数字2,9,3分别是它们的维度
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>
(
new SnavelyReprojectionError(observed_x, observed_y))
);
}
private:
//观测数据
double observed_x;
double observed_y;
};
#endif // SnavelyReprojection.h
最后运行结果是：
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 8.52e-02 2.50e-01
1 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 1.95e-01 4.44e-01
2 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 1.82e-01 6.26e-01
3 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 1.82e-01 8.08e-01
4 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 1.84e-01 9.92e-01
5 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 1.74e-01 1.17e+00
6 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 1.76e-01 1.34e+00
7 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 1.80e-01 1.52e+00
8 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 1.84e-01 1.71e+00
9 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 1.83e-01 1.89e+00
10 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 1.77e-01 2.07e+00
11 1.234386e+04 2.24e+00 3.27e+02 8.44e-01 1.90e+00 1.77e+09 1 1.82e-01 2.25e+00
12 1.234232e+04 1.54e+00 3.44e+02 6.69e-01 1.82e+00 5.31e+09 1 1.80e-01 2.43e+00
13 1.234126e+04 1.07e+00 2.21e+02 5.45e-01 1.91e+00 1.59e+10 1 1.80e-01 2.61e+00
14 1.234047e+04 7.90e-01 1.12e+02 4.84e-01 1.87e+00 4.78e+10 1 1.79e-01 2.79e+00
15 1.233986e+04 6.07e-01 1.02e+02 4.22e-01 1.95e+00 1.43e+11 1 1.80e-01 2.97e+00
16 1.233934e+04 5.22e-01 1.03e+02 3.82e-01 1.97e+00 4.30e+11 1 1.80e-01 3.15e+00
17 1.233891e+04 4.25e-01 1.07e+02 3.46e-01 1.93e+00 1.29e+12 1 1.81e-01 3.33e+00
18 1.233855e+04 3.59e-01 1.04e+02 3.15e-01 1.96e+00 3.87e+12 1 1.79e-01 3.51e+00
19 1.233825e+04 3.06e-01 9.27e+01 2.88e-01 1.98e+00 1.16e+13 1 1.78e-01 3.69e+00
20 1.233799e+04 2.61e-01 1.17e+02 2.16e-01 1.97e+00 3.49e+13 1 1.82e-01 3.87e+00
21 1.233777e+04 2.18e-01 1.22e+02 1.15e-01 1.97e+00 1.05e+14 1 1.86e-01 4.05e+00
22 1.233760e+04 1.73e-01 1.10e+02 9.29e-02 1.89e+00 3.14e+14 1 1.91e-01 4.24e+00
23 1.233746e+04 1.37e-01 1.14e+02 9.61e-02 1.98e+00 9.41e+14 1 1.84e-01 4.43e+00
24 1.233735e+04 1.13e-01 1.17e+02 2.04e-01 1.96e+00 2.82e+15 1 1.82e-01 4.61e+00
25 1.233725e+04 9.50e-02 1.20e+02 6.20e-01 1.99e+00 8.47e+15 1 1.84e-01 4.79e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W20201110 21:55:55.961078 4274 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.20e+02 0.00e+00 0.00e+00 4.24e+15 1 5.30e-02 4.85e+00
W20201110 21:55:56.055732 4274 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.20e+02 0.00e+00 0.00e+00 1.06e+15 1 9.45e-02 4.94e+00
28 1.233718e+04 6.92e-02 5.70e+01 4.81e-01 1.70e+00 3.18e+15 1 2.23e-01 5.16e+00
W20201110 21:55:56.329847 4274 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.70e+01 0.00e+00 0.00e+00 1.59e+15 1 5.13e-02 5.21e+00
30 1.233714e+04 3.65e-02 5.90e+01 6.44e-01 1.93e+00 4.77e+15 1 2.24e-01 5.44e+00
W20201110 21:55:56.604920 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
31 1.233714e+04 0.00e+00 5.90e+01 0.00e+00 0.00e+00 2.38e+15 1 5.10e-02 5.49e+00
W20201110 21:55:56.701869 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
32 1.233714e+04 0.00e+00 5.90e+01 0.00e+00 0.00e+00 5.96e+14 1 9.69e-02 5.59e+00
33 1.233711e+04 3.32e-02 6.02e+01 1.61e-01 2.00e+00 1.79e+15 1 2.32e-01 5.82e+00
W20201110 21:55:56.986505 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
34 1.233711e+04 0.00e+00 6.02e+01 0.00e+00 0.00e+00 8.94e+14 1 5.27e-02 5.87e+00
35 1.233708e+04 3.14e-02 5.95e+01 7.49e-01 2.00e+00 2.68e+15 1 2.31e-01 6.10e+00
36 1.233705e+04 2.50e-02 2.57e+01 1.83e+00 1.68e+00 8.04e+15 1 1.82e-01 6.28e+00
W20201110 21:55:57.450224 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
37 1.233705e+04 0.00e+00 2.57e+01 0.00e+00 0.00e+00 4.02e+15 1 5.11e-02 6.34e+00
38 1.233704e+04 1.58e-02 1.96e+01 9.37e-01 1.95e+00 1.00e+16 1 2.25e-01 6.56e+00
W20201110 21:55:57.727345 4274 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.96e+01 0.00e+00 0.00e+00 5.00e+15 1 5.16e-02 6.61e+00
40 1.233702e+04 1.50e-02 6.27e+01 4.78e+00 1.99e+00 1.00e+16 1 2.26e-01 6.84e+00
W20201110 21:55:58.003998 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
41 1.233702e+04 0.00e+00 6.27e+01 0.00e+00 0.00e+00 5.00e+15 1 5.10e-02 6.89e+00
W20201110 21:55:58.099503 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
42 1.233702e+04 0.00e+00 6.27e+01 0.00e+00 0.00e+00 1.25e+15 1 9.55e-02 6.98e+00
43 1.233701e+04 1.48e-02 2.06e+01 3.80e-01 1.98e+00 3.75e+15 1 2.24e-01 7.21e+00
W20201110 21:55:58.373703 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
44 1.233701e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 1.88e+15 1 5.06e-02 7.26e+00
W20201110 21:55:58.469121 4274 levenberg_marquardt_strategy.cc:116] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
45 1.233701e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 4.69e+14 1 9.54e-02 7.35e+00
46 1.233700e+04 1.42e-02 2.08e+01 1.53e-01 1.99e+00 1.41e+15 1 2.29e-01 7.58e+00
47 1.233698e+04 1.39e-02 2.11e+01 3.22e-01 2.00e+00 4.22e+15 1 1.89e-01 7.77e+00
W20201110 21:55:58.941833 4274 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 2.11e+15 1 5.50e-02 7.83e+00
49 1.233697e+04 1.36e-02 2.16e+01 8.38e-01 2.00e+00 6.33e+15 1 2.27e-01 8.05e+00
W20201110 21:55:59.221495 4274 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.16e+01 0.00e+00 0.00e+00 3.16e+15 1 5.24e-02 8.11e+00
Solver Summary (v 2.0.0-eigen-(3.3.7)-lapack-suitesparse-(4.4.6)-cxsparse-(3.1.4)-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.164523
Residual only evaluation 1.127990 (36)
Jacobian & residual evaluation 2.460377 (37)
Linear solver 3.696642 (50)
Minimizer 7.945671
Postprocessor 0.009788
Total 8.119983
Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)
利用 meshlab显示 优化前后的点云
优化前点云：
优化后点云：
四、利用g20进行BA优化（多个相机和路标点）
g2o_ba源码如下：
//
// Created by nnz on 2020/11/11.
//
#include <iostream>
#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 "common.h"
#include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;
using namespace Sophus;
//定义一个结构体表示相机 相机9维
struct PoseAndIntrinsics
{
PoseAndIntrinsics() {}
//显示构造进行赋值，把数据集的内容赋值过去
explicit PoseAndIntrinsics(double *data_addr)
{
rotation=SO3d::exp(Sophus::Vector3d(data_addr[0],data_addr[1],data_addr[2]));
translation=Sophus::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++)
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;//畸变系数
};
//定义两个顶点 一个是 相机（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+(_estimate.k1+_estimate.k2*r2)*r2;
return Sophus::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,Sophus::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;
}
//存盘和读盘 ： 留空
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<<"usge : ./g2o_ba problem-16-22106-pre.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 camera_block_size=bal_problem.camera_block_size();
const int point_block_size=bal_problem.point_block_size();
//获得相机和点各自参数的首地址
double *cameras=bal_problem.mutable_cameras();
double *points=bal_problem.mutable_points();
//构建图优化
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3>> BlockSolverType;//两个顶点的维度
typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
//使用LM方法
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);//设置求解器
optimizer.setVerbose(true);//打开调试输出
//获得观测值的首地址
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]));//传入待优化的系数 此处为路标点
//g20需要手动设置待边缘化（Marg）的点
v->setMarginalized(true);//设置边缘化
optimizer.addVertex(v);//加入顶点
vertex_points.push_back(v);//将顶点一个一个放回到容器里面
}
//加入边
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);
//优化后在存到内从中去
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() 所对应的地址中
}
运行结果如下图所示：
优化前：
优化后：
补充：在rotation. h中
函数AngleAxisRotatePoint()用到的公式如下图所示：
函数AngleAxisToQuaternion()用到的公式如下图所示：
函数QuaternionToAngleAxis()用到的公式如下图所示：
386
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
