SLAM编程：优化问题求解（1）_程序设计

---

前言

很久没有写技术博客了，想想看还是要固定时间养成习惯写，中间探索了很多学习编程的方法，最终还是得自己编出来+技术博客输出的方法是最好的。不能纯粹的画思维脑图，PPT之类的画板也有限，而且大量的时间浪费在思考与绘制精致的图表结构上，很费力，效率也很低。Word文档笔记不是很方便，最终还是技术博客比较适合，像LateX那样不用过多的考虑格式的问题（英文期刊提供的LateX模板会让你爱上写作英文论文），这篇是回坑吹水的第一篇，选择优化问题来写，希望能感受到越来越稀有的专注力带来的快乐吧。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、大量的问题都是优化问题

优化问题随处可见，深度学习就是把优化问题干到了极致（把参数直接往目标上靠）。平常所谓的“估计”问题，看起来是计算问题（因为表面上有固定的模型），但是也是优化问题。毕竟模型的形式是抽象的、能够使用人类思维来解释的，而模型的参数取值是具体的、令人头大的，需要使用数学方法来解释的。一份优秀的科研成果，少不了优化。我们经常会看到以下的形式出现在论文中：

这就是一个典型的优化求参数问题。这一句话的含义是，找到使得后面那个平方求和式值最小的T。我们正常看到的是min，而这里前面的arg指的就是“argument”的缩写，表示“参数”。这里再多说一嘴，有的时候一些程序里面，找到数组最大/最小值标号，也是argmax或者argmin（不信你去看PyTorch）。这个里面，就是把整个数组看成是一个函数，数组的index看成是参数，那么要找下标的话，自然是argmax和argmin了。

二、如何以朴素理论手写优化问题的程序

如果我们涉及到优化问题，首先要分清楚是有监督的优化问题还是无监督的优化问题。无监督优化问题里叫“目标函数”，我们只需要去寻找他的数学性质，寻找其极值点即可。有监督的优化问题里叫“损失函数”，我们必须去减少我们模型的表现和某个真值之间的损失。一提到有监督的优化问题，啪的一下就是四个要素很快啊！
——————————
数据：数据必须是成对的，要有观测值，也就是因变量y，以及自变量X。
抽象模型：负责对自变量X进行前向传播；
损失函数：传播得到的预测值要和因变量y进行作差来计算损失；
优化算法：最后一个要点，采用何种算法来进行优化？
——————————
以深度学习图像识别为例，自变量X是输入图像，与之对应的观测值是标签，模型是我们预先定义好的卷积神经网络，优化算法是随机梯度下降法SGD；以SLAM位姿估计为例，自变量是物方点的XYZ坐标，与之对应的观测值是图像上的(u,v)像素坐标，模型是共线条件方程（或者叫PnP投影关系），优化算法是GuassNewton或者是LM方法。这一节讲的是“朴素理论”，也就是编写梯度下降的循环迭代方法，不包含图优化（在后文中会介绍到）等理论。

1.程序总体设计

涉及到这种机器学习的优化方法，都需要求取梯度，如果不使用一些具有自动求导的框架（TensorFlow、PyTorch、Paddle）或者是库（Ceres、g2o、Matlab等），就需要我们告诉程序求导的规则。通俗而言，求一阶导数构成的矩阵叫“Jacbian”矩阵，求二阶导数构成的矩阵叫“Hessien”矩阵（这样并不是很严密的说法，但可以理解了）。我们不希望求取二阶的Hessian矩阵，尽管它在增量方程中是需要的，对于Hessian矩阵的不同方法的近似构成了GuassNewton方法和LM等方法，前者只计算Jacbian矩阵，而后用（JTJ）的方式来近似Hessian矩阵，构成了我们熟知的“最小二乘解”，在程序中，对于大量的数据点，采用叠加的方法进行统计（按道理来说，要将数据点进行拍列后再相乘，但是变长的数据结构是不利于程序写作的，因此考虑到JTJ的特殊性，采用相加的方法，这样可以保证结果一致）。关于求导，我会再新开一个文章，专门讲如何求取Jacbian矩阵的问题。
——————————
划重点：一个手写的优化程序势必由两个循环组成：
大循环：要将所有数据输入多少次（在深度学习中，叫“Epoch”）
小循环：逐个数据点，或者逐批次数据点来计算误差、求导等处理。
——通常而言，对于小规模的参数求解问题，都是在小循环结束之后，汇总误差与梯度信息来计算参数更新量；但是在大规模参数求解问题中（例如那个1750亿参数的模型），就需要每一个批次（batch）来不断调整参数，以至于深度学习还会存在mini-batch这种说法。
——————————
为避免让读者觉得乏善可陈，下面我们以一个比“曲线拟合”问题稍微高级一点的高斯牛顿法位姿估计程序为例，分享一些心得。这是因为，在一些复杂的场景下，我们需要重新定义参数更新方法和思考方式，源码来自高翔博士的《视觉SLAM十四讲》，一并感谢高翔博士拯救我这个编程小白。

