视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化
Chapter6 非线性优化
- 实践：Ceres
- 1. 安装Ceres依赖库
- 2. 编译安装Ceres
- 3. 曲线拟合问题描述
- 4. ceres使用方法
- 5.完整代码
- 实践：g2o
- 1.安装g2o依赖库
- 2.编译安装g2o
- 3. g2o使用方法
- 4. 完整代码
实践：Ceres
1. 安装Ceres依赖库
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev
出现找不到package的错误：
解决办法：
添加源，用下面命令打开source.list
sudo gedit /etc/apt/sources.list
在第一行添加：
deb http://cz.archive.ubuntu.com/ubuntu trusty main universe
然后更新一下：
sudo apt-get update
再执行Ceres依赖库的安装就不会报错啦
2. 编译安装Ceres
github上下载Ceres源码，下载地址为：https://github.com/ceres-solver
使用cmake编译它并安装到机器上，安装完成之后，在/usr/local/include/ceres/下可以找到Ceres的头文件，在/usr/local/lib/下可以找到名为libceres.a的库文件。
make过程出现错误：
error: no type named ‘Literal’ in ‘struct Eigen::NumTraits
这是因为eigen3与ceres发生了冲突。
解决办法：
下载与书中提供的ceres版本兼容的eigen，与书中提供的ceres版本兼容的eigen下载
编译并且安装它，然后再编译ceres工程就可以顺利通过啦
3. 曲线拟合问题描述
假设有一条满足以下方程的曲线：y=exp(ax2+bx+c)+w
其中，a,b,c为曲线的参数，w为高斯噪声。
为了模拟实际的曲线拟合问题，我们假设曲线参数a,b,c的真值，然后引入高斯噪声w生成观测x和y
我们在非线性优化问题中会首先给待估计变量a,b,c一个初始值，然后把观测(x,y)输入导ceres中用ceres拟合这条曲线。
4. ceres使用方法
（1）定义Cost Function模型。 方法是书写一个类。并在类中定义带模板的( )运算符，这样该类就成为了一个拟函数。这种定义方式使得ceres可以像调用函数一样，对该类的某个对象(比如 a)调用a( )方法，这使对象具有像函数那样的行为。
//代价函数的计算模型
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST(double x, double y):_x(x),_y(y) {}
//残差的计算
template
bool operator() (
const T* const abc, //模型参数，有3维
T* residual) const //残差
{
//y-exp(ax^2+bx+c)
residual[0] = T(_y) -ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) +abc[2]);
return true;
}
const double _x,_y;//x,y数据
};
（2）构建最小二乘问题
定义一个问题对象：
ceres::Problem problem;
调用AddResidualBlock将误差项添加导目标函数中，残差block需要配置求导方式，核函数以及待估计的参数。
其中求导方式有a.使用ceres的自动求导(Auto Diff); b.使用数值求导(Numeric Diff); c.自行推导解析的导数形式，提供给ceres
自动求导需要设定误差项和优化变量的维度。这里误差项是我们定义的CURVE_FITTING_COST类；误差是标量，维度为1； 优化变量为abc，维度为3；不使用核函数；待估计参数为abc
//构建最小二乘问题
ceres::Problem problem;
for(int i=0;i(new CURVE_FITTING_COST(x_data[i],y_data[i])),
nullptr, //核函数，这里不使用，为空
abc //待估计参数
);
}
（3）配置求解器
求解选项配置：
ceres::Solver::Options options;//这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR;// 增量方程如何求解
options.minimizer_progress_to_stdout = true;//输出到cout
优化信息对象的定义：
ceres::Solver::Summary summary;
（4）对问题进行优化
ceres::Solve (options,&problem,&summary);//开始优化
优化信息对象输出结果：
cout<最后优化的结果放在abc变量中
5.完整代码
ceres_curve_fitting.cpp
//2019.08.08
#include
#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维
T* residual) const //残差
{
//y-exp(ax^2+bx+c)
residual[0] = T(_y) -ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) +abc[2]);
return true;
}
const double _x,_y;//x,y数据
};
int main()
{
double a=1, 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参数的估计值,初始值为0,0,0
vector x_data,y_data; //观测数据
cout<<"generating data: "<(new CURVE_FITTING_COST(x_data[i],y_data[i])),
nullptr, //核函数，这里不使用，为空
abc //待估计参数
);
}
//配置求解器
ceres::Solver::Options options;//这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR;// 增量方程如何求解
options.minimizer_progress_to_stdout = true;//输出到cout
ceres::Solver::Summary summary; //优化信息
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 time_used = chrono::duration_cast>(t2-t1);
cout<<"solve time cost = "< CMakeLists.txt
#2019.08.08-08.09
# 添加C++标准文件
set(CMAKE_CXX_FLAGS "-std=c++11")
#寻找opencv库
find_package(OpenCV REQUIRED)
#寻找ceres库
find_package(Ceres REQUIRED)
#添加Eigen的头文件
include_directories("/usr/include/eigen3")
#添加Opencv的头文件
include_directories( ${OpenCV_INCLUDE_DIRS})
#添加ceres的头文件，下面两种方法等价
#include_directories(${CERES_INCLUDE_DIRS})
include_directories("/usr/local/include/ceres/")
add_executable(ceres_curve_fitting ceres_curve_fitting.cpp)
#链接OpenCV库
target_link_libraries(ceres_curve_fitting ${OpenCV_LIBS} )
#链接ceres库，以下两种方法不等价
target_link_libraries(ceres_curve_fitting ${CERES_LIBRARIES})
#target_link_libraries(ceres_curve_fitting "/usr/local/lib/")
运行结果：
实践：g2o
1.安装g2o依赖库
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev
安装时出现：
E: Unable to locate package libcholmod-dev
解决办法：
先把其他依赖库安装好：
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2
然后单独安装libcholmod-dev,输入以下命令然后按下Tab键
sudo apt-get install libcholmod
我们安装libcholmod3.0.6
sudo apt-get install libcholmod3.0.6
至此，依赖库安装完毕
2.编译安装g2o
github上下载g2o源码，下载地址为：https://github.com/RainerKuemmerle/g2o
使用cmake编译它并安装到机器上，安装完成之后，在/usr/local/include/g2o/下可以找到g2o的头文件，在/usr/local/lib/下可以找到它的库文件。
make出现错误：
moc: Cannot open options file specified with @
这是因为路径中包含了中文名称，把源码放在无中文名称的路径中，make就可以顺利通过。
3. g2o使用方法
（1）图优化基础
传统的非线性优化问题仅有一组优化变量和很多个误差项，我们并不知道它们之间的关联。图优化是把优化问题表现成图的一种方式，用顶点表示优化变量，用边表示误差项。
（2）SLAM中图优化问题举例
（3） 曲线拟合问题的图优化表示
节点为优化变量，边为误差项，这里的边是一元边
（4）使用g2o的基本步骤
定义顶点和边的类型
构建图
选择优化算法
调用g2o进行优化，返回结果
（5）g2o头文件
#include
#include
#include
#include
#include
#include
#include
（6）派生顶点CurveFittingVertex
在其中要重写两个重要的虚函数：
顶点的更新函数：oplusImpl(),该函数处理的是xk+1=xk+增量x
顶点的重置函数：setToOriginImpl()，把估计值置零，是平凡的
//定义曲线模型的顶点(优化变量)，参数模板：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3,Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl()//重置
{
_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 {}
};
（7）派生边CurveFittingEdge
其中要重写一个重要的虚函数：
边的误差计算函数：computeError()，这里是观测值-曲线系数拟合的值
//定义曲线模型的边(误差项) 模板参数：观测值维度，数据类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){}
//计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_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
};
（8）配置图优化
//构建图优化，先设定g2o
//矩阵块，每个误差项优化变量维度为3,误差值维度为1
typedef g2o::BlockSolver> Block;
//线性方程求解器，稠密的增量方程
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
Block* solver_ptr = new Block(linearSolver);//矩阵块求解器
//梯度下降方法，从GN,LM,DogLeg中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//取消下面的注释，以使用GN或DogLeg
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
//g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
g2o::SparseOptimizer optimizer;//图模型
optimizer.setAlgorithm(solver);//设置求解器
optimizer.setVerbose(true);//打开调试输出
（9）向图中填充顶点和边
//向图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(0,0,0));
v->setId(0);
optimizer.addVertex(v);
//向图中增加边
for(int i=0;isetId(i);
edge->setVertex(0,v);//设置连接的顶点
edge->setMeasurement(y_data[i]);//观测数值
//信息矩阵：协防差矩阵的逆
edge->setInformation(Eigen::Matrix::Identity()*1/(w_sigma*w_sigma));
optimizer.addEdge(edge);
}
（10）执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);
4. 完整代码
g2o_curve_fitting.cpp
//2019.08.09
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
//定义曲线模型的顶点(优化变量)，参数模板：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3,Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl()//重置
{
_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 {}
};
//定义曲线模型的边(误差项) 模板参数：观测值维度，数据类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x):BaseUnaryEdge(),_x(x){}
//计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_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()
{
double a=1, 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参数的估计值,初始值为0,0,0
vector x_data,y_data; //观测数据
cout<<"generating data: "<> Block;
//线性方程求解器，稠密的增量方程
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
Block* solver_ptr = new Block(linearSolver);//矩阵块求解器
//梯度下降方法，从GN,LM,DogLeg中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//取消下面的注释，以使用GN或DogLeg
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
//g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
g2o::SparseOptimizer optimizer;//图模型
optimizer.setAlgorithm(solver);//设置求解器
optimizer.setVerbose(true);//打开调试输出
//向图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(0,0,0));
v->setId(0);
optimizer.addVertex(v);
//向图中增加边
for(int i=0;isetId(i);
edge->setVertex(0,v);//设置连接的顶点
edge->setMeasurement(y_data[i]);//观测数值
//信息矩阵：协防差矩阵的逆
edge->setInformation(Eigen::Matrix::Identity()*1/(w_sigma*w_sigma));
optimizer.addEdge(edge);
}
//执行优化
cout<<"start optimization"< time_used = chrono::duration_cast>(t2-t1);
cout<<"solve time cost = "<estimate();
cout<<"estimated model:"< CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# 添加cmake模块以使用g20库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# 寻找G2O
find_package( G2O REQUIRED )
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)
# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )
add_executable( curve_fitting g2o_curve_fitting.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting
${OpenCV_LIBS}
g2o_core g2o_stuff
)
运行结果：
