ceres
// ceres 版本
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
// 残差的计算
template <typename T>
bool operator() (
const T* const abc, // 模型参数,有3维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理,而不是设3个变量,之后在()重载函数的形式参数个数变为3个
T* residual ) const // 残差
{
residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值(根号下方差)
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0.8,2.1,0.9}; // abc参数的估计值 (修改初始值 下面求解迭代过程会不同)
vector<double> x_data, y_data; // 数据
/*生成符合曲线的样本*/
cout<<"generating data: "<<endl; //下面是从真实的曲线中取得样本数据
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
//cout<<x_data[i]<<" "<<y_data[i]<<endl;//输出生成数据
}
// 构建最小二乘问题
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
/* 第一个参数 CostFunction* : 描述最小二乘的基本形式即代价函数 例如书上的116页fi(.)的形式
* 第二个参数 LossFunction* : 描述核函数的形式 例如书上的ρi(.)
* 第三个参数 double* : 待估计参数(用数组存储)
* 这里仅仅重载了三个参数的函数,如果上面的double abc[3]改为三个double a=0 ,b=0,c = 0;
* 此时AddResidualBlock函数的参数除了前面的CostFunction LossFunction 外后面就必须加上三个参数 分别输入&a,&b,&c
* 那么此时下面的 ceres::AutoDiffCostFunction<>模板参数就变为了 <CURVE_FITTING_COST,1,1,1,1>后面三个1代表有几类未知参数
* 我们修改为了a b c三个变量,所以这里代表了3类,之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中,
* 把形式参数变为了const T* const a, const T* const b, const T* const c ,T* residual
* 上面修改的方法与本例程实际上一样,只不过修改的这种方式显得乱,实际上我们在用的时候,一般都是残差种类有几个,那么后面的分类 就分几类
* 比如后面讲的重投影误差,此事就分两类 一类是相机9维变量,一类是点的3维变量,然而残差项变为了2维
*
* (1): 修改后的写法(当然自己定义的代价函数要对应修改重载函数的形式参数,对应修改内部的残差的计算):
* ceres::CostFunction* cost_function
* = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>(
* new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
* problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
* 修改后的代价函数的计算模型:
* struct CURVE_FITTING_COST
* {
* CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
* // 残差的计算
* template <typename T>
* bool operator() (
* const T* const a,
* const T* const b,
* const T* const c,
* T* residual ) const // 残差
* {
* residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
* return true;
* }
* const double _x, _y; // x,y数据
* };//代价类结束
*
*
* (2): 本例程下面的语句通常拆开来写(看起来方便些):
* ceres::CostFunction* cost_function
* = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
* new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
* problem.AddResidualBlock(cost_function,nullptr,abc)
* */
problem.AddResidualBlock ( // 向问题中添加误差项
// 使用自动求导,模板参数:误差类型,Dimension of residual(输出维度 表示有几类残差,本例程中就一类残差项目,所以为1),输入维度,维数要与前面struct中一致
/*这里1 代表*/
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表输入的获得的试验数据
),
nullptr, // 核函数,这里不使用,为空 这里是LossFunction的位置
abc // 待估计参数3维
);
}
// 配置求解器ceres::Solver (是一个非线性最小二乘的求解器)
ceres::Solver::Options options; // 这里有很多配置项可以填Options类嵌入在Solver类中 ,在Options类中可以设置关于求解器的参数
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
options.minimizer_progress_to_stdout = true; // 为真时 内部错误输出到cout,我们可以看到错误的地方,默认情况下,会输出到日志文件中保存
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//记录求解时间间隔
//cout<<endl<<"求解前....."<<endl;
/*下面函数需要3个参数:
* 1、 const Solver::Options& options <----> optione
* 2、 Problem* problem <----> &problem
* 3、 Solver::Summary* summary <----> &summart (即使默认的参数也需要定义该变量 )
* 这个函数会输出一些迭代的信息。
* */
ceres::Solve ( options, &problem, &summary ); // 开始优化
//cout<<endl<<"求解后....."<<endl;
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 输出结果
// BriefReport() : A brief one line description of the state of the solver after termination.
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
/*auto a:abc 或者下面的方式都可以*/
for ( auto &a:abc ) cout<<a<<" ";
cout<<endl;
return 0;
}
g20
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <memory>
using namespace std;
// 曲线模型的顶点,模板参数:优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //表示在利用Eigen库的数据结构时new的时候 需要对齐,所以加入EIGEN特有的宏定义即可实现
//下面几个虚函数都是覆盖了基类的对应同名同参数的函数
virtual void setToOriginImpl() // 重置 这个虚函数override 覆盖了Vertex类的对应函数 函数名字和参数都是一致的,是多态的本质
{
_estimate << 0,0,0;//输入优化变量初始值
}
virtual void oplusImpl( const double* update ) // 更新 对于拟合曲线这种问题,这里更新优化变量仅仅是简单的加法,
// 但是到了位姿优化的时候,旋转矩阵更新是左乘一个矩阵 此时这个更新函数就必须要重写了
{ //更新参数估计值
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘:留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
// 误差模型 模板参数:观测值维度,类型,连接顶点类型 //这里观测值维度是1维,如果是108页6.12式,则观测值维度是2
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//自己添加explicit 防止隐式转换
explicit CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
/* _vertices是std::vector<Vertex *>类型的变量,我们这里把基类指针_vertices【0】强制转换成const CurveFittingVertex* 自定义子类的常量指针
这里的转换是上行转换(子类指针转换到基类),对于static_cast 和dynamic_cast两种的结果都是一样的,但是对于这种下行转换则dynamic_cast比static_cast多了类型检查功能
更安全些,但是dynamic_cast只能用在类类型的指针 引用,static_cast则不限制,即可以用在类型也可以用在其他类型,所以这里应该更改为dynamic_cast
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
*/
//修改后
const CurveFittingVertex* v = dynamic_cast<const CurveFittingVertex*> (_vertices[0]);
//获取此时待估计参数的当前更新值 为下面计算误差项做准备
const Eigen::Vector3d abc = v->estimate();
//这里的error是1x1的矩阵,因为误差项就是1个 _measurement是测量值yi
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值, y 值为 _measurement
};
int main( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
// cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 构建图优化,先设定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,3> > Block; // 每个误差项优化变量维度为3,误差值维度为1后面的那个参数与误差变量无关 仅仅表示路标点的维度 这里因为没有用到路标点 所以为什么值都可以
/*
原版错误方式 : 这样会出错
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//LM法
*/
/*第一种解决方式: 将普通指针强制转换成智能指针 需要注意的是 转化之后 原来的普通指针指向的内容会有变化
普通指针可以强制转换成智能指针,方式是通过智能指针的一个构造函数来实现的, 比如下面的Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) );
这里面就是将linearSolver普通指针作为参数用智能指针构造一个临时的对象,此时原来的普通指针就无效了,一定不要再次用那个指针了,否则会有意想不到的错误,如果还想保留原来的指针
那么就可以利用第二种方式 定义的时候就直接用智能指针就好,但是就如第二种解决方案那样,也会遇到类型转换的问题。详细见第二种方式说明
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<g2o::Solver>(solver_ptr) );//LM法
*/
/*第二种解决方案: 定义变量时就用智能指针 需要注意的是 需要std::move移动
*下面可以这样做 std::make_unique<>是在c++14中引进的 而std::make_shared<>是在c++11中引进的,都是为了解决用new为智能指针赋值的操作。这种更安全。
* 对于(2)将linearSovler智能指针的资源利用移动构造函数转移到新建立的Block中,此时linearSolver这个智能指针默认不能够访问以及使用了。
* 对于(3)来说,因为solver_ptr是一个指向Block类型的智能指针,但是g2o::OptimizationAlgorithmLevenberg 构造函数接受的是std::unique_ptr<Solver>的参数,引起冲突,但是智能指针指向不同的类型时,
* 不能够通过强制转换,所以此时应该用一个std::move将一个solver_ptr变为右值,然后调用std::unique_ptr的移动构造函数,而这个函数的本身并没有限制指针
* 指向的类型,只要是std::unique_ptr类的对象,我们就可以调用智能指针的移动构造函数进行所属权的移动。
*
* */
std::unique_ptr<Block::LinearSolverType>linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>() );// 线性方程求解器(1)
std::unique_ptr<Block> solver_ptr ( new Block( std::move(linearSolver) ) );// 矩阵块求解器 (2)
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );//(3) LM法
// 梯度下降方法,从GN, LM, DogLeg 中选(下面的两种方式要按照上面的两种解决方案对应修改,否则会编译出错 )
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr) );
//g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::move(solver_ptr) );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );//增加顶点的初始值,如果是位姿 则初始值是用ICP PNP来提供初始化值
v->setId(0);//增加顶点标号 多个顶点要依次增加编号
optimizer.addVertex( v );//将新增的顶点加入到图模型中
// 往图中增加边 N个
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值 经过高斯噪声的
//这里的信息矩阵可以参考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有说明
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆 这里为1表示加权为1
optimizer.addEdge( edge );
}
// 执行优化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
return 0;
}