2.编写大循环：更新参数并输出信息

两重循环嵌套的时候，一定不能照着程序一行一行的敲。人只能一次干一件事，先完成大循环，就当是一次小循环已经结束了、输出了我想要的东西，然后只需要对这些东西进行一些处理就可以了，这样在编写程序的时候，能够保证“线性思路进入程序”，而不是陷进下一个循环里面。简单的来说，大循环以自己指定的迭代次数（Epoch）为目的，是最终完成优化的那个循环，也得具备终止条件。
——————————
在小循环之前要做这么几件事：
（1）梯度要清零，因为所有数据已经遍历过一次了；
（2）记录当前大循环的Cost也要清零，理由同上；
（3）用于计算增量dx的H和b要清零（梯度包含在其中，回顾GN方法）
——————————
假设小循环已经结束，得到了我们需要的H和b：
则可以立即求出更新量dx，但是在更新之前，有3个条件得考虑：
（1）求解出来的dx不是数字（分母为零/NaN)，就得让程序报错停止了；
（2）记录每一次大循环得到的cost，因为大循环的cost记录的是当前参数在所有数据上的表现，如果说这一次的cost比上一次循环的cost还大，就说明更新是不合适的，应当退出；
（3）如果dx本身的范数很小，也说明收敛了，不需要更新迭代了；代码如下（示例）：

int iteration = 10;
typedef Eigen::Matrix<double,6,1> Vector6d;
double lastCost = 0;
double cost = 0;
Sophus::SE3d pose;
for(int iter=0;iter<iteration;iter++){
	Eigen::Matrix<double,6,6> H = Eigen::Matrix<double,6,6>::Zero();
	Vector6d b = Vector6d::Zero();
	cost = 0;
	//此处写小循环
	//.....
	//假设我们已经得到了H和b
	Vector6d dx = H.ldlt().solve(b);
	if(isnan(dx[0])){
		cout<<"The result isnan！"<<endl;
		break;
	}
	if(dx.norm()<1e-6){
		cout<<"Covergence!"<<endl;
		break;
	}
	if(iter > 0 && cost>=lastCost){
		cout<<"The cost:"<<cost<<">=lastCost"<<lastCost<<endl;
		break;
	}
	//然后才是更新
	pose = Sophus::SE3d::exp(dx)*pose;
	lastCost = cost;
	cout<<"Iterattion:"<<iter<<"Cost:"<<cost<<endl;
}

3.编写小循环：前向传播，计算误差，反向传播=>计算H、b

小循环是针对“每一对数据”的，这就不是我们所指定的了，要根据数据点来算。三维坐标数据点的格式是vector<Eigen::Vector3d> points_3d; 二维像素点的数据格式仿照上面，区别在于2d，传入到函数当中。代码如下（示例）：

for(int i=0;i<points_3d.size();i++){
	//前向传播，计算重投影:KTP
	Eigen::Vector3d pc = pose * points_3d[i];
	Eigen::Vector2d proj(
		fx * pc[0]/pc[2]+cx,
		fy * pc[1]/pc[2]+cy,
	);
	Eigen::Vector2d e = points_2d[i]-proj;
	cost += e.squaredNrm();
	//在此输入2×6的Jacbian矩阵，由pc[i],fx,fy,cx,cy构成
	Eigen::Matrix<double,2,6> J;
	J << ....;
	//计算H和b,采用累加法
	H += J.transpose()*J;
	b += -J.transpose()*e;
}

