// ceres 版本
1 #include
#include
#include

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
// 残差的计算
template
bool operator() (
const T* const abc, // 模型参数，有3维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理，而不是设3个变量，之后在()重载函数的形式参数个数变为3个
T* residual ) const // 残差
{
residual[] = T ( _y ) - ceres::exp ( abc[]*T ( _x ) *T ( _x ) + abc[]*T ( _x ) + abc[] ); // 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=; // 数据点
double w_sigma=1.0; // 噪声Sigma值(根号下方差)
cv::RNG rng; // OpenCV随机数产生器
double abc[] = {0.8,2.1,0.9}; // abc参数的估计值 (修改初始值 下面求解迭代过程会不同)

 vector<double> x\_data, y\_data;      // 数据

 /\*生成符合曲线的样本\*/  
 cout<<"generating data: "<<endl;   //下面是从真实的曲线中取得样本数据  
 for ( int i=; 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=; 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, , > (  
             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 ;  

}

g2o 版本（不太详细）

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //表示在利用Eigen库的数据结构时new的时候 需要对齐，所以加入EIGEN特有的宏定义即可实现
//下面几个虚函数都是覆盖了基类的对应同名同参数的函数
virtual void setToOriginImpl() // 重置 这个虚函数override 覆盖了Vertex类的对应函数 函数名字和参数都是一致的，是多态的本质
{
_estimate << ,,;//输入优化变量初始值
}

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<,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//自己添加explicit 防止隐式转换
explicit CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
/* _vertices是std::vector类型的变量，我们这里把基类指针_vertices【0】强制转换成const CurveFittingVertex* 自定义子类的常量指针
这里的转换是上行转换（子类指针转换到基类），对于static_cast 和dynamic_cast两种的结果都是一样的，但是对于这种下行转换则dynamic_cast比static_cast多了类型检查功能
更安全些，但是dynamic_cast只能用在类类型的指针 引用，static_cast则不限制,即可以用在类型也可以用在其他类型，所以这里应该更改为dynamic_cast
const CurveFittingVertex* v = static_cast (_vertices[0]);
*/
//修改后
const CurveFittingVertex* v = dynamic_cast (_vertices[]);
//获取此时待估计参数的当前更新值 为下面计算误差项做准备
const Eigen::Vector3d abc = v->estimate();
//这里的error是1x1的矩阵，因为误差项就是1个 _measurement是测量值yi
_error(,) = _measurement - std::exp( abc(,)*_x*_x + abc(,)*_x + abc(,) ) ;
}
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=; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[] = {,,}; // abc参数的估计值

vector<double> x\_data, y\_data;      // 数据

cout<<"generating data: "<<endl;  
for ( int i=; 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<,> > Block;  // 每个误差项优化变量维度为3，误差值维度为1后面的那个参数与误差变量无关 仅仅表示路标点的维度 这里因为没有用到路标点 所以为什么值都可以

/*
原版错误方式 : 这样会出错
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); // 线性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//LM法
*/

/*第一种解决方式: 将普通指针强制转换成智能指针 需要注意的是 转化之后 原来的普通指针指向的内容会有变化
普通指针可以强制转换成智能指针，方式是通过智能指针的一个构造函数来实现的， 比如下面的Block( std::unique_ptr( linearSolver ) );
这里面就是将linearSolver普通指针作为参数用智能指针构造一个临时的对象，此时原来的普通指针就无效了，一定不要再次用那个指针了，否则会有意想不到的错误，如果还想保留原来的指针
那么就可以利用第二种方式 定义的时候就直接用智能指针就好，但是就如第二种解决方案那样，也会遇到类型转换的问题。详细见第二种方式说明
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); // 线性方程求解器
Block* solver_ptr = new Block( std::unique_ptr( linearSolver ) ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr(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的参数，引起冲突，但是智能指针指向不同的类型时，
* 不能够通过强制转换，所以此时应该用一个std::move将一个solver_ptr变为右值，然后调用std::unique_ptr的移动构造函数，而这个函数的本身并没有限制指针
* 指向的类型，只要是std::unique_ptr类的对象，我们就可以调用智能指针的移动构造函数进行所属权的移动。
*
* */
std::unique_ptrlinearSolver( new g2o::LinearSolverDense() );// 线性方程求解器(1)
std::unique_ptr 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(,,) );//增加顶点的初始值，如果是位姿 则初始值是用ICP PNP来提供初始化值  
v->setId();//增加顶点标号 多个顶点要依次增加编号  
optimizer.addVertex( v );//将新增的顶点加入到图模型中

// 往图中增加边 N个  
for ( int i=; i<N; i++ )  
{  
    CurveFittingEdge\* edge = new CurveFittingEdge( x\_data\[i\] );  
    edge->setId(i);  
    edge->setVertex( , v );                // 设置连接的顶点  
    edge->setMeasurement( y\_data\[i\] );      // 观测数值 经过高斯噪声的  
    //这里的信息矩阵可以参考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有说明  
    edge->setInformation( Eigen::Matrix<double,,>::Identity()\*/(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();  
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 ;  

}

欢迎大家关注我的微信公众号「佛系师兄」，里面有关于 Ceres 以及 OpenCV 等更多技术文章。

比如

「反复研究好几遍，我才发现关于 CMake 变量还可以这样理解！」

更多好的文章会优先在里面不定期分享！打开微信客户端，扫描下方二维码即可关注！