视觉SLAM十四讲读书编程笔记 Chapter6 非线性优化

最新推荐文章于 2022-09-21 13:33:14 发布

Leo-Ma

最新推荐文章于 2022-09-21 13:33:14 发布

阅读量1.5k

点赞数 2

分类专栏： SLAM 文章标签： slam十四讲

本文链接：https://blog.csdn.net/wolfcsharp/article/details/98854184

版权

SLAM 专栏收录该内容

30 篇文章 7 订阅

订阅专栏

Chapter6 非线性优化

实践：Ceres
实践：g2o

实践：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<ceres::Jet<double, 2> >’
这是因为eigen3与ceres发生了冲突。
解决办法：
下载与书中提供的ceres版本兼容的eigen，与书中提供的ceres版本兼容的eigen下载
编译并且安装它，然后再编译ceres工程就可以顺利通过啦

3. 曲线拟合问题描述

假设有一条满足以下方程的曲线：y=exp(ax²+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 <typename T>
	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<N;i++)
	{
		problem.AddResidualBlock(	//向问题中添加误差项
			//使用自动求导，模板参数：误差类型，输出维度，输入维度
			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.linear_solver_type = ceres::DENSE_QR;//	增量方程如何求解
	options.minimizer_progress_to_stdout = true;//输出到cout

优化信息对象的定义：

	ceres::Solver::Summary summary;

（4）对问题进行优化

	ceres::Solve (options,&problem,&summary);//开始优化

优化信息对象输出结果：

	cout<<summary.BriefReport()<<endl;

最后优化的结果放在abc变量中

5.完整代码

ceres_curve_fitting.cpp

//2019.08.08
#include <iostream>
#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维
		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<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++)
	{
		problem.AddResidualBlock(	//向问题中添加误差项
			//使用自动求导，模板参数：误差类型，输出维度，输入维度
			new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(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<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

#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 <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>

（6）派生顶点CurveFittingVertex
在其中要重写两个重要的虚函数：
顶点的更新函数：oplusImpl(),该函数处理的是x_k+1=x_k+增量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<const CurveFittingVertex*>(_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<g2o::BlockSolverTraits<3,1>> Block;
	//线性方程求解器，稠密的增量方程
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	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;i<N;i++)
	{
		CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
		edge->setId(i);
		edge->setVertex(0,v);//设置连接的顶点
		edge->setMeasurement(y_data[i]);//观测数值
		//信息矩阵：协防差矩阵的逆
		edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
		optimizer.addEdge(edge);
	}

（10）执行优化

	optimizer.initializeOptimization();
	optimizer.optimize(100);

4. 完整代码

g2o_curve_fitting.cpp

//2019.08.09
#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>

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<const CurveFittingVertex*>(_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<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
	//矩阵块，每个误差项优化变量维度为3,误差值维度为1
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> Block;
	//线性方程求解器，稠密的增量方程
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	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;i<N;i++)
	{
		CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
		edge->setId(i);
		edge->setVertex(0,v);//设置连接的顶点
		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;
	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

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
)

运行结果：
在这里插入图片描述

Leo-Ma

关注

2
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
视觉SLAM十四讲读书编程笔记 Chapter6 非线性优化

Chapter6 非线性优化实践：Ceres1. 安装Ceres依赖库2. 编译安装Ceres3. 曲线拟合问题描述4. ceres使用方法5.完整代码实践：g2o1.安装g2o依赖库2.编译安装g2o3. g2o使用方法实践：Ceres1. 安装Ceres依赖库sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse...
复制链接

扫一扫