至此，一个GuassNewton法进行位姿估计的小程序就写完了。实际上对于以最小二乘的优化问题，还有ceres库和g2o库。ceres库比较契合这种写法，本帖会不断的更新，先放出以图优化理论的g2o库的相关理解。这个库的典型特点是不好使用和理解，适合看别人的程序。

三、使用库来完成优化问题

1.使用g2o库完成单一位姿优化问题

要使用g2o库，首先要把优化问题建模为图优化问题。简而言之，就是定义好图的各个要素
——————————
1、定义图的顶点——待优化的变量。
待优化的变量通常叫“参数块”（和Ceres类似），一个参数块包含着从自变量X到因变量y这个模型里面的所有待优化参数，因此与不是某一个单一变量就可以成为一个顶点的（除非模型里只有这一个参数）。定义顶点有三个要点：
（1）定义顶点的模板实例；
（2）定义顶点的归零函数；
（3）定义顶点的更新函数。
首先说模板实例，所有的顶点继承自g2o的BaseVertex类。类模板参数为<参数更新量的维度，待优化变量的数据类型>。这两者有可能不一样。一般来说更新值和待优化变量的数据类型应当一致，这样可以使用简单的向量空间加法进行更新（例如深度学习采用的梯度下降法将所有参数看成是一个向量直接求解），然而对于位姿优化问题，具有明确的李群和李代数更新方式——左右扰动，i.e.,用一个6元素的一维向量对一个4×4位姿变换矩阵T进行更新，这样的话就不一样了；而后再说归零函数，就是顶点如何初始化，换言之就是待优化变量如何初始化；第三点就是更新函数，这里就是我们要定义的更新方法。代码如下（示例）：

class VertexPose:public g2o::BaseVertex<6,Sophus::SE3d>{
	//更新维度是6，参数块类型是4×4李群
public:
	//防止在内存分配上出现问题
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	
	//顶点归零函数，这里_estimate是内置变量
	virtual void setToOriginImpl() override {
		_estimate = Sophus::SE3d();
	}
	//顶点更新函数，对_estimate进行更新
	virtual void OplusImpl(const double *update)  override {
		//指针类型的可以采用下标进行访问
		Eigen::Matrix<double,6,1> update_eigen;
		update_eigen << update[0],update[1],update[2],update[3],update[4],update[5];
		_estimate = Sophus::SE3d::exp(update_eigen)*_estimate;
	}
	virtual bood read(istream &in) override {}
	virtual bood write(otream &out) override {}
}

——————————
2、定义边，也就是和模型相关的部分。
（1）定义边的类型（指定模板参数）
（2）定义构造函数并在最后指定私有成员；
（2）定义误差计算函数（computeError）
（3）定义导数函数（linearizeOplus）
边的类型分为一元（Unary）边和多元边，对于一个简单的曲线拟合或者是单一位姿估问题，都是一元边，对于大型的SLAM问题，往往是采用多元边。边的模板参数很有意思，<观测值维度，观测值数据类型，顶点类型>。对于单一位姿估计（光束法平差）问题，观测值就是像素坐标，维度为2，观测值数据类型就是Eigen::Vector2d;数据类型就是我们之前定义好的VertexPose。定义构造函数的写法一般是简写，这在函数里面会有，刻画的是投影关系，自然要传入的是自变量X（物方空间坐标点）+相机内参矩阵；误差计算函数完成前向传播后的loss计算功能，而导数函数+顶点更新函数则完成反向传播功能。特别要注意的是，每一对自变量X和因变量y之间都会构造出一条边。代码如下（示例）：

class EdgeProjection:g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>{
public:
	//防止在内存分配上出现问题
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	//构造函数
	EdgeProjection(const Eigen::Vector3d &pos_3d,const Eigen::Matrix3d &K):
		_pos3d(pos_3d),
		_K(K) {} //等价于{_pos3d=pos_3d;_K=K}

	//误差计算函数，该函数不接受参数，因此需要构造函数
	virtual void computeError() override {
		//取出第0号顶点，也就是参数块顶点，估计得到当前的参数
		const VertexPose *v = static_cast<VertexPose *>(_vertics[0]);
		Sophus::SE3d T = v->estimate();
		//根据模型进行前向传播
		Eigen::Vector3d pos_pixel = _K * T * (_pos3d[i]);
		pos_pixel /= pos_pixel[2]; //(u,v,1)
		//计算误差,_measurement和_error是内置变量
		_error = _measurement - pos_pixel.head<2>();
	}

