视觉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过程出现错误:
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第1张图片
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/")

运行结果:
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第2张图片
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第3张图片

实践: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出现错误:
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第4张图片
moc: Cannot open options file specified with @
这是因为路径中包含了中文名称,把源码放在无中文名称的路径中,make就可以顺利通过。

3. g2o使用方法

(1)图优化基础
传统的非线性优化问题仅有一组优化变量和很多个误差项,我们并不知道它们之间的关联。图优化是把优化问题表现成图的一种方式,用顶点表示优化变量,用边表示误差项。
(2)SLAM中图优化问题举例
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第5张图片
(3) 曲线拟合问题的图优化表示
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第6张图片
节点为优化变量,边为误差项,这里的边是一元边

(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
)

运行结果:
视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第7张图片视觉SLAM十四讲 读书编程笔记 Chapter6 非线性优化_第8张图片

你可能感兴趣的:(SLAM)