gaussNewton.cpp
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>//Eigen核心模块
#include <Eigen/Dense>//Eigen稠密矩阵运算模块
using namespace std;
using namespace Eigen;
//高斯牛顿法拟合曲线y = exp(a * x^2 + b * x + c)
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值,并赋初始值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // double数据x_data, y_data
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));//rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
}
// 开始Gauss-Newton迭代 求ae，be和ce的值，使得代价最小
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost cost表示本次迭代的代价，lastCost表示上次迭代的代价
//cost = error * error，error表示测量方程的残差
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//std::chrono是c++11引入的日期处理库，其中包含三种时钟（system_clock,steady_clock,high_resolution_clock）
//t1表示steady_clock::time_point类型
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton 将矩阵H初始化为3*3零矩阵，表示海塞矩阵，H = J * (sigma * sigma).transpose() * J.transpose()
//（视觉slam十四讲p133式6.41左边）
Vector3d b = Vector3d::Zero(); // bias 将b初始化为3*1零向量，b = -J * (sigma * sigma).transpose() * error，error表示测量方程的残差（视觉slam十四讲p133式6.41右边）
cost = 0;
//遍历所有数据点计算H，b和cost
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = yi - exp(ae * xi * xi + be * xi + ce);//视觉slam十四讲p133式6.39
Vector3d J; // 雅可比矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da 视觉slam十四讲p133式6.40 第一个
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db 视觉slam十四讲p133式6.40 第二个
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc 视觉slam十四讲p133式6.40 第三个
H += inv_sigma * inv_sigma * J * J.transpose();//视觉slam十四讲p133式6.41左边 求和
b += -inv_sigma * inv_sigma * error * J;//视觉slam十四讲p133式6.41右边 求和
cost += error * error;//残差平方和
}
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b); //ldlt()表示利用Cholesky分解求dx
if (isnan(dx[0]))//isnan()函数判断输入是否为非数字，是非数字返回真，nan全称为not a number
{
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) //因为iter要大于0，第1次迭代(iter = 0, cost > lastCost)不执行！
{
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
//更新优化变量ae，be和ce！
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost; //更新上一时刻代价
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated params: " << ae << "," << be << "," << ce << 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;
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch6)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")
add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})
add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})
add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})执行结果：
total cost: 3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated params: 2.04558,-0.921836,4.01467
total cost: 376785, update: 0.065762 0.224972 -0.962521 estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6, update: -0.0670241 0.617616 -0.907497 estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01, update: -0.522767 1.19192 -0.756452 estimated params: 1.52155,1.11267,1.3882
total cost: 174.853, update: -0.537502 0.909933 -0.386395 estimated params: 0.984045,2.0226,1.00181
total cost: 102.78, update: -0.0919666 0.147331 -0.0573675 estimated params: 0.892079,2.16994,0.944438
total cost: 101.937, update: -0.00117081 0.00196749 -0.00081055 estimated params: 0.890908,2.1719,0.943628
total cost: 101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated params: 0.890912,2.1719,0.943629
total cost: 101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00112835 seconds.
estimated abc = 0.890912, 2.1719, 0.943629
ceresCurveFitting.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>//ceres库头文件
#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维
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) residual表示残差
return true;
//返回bool类型，计算结果已经存入函数外的residual变量中
}
const double _x, _y; // x,y数据 结构体CURVE_FITTING_COST中的成员变量
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值 初始化为1
double inv_sigma = 1.0 / w_sigma; //标准差的逆
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // 数据 // double数据x_data, y_data
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值 所给的100个观测点数据
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
//rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
}
double abc[3] = {ae, be, ce};//定义优化变量
// 构建最小二乘问题
ceres::Problem problem;//定义一个优化问题类problem
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数，这里不使用，为空 添加损失函数(即鲁棒核函数)，这里不使用，为空
abc // 待估计参数 优化变量，3维数组
);
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填 定义一个配置项集合类options
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
// 增量方程如何求解 增量方程求解方式，本质上是稠密矩阵求逆的加速方法选择
options.minimizer_progress_to_stdout = true;
// 输出到cout minimizer_progress_to_stdout表示是否向终端输出优化过程信息
ceres::Solver::Summary summary; // 优化信息 利用ceres执行优化
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
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;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";//输出估计值
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
CMakeLists.txt 和上面是一样的。
执行结果：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 4.82e-05 1.25e-04
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 7.30e-05 2.73e-04
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 2.50e-05 3.11e-04
3 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 2.41e-05 3.48e-04
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 2.38e-05 3.81e-04
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 2.41e-05 4.15e-04
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 2.38e-05 4.48e-04
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 2.41e-05 4.81e-04
solve time cost = 0.00053221 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890908 2.1719 0.943628
g2oCurveFitting.cpp
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_unary_edge.h>//g2o边（edge）头文件
#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>//dogleg算法头文件
#include <g2o/solvers/dense/linear_solver_dense.h>//稠密矩阵求解
#include <Eigen/Core>//Eigen核心模块
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> //:表示继承，public表示公有继承；CurveFittingVertex是派生类，BaseVertex<3, Eigen::Vector3d>是基类
{
public://以下定义的成员变量和成员函数都是公有的
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
// 重置
virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
{
_estimate << 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override
{
_estimate += Eigen::Vector3d(update);//更新量累加
}
// 存盘和读盘：留空
virtual bool read(istream &in) {} //istream类是c++标准输入流的一个基类
//可参照C++ Primer Plus第六版的6.8节
virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
//可参照C++ Primer Plus第六版的6.8节
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}//使用列表赋初值
// 计算曲线模型误差
virtual void computeError() override //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);//创建指针v
const Eigen::Vector3d abc = v->estimate();//将estimate()值赋给abc
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));//视觉slam十四讲p133式6.39
}
// 计算雅可比矩阵
virtual void linearizeOplus() override //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);//视觉slam十四讲p133式6.38上面的式子
_jacobianOplusXi[0] = -_x * _x * y;//视觉slam十四讲p133式6.40第一个
_jacobianOplusXi[1] = -_x * y;//视觉slam十四讲p133式6.40第二个
_jacobianOplusXi[2] = -y;//视觉slam十四讲p133式6.40第三个
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma; //标准差的逆
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // double型数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值 所给的100个观测点数据
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
//rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
}
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
//c++中的make_unique表示智能指针类型，而g2o中的make_unique表示
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();//指针v
v->setEstimate(Eigen::Vector3d(ae, be, ce)); //ae、be和ce表示优化变量
v->setId(0);//对顶点进行编号，里面的0你可以写成任意的正整数，但是后面设置edge连接顶点时，必须要和这个一致
optimizer.addVertex(v);//添加顶点
// 往图中增加边
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);//对边进行编号
edge->setVertex(0, v); // 设置连接的顶点
//edge->setVertex(0, v); 其中的0为该边连接的第一个顶点，即前面编号为0的顶点v，因为是一元边，因此只需这一句。
//根据函数公式：y=exp(ax^2+bx+c)+w可知，y为观测值，故有edge->setMeasurement(y_data[i])。图构建出之后，即可进行迭代求解。
edge->setMeasurement(y_data[i]); // 观测数值
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge);//添加边
}
// 执行优化
cout << "start optimization" << endl;//输出start optimization
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization(); //优化过程初始化
optimizer.optimize(10);//设置优化的迭代次数
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;
}
CMakeLists.txt 和上面是一样的。
执行结果：
start optimization
iteration= 0 chi2= 376785.128234 time= 7.826e-05 cumTime= 7.826e-05 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 6.3717e-05 cumTime= 0.000141977 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 1.4645e-05 cumTime= 0.000156622 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 1.5095e-05 cumTime= 0.000171717 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 1.4614e-05 cumTime= 0.000186331 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 1.4855e-05 cumTime= 0.000201186 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 1.4874e-05 cumTime= 0.00021606 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 1.4875e-05 cumTime= 0.000230935 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 1.4564e-05 cumTime= 0.000245499 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 1.4535e-05 cumTime= 0.000260034 edges= 100 schur= 0
solve time cost = 0.00184401 seconds.
estimated model: 0.890912 2.1719 0.943629
课后习题：
1. 证明线性方程Ax = b 当系数矩阵A 超定时，最小二乘解为
。
2. 调研最速下降法、牛顿法、GN 和LM 各有什么优缺点。除了我们举的Ceres 库和g2o 库，还有哪些常用的优化库？你可能会找到一些MATLAB 上的库。
Ipopt:
Ipopt
https://github.com/coin-or/Ipopt
SLAM++:
【论文阅读1】SLAM++: SLAM at the Level of Objects - 知乎去年读了不少论文，但是感觉还是记录下来印象更深刻一些。就在知乎记录吧，内容基本都是是翻译过来的，由于时间有限，没有仔细润色，可能不太通顺，以后再慢慢改。 这里记录第一篇文章： R. F. Salas-Moreno, R. A…
https://zhuanlan.zhihu.com/p/102562833SLAM++ 增量式BA优化库_Roby-CSDN博客incSLAM++是3DV 2017最佳论文，源文件网址 https://sourceforge.net/p/slam-plus-plus/wiki/Home/ incSLAM++有两个创新点：一是增量式Schur补更新。如果delta更新是稀疏的，则采用增量式Schur补来求解非线性最小二乘问题，即增量式BA问题，会缩短计算时间。求解不同关键帧之间的位姿和每帧上的点即求解增量式BA（解不是全局的...
https://blog.csdn.net/houlianfeng/article/details/79993162
liblbfgs:
Optimization Toolbox:
Optimization ToolboxDocumentationhttps://www.mathworks.com/help/optim/index.html?s_tid=CRUX_lftnav
Nlopt：Overview - NLopt Documentation
Ubuntu下面安装：
git clone git://github.com/stevengj/nlopt
cd nloptmkdir build
cd buildcmake ..makesudo make install
详细情况可以阅读readme文件：
大家可以参考这些文章：
3. 为什么GN 的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？
转载于：
4. DogLeg 是什么？它与GN 和LM 有何异同？请搜索相关的材料。
参考下面的文章：
5. 阅读Ceres 的教学材料以更好地掌握它的用法。
*************************************************************************
6. 阅读g2o 自带的文档，你能看懂它吗？如果还不能完全看懂，请在第10、11两讲之后回来再看。
*************************************************************************
7.* 请更改曲线拟合实验中的曲线模型，并用Ceres 和g2o 进行优化实验。例如，你可以使用更多的参数和更复杂的模型。
转载于：
视觉slam十四讲ch6曲线拟合 代码注释（笔记版） - 灰色的石头 - 博客园
我加了些新的注释。
ceresCurveFitting1.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>//ceres库头文件
#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) residual表示残差
return true; //返回bool类型，计算结果已经存入函数外的residual变量中
}
const double _x, _y; // x,y数据
};
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
double abc[3] = {0.8,2.1,0.9}; // abc参数的估计值 (修改初始值 下面求解迭代过程会不同)
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值(根号下方差)
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // 数据
/*生成符合曲线的样本*/
cout<<"generating data: "<<endl; //下面是从真实的曲线中取得样本数据
for ( int i=0; i<N; i++ )
{
double x = i/100.0;//相当于x范围是0-1
x_data.push_back ( x );//x_data存储的数值 所给的100个观测点数据
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
//rng.gaussian(w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
//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
* 我们修改为了a b c三个变量，所以这里代表了3类，之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中，
* 上面修改的方法与本例程实际上一样，只不过修改的这种方式显得乱，实际上我们在用的时候，一般都是残差种类有几个，那么后面的分类 就分几类
*
* （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);
* CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
* // 残差的计算
* template <typename T>
* bool operator() (
* const T* const a,
* const T* const b,
* 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数据
* };//代价类结束
*
*
* 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,// 核函数，这里不使用，为空 添加损失函数(即鲁棒核函数)
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;
}CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch6)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")
add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})
add_executable(ceresCurveFitting1 ceresCurveFitting1.cpp)
target_link_libraries(ceresCurveFitting1 ${OpenCV_LIBS} ${CERES_LIBRARIES})
add_executable(g2oCurveFitting1 g2oCurveFitting1.cpp)
target_link_libraries(g2oCurveFitting1 ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})执行结果：
generating data:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 5.551800e+02 0.00e+00 5.20e+03 0.00e+00 0.00e+00 1.00e+04 0 5.20e-05 1.28e-04
1 5.678997e+01 4.98e+02 6.58e+02 1.44e-01 9.88e-01 3.00e+04 1 6.39e-05 2.27e-04
2 5.096903e+01 5.82e+00 5.68e+00 3.34e-02 1.00e+00 9.00e+04 1 2.88e-05 2.63e-04
3 5.096851e+01 5.15e-04 6.48e-04 1.63e-03 9.99e-01 2.70e+05 1 2.62e-05 2.95e-04
solve time cost = 0.000339815 seconds.
Ceres Solver Report: Iterations: 4, Initial cost: 5.551800e+02, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890919 2.17189 0.943633
g2oCurveFitting1.cpp
#include <iostream>
#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_unary_edge.h>//g2o边（edge）头文件
#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>//dogleg算法头文件
#include <g2o/solvers/dense/linear_solver_dense.h>//稠密矩阵求解
#include <Eigen/Core>//Eigen核心模块
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <memory>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
//:表示继承，public表示公有继承；CurveFittingVertex是派生类，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 ) {} //istream类是c++标准输入流的一个基类
//可参照C++ Primer Plus第六版的6.8节
virtual bool write( ostream& out ) const {}//ostream类是c++标准输出流的一个基类
//可参照C++ Primer Plus第六版的6.8节
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型 //这里观测值维度是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; // 真实参数值
double abc[3] = {0,0,0}; // abc参数的估计值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;//相当于x范围是0-1
x_data.push_back ( x );//x_data存储的数值 所给的100个观测点数据
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
//rng.gaussian(w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
// 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);//增加顶点标号 多个顶点要依次增加编号
//对顶点进行编号，里面的0你可以写成任意的正整数，但是后面设置edge连接顶点时，必须要和这个一致
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;//输出start optimization
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;
}CMakeLists.txt 和上面一样。
执行结果：
generating data:
start optimization
iteration= 0 chi2= 30373.727656 time= 0.000163797 cumTime= 0.000163797 edges= 100 schur= 0 lambda= 699.050482 levenbergIter= 7
iteration= 1 chi2= 13336.948287 time= 0.00011325 cumTime= 0.000277047 edges= 100 schur= 0 lambda= 1864.134619 levenbergIter= 3
iteration= 2 chi2= 6946.262238 time= 3.5609e-05 cumTime= 0.000312656 edges= 100 schur= 0 lambda= 1242.756412 levenbergIter= 1
iteration= 3 chi2= 271.023143 time= 2.8986e-05 cumTime= 0.000341642 edges= 100 schur= 0 lambda= 414.252137 levenbergIter= 1
iteration= 4 chi2= 118.903888 time= 2.8816e-05 cumTime= 0.000370458 edges= 100 schur= 0 lambda= 138.084046 levenbergIter= 1
iteration= 5 chi2= 113.568661 time= 2.9216e-05 cumTime= 0.000399674 edges= 100 schur= 0 lambda= 46.028015 levenbergIter= 1
iteration= 6 chi2= 107.476468 time= 2.9187e-05 cumTime= 0.000428861 edges= 100 schur= 0 lambda= 15.342672 levenbergIter= 1
iteration= 7 chi2= 103.014521 time= 2.9116e-05 cumTime= 0.000457977 edges= 100 schur= 0 lambda= 5.114224 levenbergIter= 1
iteration= 8 chi2= 101.988349 time= 2.8866e-05 cumTime= 0.000486843 edges= 100 schur= 0 lambda= 1.704741 levenbergIter= 1
iteration= 9 chi2= 101.937388 time= 2.8896e-05 cumTime= 0.000515739 edges= 100 schur= 0 lambda= 0.568247 levenbergIter= 1
iteration= 10 chi2= 101.937021 time= 2.8897e-05 cumTime= 0.000544636 edges= 100 schur= 0 lambda= 0.378831 levenbergIter= 1
iteration= 11 chi2= 101.937020 time= 2.8976e-05 cumTime= 0.000573612 edges= 100 schur= 0 lambda= 0.252554 levenbergIter= 1
iteration= 12 chi2= 101.937020 time= 2.8765e-05 cumTime= 0.000602377 edges= 100 schur= 0 lambda= 0.168370 levenbergIter= 1
iteration= 13 chi2= 101.937020 time= 2.7784e-05 cumTime= 0.000630161 edges= 100 schur= 0 lambda= 0.112246 levenbergIter= 1
iteration= 14 chi2= 101.937020 time= 3.2503e-05 cumTime= 0.000662664 edges= 100 schur= 0 lambda= 0.074831 levenbergIter= 1
iteration= 15 chi2= 101.937020 time= 4.9056e-05 cumTime= 0.00071172 edges= 100 schur= 0 lambda= 13391510.122618 levenbergIter= 8
iteration= 16 chi2= 101.937020 time= 3.4597e-05 cumTime= 0.000746317 edges= 100 schur= 0 lambda= 857056647.847525 levenbergIter= 3
solve time cost = 0.00753176 seconds.
estimated model: 0.890912 2.1719 0.943629
转载于：视觉SLAM十四讲（第二版）第6讲习题解答 - 知乎
ceresCurveFitting2.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>/ceres库头文件
#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, // 模型参数，有4维
T *residual) const//重载运算符()
{
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2])- abc[3] * T(_x) ;//y-exp(ax^2+bx+c)-dx)
return true; //返回bool类型，计算结果已经存入函数外的residual变量中
}
const double _x, _y; // x,y数据
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0, dr=2.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0, de=-1.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma; //标准差的逆
cv::RNG rng; // OpenCV随机数产生器 RNG为OpenCV中生成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // 数据 double数据x_data, y_data
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值 所给的100个观测点数据
y_data.push_back(exp(ar * x * x + br * x + cr) + dr * x + rng.gaussian(w_sigma * w_sigma));
//rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
}
double abc[4] = {ae, be, ce, de};//定义优化变量
// 构建最小二乘问题
ceres::Problem problem;//定义一个优化问题类problem
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 4>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数，这里不使用，为空 添加损失函数(即鲁棒核函数)，这里不使用，为空
abc // 待估计参数 优化变量，4维数组
);
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填 定义一个配置项集合类options
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
// 增量方程如何求解 增量方程求解方式，本质上是稠密矩阵求逆的加速方法选择
options.minimizer_progress_to_stdout = true;
// 输出到cout minimizer_progress_to_stdout表示是否向终端输出优化过程信息
ceres::Solver::Summary summary; // 优化信息 利用ceres执行优化
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
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;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c,d= "; //输出估计值 a,b,c,d
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch6)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")
add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})
add_executable(ceresCurveFitting2 ceresCurveFitting2.cpp)
target_link_libraries(ceresCurveFitting2 ${OpenCV_LIBS} ${CERES_LIBRARIES})
add_executable(g2oCurveFitting2 g2oCurveFitting2.cpp)
target_link_libraries(g2oCurveFitting2 ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})执行结果：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.568785e+06 0.00e+00 3.49e+06 0.00e+00 0.00e+00 1.00e+04 0 8.99e-05 2.38e-04
1 1.990794e+05 1.37e+06 4.93e+05 8.33e+00 8.73e-01 1.71e+04 1 1.44e-04 5.17e-04
2 1.974116e+04 1.79e+05 6.84e+04 1.09e+00 9.01e-01 3.54e+04 1 4.60e-05 5.87e-04
3 7.916989e+02 1.89e+04 6.65e+03 6.94e+00 9.63e-01 1.06e+05 1 4.41e-05 6.47e-04
4 6.835090e+01 7.23e+02 1.97e+02 4.28e+00 9.77e-01 3.18e+05 1 4.29e-05 7.07e-04
5 5.525052e+01 1.31e+01 4.46e+02 5.62e+00 7.54e-01 3.66e+05 1 3.81e-05 7.59e-04
6 5.097074e+01 4.28e+00 4.68e+00 1.85e-01 1.00e+00 1.10e+06 1 3.81e-05 8.12e-04
7 5.096847e+01 2.27e-03 1.58e-02 2.53e-02 1.00e+00 3.30e+06 1 4.51e-05 8.73e-04
solve time cost = 0.000960992 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.568785e+06, Final cost: 5.096847e+01, Termination: CONVERGENCE
estimated a,b,c,d= 0.889633 2.17262 0.944485 1.98114 g2oCurveFitting2.cpp
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_unary_edge.h>//g2o边（edge）头文件
#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>//dogleg算法头文件
#include <g2o/solvers/dense/linear_solver_dense.h>//稠密矩阵求解
#include <Eigen/Core>//Eigen核心模块
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<4, Eigen::Vector4d> //:表示继承，public表示公有继承；CurveFittingVertex是派生类，BaseVertex<3, Eigen::Vector4d>是基类
{
public://以下定义的成员变量和成员函数都是公有的
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
{
_estimate << 0, 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector4d(update);//更新量累加
}
// 存盘和读盘：留空
virtual bool read(istream &in) {}//istream类是c++标准输入流的一个基类
//可参照C++ Primer Plus第六版的6.8节
virtual bool write(ostream &out) const {}//ostream类是c++标准输出流的一个基类
//可参照C++ Primer Plus第六版的6.8节
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}//使用列表赋初值
// 计算曲线模型误差
virtual void computeError() override//virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);//创建指针v
const Eigen::Vector4d abc = v->estimate();//将estimate()值赋给abc
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0)) - abc(3, 0) * _x;
//视觉slam十四讲p133式6.39
}
// 计算雅可比矩阵
virtual void linearizeOplus() override //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector4d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]) + abc[3] * _x;//视觉slam十四讲p133式6.38上面的式子
_jacobianOplusXi[0] = -_x * _x * y;//视觉slam十四讲p133式6.40第一个
_jacobianOplusXi[1] = -_x * y;//视觉slam十四讲p133式6.40第二个
_jacobianOplusXi[2] = -y;//视觉slam十四讲p133式6.40第三个
_jacobianOplusXi[3] = -_x;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0, dr=2.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0, de=-1.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma; //标准差的逆
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值 所给的100个观测点数据
y_data.push_back(exp(ar * x * x + br * x + cr) + dr * x + rng.gaussian(w_sigma * w_sigma));
//rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
//rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数 视觉slam十四讲p133式6.38上面的表达式
}
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<4, 1>> BlockSolverType; // 每个误差项优化变量维度为4，误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
//c++中的make_unique表示智能指针类型，而g2o中的make_unique表示
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();//指针v
v->setEstimate(Eigen::Vector4d(ae, be, ce, de));//ae、be、ce、de表示优化变量
v->setId(0);//对顶点进行编号，里面的0你可以写成任意的正整数，但是后面设置edge连接顶点时，必须要和这个一致
optimizer.addVertex(v);//添加顶点
// 往图中增加边
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);//对边进行编号
edge->setVertex(0, v); // 设置连接的顶点
//edge->setVertex(0, v); 其中的0为该边连接的第一个顶点，即前面编号为0的顶点v，因为是一元边，因此只需这一句。
//根据函数公式：y=exp(ax^2+bx+c)+w可知，y为观测值，故有edge->setMeasurement(y_data[i])。图构建出之后，即可进行迭代求解
edge->setMeasurement(y_data[i]); // 观测数值
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge);//添加边
}
// 执行优化
cout << "start optimization" << endl;//输出start optimization
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();//优化过程初始化
optimizer.optimize(10);//设置优化的迭代次数
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::Vector4d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
}
执行结果：
start optimization
iteration= 0 chi2= 395400.237834 time= 5.0745e-05 cumTime= 5.0745e-05 edges= 100 schur= 0
iteration= 1 chi2= 46668.610919 time= 3.4935e-05 cumTime= 8.568e-05 edges= 100 schur= 0
iteration= 2 chi2= 1735.210242 time= 8.606e-06 cumTime= 9.4286e-05 edges= 100 schur= 0
iteration= 3 chi2= 135.874544 time= 8.396e-06 cumTime= 0.000102682 edges= 100 schur= 0
iteration= 4 chi2= 109.507970 time= 8.466e-06 cumTime= 0.000111148 edges= 100 schur= 0
iteration= 5 chi2= 101.983966 time= 8.415e-06 cumTime= 0.000119563 edges= 100 schur= 0
iteration= 6 chi2= 101.937085 time= 8.386e-06 cumTime= 0.000127949 edges= 100 schur= 0
iteration= 7 chi2= 101.936983 time= 8.426e-06 cumTime= 0.000136375 edges= 100 schur= 0
iteration= 8 chi2= 101.936980 time= 8.346e-06 cumTime= 0.000144721 edges= 100 schur= 0
iteration= 9 chi2= 101.936980 time= 8.256e-06 cumTime= 0.000152977 edges= 100 schur= 0
solve time cost = 0.00130745 seconds.
estimated model: 0.889889 2.17192 0.945022 1.9763
扫一扫
1079
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