	//导数函数，该函数不接受参数，因此需要构造函数
	virtual void linearizeOplus() override {
		//取出第0号顶点，也就是参数块顶点，估计得到当前的参数
		const VertexPose *v = static_cast<VertexPose *>(_vertics[0]);
		Sophus::SE3d T = v->estimate();
		//由于导数的形式是针对图像坐标系来的，因此要再计算一次参数
		//导数中的形式是[X',Y',Z']形式过来的
		Eigen::Vector3d pos_cam = T * _pos3d[i];
		double fx = _K(0,0);
		double fy = _K(1,1);
		double cx = _K(0,2);
		double cy = _K(1,2);
		double X = pos_cam[0];
		double Y = pos_cam[1];
		double Z = pos_cam[2];
		_jacbianOpluXi<<.....;//2×6的偏导数
	}
	
	virtual bood read(istream &in) override {}
	virtual bood write(otream &out) override {}

//这里是要和构造函数在一块写的
private:
	Eigen::Vector3d _pos3d;
	Eigen::Matrix3d _K;
}

——————————
3、配置g2o
我们必须让g2o适配于我们当前的问题，采用的是形象的make_unique 指定2个类型
（1）BlockSolver类型，也就是我们要求解的参数块的性质；
（2）LinearSolver类型，也就是我们要采用什么方法来求解当前问题；
对于（1），通过指定BlockSolverTraits的模板参数来设置。这一点看了网上的回答也没有看懂。有一个大佬的回复是https://www.zhihu.com/question/337617035/answer/767355216，这两个模板参数分别代表一条边对应的两个顶点的维度。SLAM十四讲里面，说第二个参数指的是优化变量维度。在几种特定的情境下验证一下对不对吧：首先是曲线拟合，当时给出的是模板参数是<3,1>，表示一头连着优化变量abc，三维，一头连着因变量y，1维；但是到这里来的话明显有问题，因为这个里面误差变量明显是因变量y，也就是像素坐标(u,v),，这个是在_error私有成员里面限定好的，按照这个理论，这里的写法应该是<6,2>，但是实例程序给出的注释是<6,3>，位姿优化变量是6维，3表示的是路标点的[X,Y,Z]。但是XYZ和pose同时处在模型的一边啊，也就是T*pos_3d[i];
所以我的理解倾向于（当然不一定对，只能说符合这两种情况的条件），这里的第一个模板参数表示优化变量维度，第二个模板参数表示的是自变量的维度。这里的自变量是[X,Y,Z]。
对于曲线拟合，自变量维度就是1，因为只有一个x。代码如下（示例）：

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> BlockSolverType;
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmGuassNewton(
	g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; //generate the graph
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);//打开控制台输出

——————————
4、将顶点和边加入到图模型中
添加顶点的时候：
（1）设置编号；（2）设置估计类型；（3）添加顶点

VertexPose *v_pose = new VertexPose();
v_pose->setId(0);
v_pose->setEstimate(Sophus::SE3d);
optimizer.addVertex(v_pose);

添加边的时候，要逐对数据点（X,y）添加：每条边要设置好：
（1）Id;
（2）和哪些顶点相连？
（3）观测值是什么？（y）
（4）信息矩阵——和观测值的维度平方一致

int index = 1;
for(int i=0;i<pts_3d.size();i++){
	auto p2d = pts_2d_eigen[i];
	auto p3d = pts_3d_eigen[i];
	//construct the edge;
	EdgeProjection *edge = new EdgeProjection(p3d,K_eigen);
	edge->setId(index);
	edge->setVertex(0,v_pose);
	edge->setMeasurement(p2d);
	edge->setInformation(Eigen::Matrix2d::Identity());
	optimizer.addEdge(edge);
}

——————————
5、指定迭代次数等参数，进行求解
（1）初始化优化器；
（2）进行优化

optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
pose = v_pose->estimate();
cout<<"The pose estimated by g2o is\n"<<pose.matrix()<<endl;

---

总结

本文会不断的进行更新，后期会引入更多的BA问题，欢迎大家与我探讨，争取坚持每周一更吧，也算是对自己的一个激励，顺便也让自己不要那么沉迷于手机。