目录
概述
1.1 状态估计的概率解释
1.2 线性系统和KF
1.3 非线性优化和EKF
2 BA与图优化
2.1 投影模型和BA代价函数
2.2 BA的求解
2.3 稀疏性和边缘化
2.4 鲁棒核函数
2.5 小结
3 实践:g2o
3.1 BA数据集
3.2 g2o求解BA
3.3 求解
概述:
前端能够根据相邻的两幅图像判断出此时此刻的位姿,是暂时的;那么后端需要对前端测量以及计算的结果进行矫正,不仅用过去的信息,也用未来的信息更新自己,希望能够得到一个长时间的正确状态。
在运动方程和观测方程中,如果把位姿和路标看成随机变量,就可以把问题变为已知观测数据和运动数据情况下,如何确定状态量的分布问题,是一个状态估计问题。假设噪声和状态量服从高斯分布,只需要估计它的均值和方差,即可确定状态量的分布。
注:刻画分布用到均值和协方差矩阵,此处均值代表着状态估计值,协方差矩阵代表着不确定性。
完成后端优化可以使用滤波器,也可以使用非线性优化,本讲对两种方法都做了推导。
理解批量式 和 渐进式 处理方法
前端给到的是带有噪声的,比较粗糙的数据(换句话说视觉里程计VO得到的是粗糙的位姿估计,有累计误差,因为仅仅依靠相邻帧),后端使得平均误差更小,后端一般是一个最大似然,最大后验概率。
渐进式------EKF
批量式------非线性优化
依靠运动估计去推到轨迹是很粗糙的,累计误差会越来越大,没有约束; 观测虽然也有不确定性,但是可以依靠观测约束不确定性的增长,误差均分到整个过程。
似然可以通过观测方程推出
先验比较麻烦,先条件概率展开
假设一阶马尔可夫性,
第一项:K时刻状态只与K-1时刻有关 ------->(运动方程)。
第二项:状态估计------->(观测方程)。非线性优化
线性系统---- h 和 f函数是线性函数,线性方程性质:高斯分布的线性组合还是线性分布。
高斯分布,需要均值和协方差就可以刻画改分布长什么样。其中均值一般指状态估计的值,协方差是估计的不确定度。
卡尔曼滤波器线性的推导
预测: K-1时刻后验推导K时刻先验, 依靠运动方程。
更新: K时刻先验推导K时刻后验, 依靠观测方程。
非线性系统和EKF
非线性系统,高斯分布下,在工作点附近线性展开,得到EKF。
BA与图优化
书本定义:所谓的 Bundle Adjustment,是指从视觉重建中提炼出最优的3D模型和相机参数(内参数和外参数)。从每一个特征点反射出来的几束光线(bundles of light rays),在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后,最后收束到相机光心的这个过程 ,简称为 BA。
批量式处理方式:
1 先定义误差;
2 平方和(最小二乘),找梯度(梯度下降);
3 迭代使其收敛。
图模表示:优化量用节点表示,运动/观测用边来表示。
观测方程,其中包含畸变矫正,如下:
每个观测只关系两个变量,其中一个是相机,一个是路标,导致h具有特殊结构。
后续理论省略。。。。。。。。
实践g2o
详细代码注释
定义图模型的节点和边,节点表示相机位姿pose和路标点landmark,边表示相机路标之间的观测。
#include <Eigen/Core>
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "ceres/autodiff.h"
#include "tools/rotation.h"
#include "common/projection.h"
// 定义节点 和 边
// 节点用相机和路标来表示, 边表示它们之间的预测。
// 此处使用自动求导, 所以更新量看作向量加法。 之前是通过两个SE(3)相乘然后转化成新的SE(3),但是使用的是左绕动模型
//定义相机位姿顶点类,由于相机内参也作为优化变量,所以包含了:
//此处可能是三个外参(f,cx,cy),应该不是三个畸变系数。 另外还有3个参数的平移,3个参数的旋转。一共九个量,9维,类型为Eigen::VectorXd
// 相机顶点
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexCameraBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update )
{
//由于update是个double类型数组,而增量需要的是个矩阵,
//所以用update构造一个增量矩阵v,下面更新估计值时,直接将v加上就好了。
//ConstMapType定义如下:typedef const Eigen::Map<const Derived,Unaligned>ConstMapType;
//发现就是一个typedef,还是Map类型。
//对于动态矩阵,上方的位姿,VectorXd构造时要传入维度,VertexCameraBAL::Dimension就是位姿顶点的维度,也就是定义时传入的模板参数9。
Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
_estimate += v;
}
};
// 空间点顶点
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPointBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update )
{
//而静态矩阵,上方的landmark,Vector3d构造时就不用传入维度了,直接传出需要转化的数组就好了。
Eigen::Vector3d::ConstMapType v ( update );
_estimate += v;
}
};
// 边
//BAL观测边,边即误差,继承自基础二元边。这里误差应该是重投影的像素误差
// 参数为:误差维度2维,误差类型为Eigen::Vector2d,连接两个顶点:VertexCameraBAL和VertexPointBAL(也就是说误差和这两个优化变量有关)
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeObservationBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
// 使用G2O数值导, 不像书上调用的ceres的自动求导功能
virtual void computeError() override // The virtual function comes from the Edge base class. Must define if you use edge.
{
// 这里将第0个顶点, 相机位姿取出来
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
// 这里将第1个顶点,空间点位置取出来
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
//将相机位姿估计值,空间点位姿估计值 传给了重载的()运算符,这个重载,将计算好的结果输出到_error.data(),完成了误差的计算
( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
}
// 这里即为重载的()函数,为模板函数,需要数据为相机位姿指针,空间点位置指针,用于承接输出误差的residuals。
// 上面调用时,用的_error.data()承接,完成误差计算。
// 这个模板类其实还是用的重投影误差
template<typename T>
bool operator() ( const T* camera, const T* point, T* residuals ) const
{
T predictions[2];
//这个函数就是反应的投影过程,camera参数,point参数,然后用predictions承接算得的像素坐标。这里也发现就是二维的
CamProjectionWithDistortion ( camera, point, predictions );
residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
residuals[1] = predictions[1] - T ( measurement() ( 1 ) );
return true;
// 小总结一下,从computeError()一直到这里,搞得这一些就是为了计算一个重投影误差,
// 误差的计算被写进了重载的()中,投影过程被写进了CamProjectionWithDistortion()中
}
//这里重写线性增量方程,也就是雅克比矩阵
virtual void linearizeOplus() override
{
// use numeric Jacobians
// BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
// return;
// using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians
//使用ceres的自动求导,不然系统将调用g2o的数值求导
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
//这里贴上AutoDiff的定义,发现是一个模板结构体,模板参数为代价函数类型,模板类型,代价函数的各参数维度(这里就两个了,相机顶点维度,空间点维度)
/*template <typename Functor, typename T,
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
struct AutoDiff {
static bool Differentiate(const Functor& functor,
T const *const *parameters,
int num_outputs,
T *function_value,
T **jacobians) {...}*/
//这里来一个typedef,将模板类简化定义一下,定义成BalAutoDiff
//看一下模板参数:
//EdgeObservationBAL,就是代价函数类型,这里就是边的类型了
//模板类型为double,基本数据的类型
//VertexCameraBAL::Dimension和VertexPointBAL::Dimension就是对应的两个N0和N1,误差函数参数的维度
//这里直接把维度取出来了(Dimension即是取得维度),也可以直接输入9和3
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
//定义一个行优先的double类型矩阵,大小为Dimension*VertexCameraBAL::Dimension,也就是2*9。这里就是误差对相机的导数
Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
//定义一个行优先的double类型矩阵,大小为Dimension*VertexPointBAL::Dimension,也就是2*3。这里就是误差对空间点的导数
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
//double*类型的数组,成员为double*,这里装了相机估计值数组指针和空间点估计值数组指针。
double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
//雅克比矩阵为两块导数拼合起来的,一块是误差对相机的导数,一块是误差对空间点的导数。也就是上方定义的2*9的dError_dCamera和2*3的dError_dPoint
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
double value[Dimension];
//这里就是一直所说的利用ceres的现行求导,这个Differentiate()就是在AutoDiff结构体中定义的。
/*static bool Differentiate(const Functor& functor,
T const *const *parameters,
int num_outputs,
T *function_value,
T **jacobians) {...}*/
//看一下参数:
//const Functor& functor,代价函数,这里也就是这个边类了,直接用*this
//T const *const *parameters,参数列表,就是上面定义的有两个double指针的parameters数组,这两个指针一个指向相机参数数组,一个指向空间点数组
//int num_outputs,输出的维度,这里就是边的维度Dimension,也就是2维
//T *function_value,误差函数functor的输出值,用于承接functor的输出,也就是*this计算出来的误差。
//T **jacobians,这就是最终要求的雅克比矩阵了。用于承接。
bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
// copy over the Jacobians (convert row-major -> column-major)
//雅克比矩阵到这里就计算完成了,最后就是赋值给_jacobianOplusXi和_jacobianOplusXj了。
if ( diffState )
{
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
}
else
{
//不成功就输出警告,并将雅克比矩阵置为0矩阵。
assert ( 0 && "Error while differentiating" );
_jacobianOplusXi.setZero();
_jacobianOplusXj.setZero();
}
}
};
投影模型注释:
#ifndef PROJECTION_H
#define PROJECTION_H
#include "tools/rotation.h"
// camera : 9 dims array with
// [0-2] : angle-axis rotation
// [3-5] : translateion
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
// point : 3D location.
// predictions : 2D predictions with center of the image plane.
/***
* 去畸变,世界坐标 转 相机坐标 转 像素坐标, 基本跟书上一致
* 基本思想:
* P = R * X + t 从世界坐标系转相机坐标系
* p = -p / p.z
* p` = f * r(p) * p 转到像素坐标
*
* 相机畸变该处只考虑径向畸变,流程:
* 1)将三维空间大点投影到归一化平面
* 2)对归一化平面上的点进行畸变矫正
* 3)将矫正后的点通过内参数矩阵投影到像素平面,得到该点在图像上的正确位置。
*/
template<typename T>
inline bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions){
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation
// 旋转矩阵表示空间点
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
// Compute the center fo distortion
// xp = -x/z
// yp = -y/z
T xp = -p[0]/p[2];
T yp = -p[1]/p[2];
// Apply second and fourth order radial distortion
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
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;
}
#endif // projection.h
主文件g2o_bundle.cpp
#include <Eigen/StdVector>
#include <Eigen/Core>
#include <iostream>
#include <stdint.h>
#include <unordered_set>
#include <memory>
#include <vector>
#include <stdlib.h>
#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"
using namespace Eigen;
using namespace std;
typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
// 给求解器模块定义维度 , pose: 九维 landmark: 三维
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;
// set up the vertexs and edges for the bundle adjustment.
// 问题构建函数,传入一个BALProblem类型指针,稀疏求解器指针,参数类引用BuildProblem&
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
// 将bal_problem中的数据读出来
const int num_points = bal_problem->num_points();
const int num_cameras = bal_problem->num_cameras();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
// Set camera vertex with initial value in the dataset.
// 将相机首位置读出, 方便后方数据读取
const double* raw_cameras = bal_problem->cameras();
for(int i = 0; i < num_cameras; ++i)
{
// 这里将9维相机位姿丛数组中取出来构建矩阵, 用于下面的顶点的初始赋值
ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
// 开辟新的相机顶点类指针
VertexCameraBAL* pCamera = new VertexCameraBAL();
pCamera->setEstimate(temVecCamera); // initial value for the camera i..
pCamera->setId(i); // set id for each camera vertex
// remeber to add vertex into optimizer..
// 将顶点添加至优化器
optimizer->addVertex(pCamera);
}
// Set point vertex with initial value in the dataset.
const double* raw_points = bal_problem->points();
// const int point_block_size = bal_problem->point_block_size();
for(int j = 0; j < num_points; ++j)
{
ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
VertexPointBAL* pPoint = new VertexPointBAL();
pPoint->setEstimate(temVecPoint); // initial value for the point i..
// 设定ID,不能和前面相机顶点重复, 所以加了相机个数, 直接往后排
pPoint->setId(j + num_cameras); // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex
// remeber to add vertex into optimizer..
pPoint->setMarginalized(true);
optimizer->addVertex(pPoint);
}
// Set edges for graph..
// 取出边的个数
const int num_observations = bal_problem->num_observations();
// 取出边数组首位置
const double* observations = bal_problem->observations(); // pointer for the first observation..
for(int i = 0; i < num_observations; ++i)
{
// 开辟边的内存指针
EdgeObservationBAL* bal_edge = new EdgeObservationBAL();
const int camera_id = bal_problem->camera_index()[i]; // get id for the camera;
const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point
if(params.robustify)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(1.0);
bal_edge->setRobustKernel(rk);
}
// set the vertex by the ids for an edge observation
bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
bal_edge->setInformation(Eigen::Matrix2d::Identity());
// 设置默认值, 就是将观测数据读进去
bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));
optimizer->addEdge(bal_edge) ;
}
}
/***
* 类 BALProblem、 BundleParams 说明
* BALProblem跟优化数据.txt对接, 负责txt的读取, 写入, 同时生成PLY点云文件
* BundleParams类负责优化需要的参数值, 默认值设定和用户命令行输入等功能
* 整体这样归类后, 所有优化数据就去BALProblem类对象中询问, 参数就去BundleParams类对象中询问。
*/
//这个函数的作用是将优化后的结果再写入到BALProblem类中,
//注意,在BALProblem类中,定义的所有读取写入功能都是BALProblem类与txt数据的,并没有优化后的数据与BALProblem的,
//所以这里定义了之后,就会产生优化后的数据类BALProblem,这样再跟txt或者PLY对接的话就很容易了。
//参数很容易理解,被写入的BALProblem*,优化器
void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer)
{
const int num_points = bal_problem->num_points();
const int num_cameras = bal_problem->num_cameras();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
//用mutable_cameras()函数取得相机首地址,用于后面的数据写入
double* raw_cameras = bal_problem->mutable_cameras();
for(int i = 0; i < num_cameras; ++i)
{
//将相机顶点取出,这里说一下为什么要做这一步指针类型转化,因为optimizer->vertex(i)返回的类型是个vertex*指针类型,
//需要将其转化成VertexCameraBAL*才能访问估计值,直接像下面的用法会报错:
//optimizer->vertex(i)-> estimate();
//原程序是下面这样写的,但是感觉这里用auto比较方便一些,并且也更能体现pCamera仅是个承接的功能。
//VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
//auto pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
Eigen::VectorXd NewCameraVec = pCamera->estimate();
//取得估计值之后,就可以memcpy()了,这里当然是一个9维的数组,长度上很明显是9*double
memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
}
//同理在point上也是一样
double* raw_points = bal_problem->mutable_points();
for(int j = 0; j < num_points; ++j)
{
VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(j + num_cameras));
Eigen::Vector3d NewPointVec = pPoint->estimate();
memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
}
}
//this function is unused yet..
void SetMinimizerOptions(std::shared_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
//std::cout<<"Set Minimizer .."<< std::endl;
g2o::OptimizationAlgorithmWithHessian* solver;
if(params.trust_region_strategy == "levenberg_marquardt"){
solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr.get());
}
else if(params.trust_region_strategy == "dogleg"){
solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr.get());
}
else
{
std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
exit(EXIT_FAILURE);
}
optimizer->setAlgorithm(solver);
//std::cout<<"Set Minimizer .."<< std::endl;
}
//this function is unused yet..
void SetLinearSolver(std::shared_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params)
{
//std::cout<<"Set Linear Solver .."<< std::endl;
g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
if(params.linear_solver == "dense_schur" ){
linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
}
else if(params.linear_solver == "sparse_schur"){
linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true); // AMD ordering , only needed for sparse cholesky solver
}
solver_ptr = std::make_shared<BalBlockSolver>(linearSolver);
std::cout << "Set Complete.."<< std::endl;
}
//求解设置:使用哪种下降方式,使用哪类线性求解器
/***
* 设置求解选项,其实核心就是构建一个optimizer
* @param bal_problem 优化数据
* @param params 优化参数
* @param optimizer 稀疏优化器
*/
void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
BalBlockSolver* solver_ptr;
g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
//使用稠密计算方法
if(params.linear_solver == "dense_schur" ){
linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
}
else if(params.linear_solver == "sparse_schur"){
linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
//让solver对矩阵排序保持稀疏性
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true); // AMD ordering , only needed for sparse cholesky solver
}
//将线性求解器对象传入块求解器中,构造块求解器对象
solver_ptr = new BalBlockSolver(linearSolver);
//SetLinearSolver(solver_ptr, params);
//SetMinimizerOptions(solver_ptr, params, optimizer);
//将块求解器对象传入下降策略中,构造下降策略对象
g2o::OptimizationAlgorithmWithHessian* solver;
//根据参数选择是LM还是DL
if(params.trust_region_strategy == "levenberg_marquardt"){
solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
}
else if(params.trust_region_strategy == "dogleg"){
solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
}
else
{
//没有输入下降策略或者输入错误时,输出报警并退出
std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
exit(EXIT_FAILURE);
}
//将下降策略传入优化器的优化逻辑中,至此,一个优化器就构建好了
optimizer->setAlgorithm(solver);
}
/***
* 解决BAL问题
* 将.txt文件传入,最后将优化后的点云输出
*/
//开始优化,这个优化函数参数就是待优化文件和优化参数
void SolveProblem(const char* filename, const BundleParams& params)
{
BALProblem bal_problem(filename);
// show some information here ...
std::cout << "bal problem file loaded..." << std::endl;
//.num_cameras()返回num_cameras_ 值,显示相机数量
//.num_points()返回num_points_ 值,显示路标数量
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() << " observatoins. " << std::endl;
// store the initial 3D cloud points and camera pose..
if(!params.initial_ply.empty()){
//优化前将BALProblem类中的数据生成一下点云数据,因为优化后,这个类中的数据会被覆盖
bal_problem.WriteToPLYFile(params.initial_ply);
}
std::cout << "beginning problem..." << std::endl;
// add some noise for the intial value
srand(params.random_seed);
bal_problem.Normalize();
bal_problem.Perturb(params.rotation_sigma, params.translation_sigma,
params.point_sigma);
std::cout << "Normalization complete..." << std::endl;
// 创建一个稀疏优化器对象
g2o::SparseOptimizer optimizer;
//这里用到了上面的那个优化器的设置函数
SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
//设置完后,用BuildProblem()进行优化,参数也很清晰了:数据,优化器,参数
BuildProblem(&bal_problem, &optimizer, params);
std::cout << "begin optimizaiton .."<< std::endl;
// perform the optimizaiton
optimizer.initializeOptimization();
//输出优化信息
optimizer.setVerbose(true);
optimizer.optimize(params.num_iterations);
std::cout << "optimization complete.. "<< std::endl;
// write the optimized data into BALProblem class
WriteToBALProblem(&bal_problem, &optimizer);
// write the result into a .ply file.
if(!params.final_ply.empty()){
//优化后,将优化后的数据生成点云文件
bal_problem.WriteToPLYFile(params.final_ply);
}
}
int main(int argc, char** argv)
{
//在这里搞参数时就很简单了,因为BundleParams类中自带了BA用的所有参数,并且都有默认值,
//由argc,argv构造也是类构造函数决定的,需要读一下命令行中有没有用户自定义的参数值,有读进来将默认值覆盖
BundleParams params(argc,argv); // set the parameters here.
if(params.input.empty()){
std::cout << "Usage: bundle_adjuster -input <path for dataset>";
return 1;
}
// main()中直接调用SolveProblem()就好了,传入数据和优化参数
SolveProblem(params.input.c_str(), params);
//到这里看出一些设计思想,主函数中代码很少,其实要的就是这种效果,通过后台类的设计,在使用时,就传入待优化数据和需要的参数就能出结果
//任何功能模块在使用端都希望是这种用法,简单直接,不care函数内部实现逻辑,调用时一句话,传参出结果。
//函数黑箱内部的实现也是分块去实现,不同的功能写在不同的类中,类比函数好的一点是不光有功能,还能存储数据。
//设计思想值得借鉴,感觉已经是一个基本后端小框架了,不是之前仅为了练习写的demo。
return 0;
}
编译完成后得到两个文件一个优化前一个优化后
使用Meshlab打开.ply文件
优化前:
优化后: