SLAM编程：优化问题求解（2）_细谈求导

---

前言

优化问题（可参考我的另一篇博文，《SLAM编程：优化问题求解（1）_程序设计》）中，如何求解导数是一个重要的子问题，其复杂的数学推导往往让人很难受。这里面不仅是求导本身的复杂性，更有的文章和技术对于更新量的求解并非来自于目标函数对待优化变量的直接求导。本文也会不断更新，试图以较为朴素的语言给出我对于求导的理解。

---

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

一、正常思路：目标/损失函数直接对优化变量求导

这种思路的典型代表是深度学习中的梯度下降法。在没有任何先验知识的情况下，只能对深度学习模型这个黑箱中的所有参数进行求导，具体推导是反向传播算法，核心是链式法则。链式法则是非常重要的，它能够有效帮助我们缓解思考压力，推导出更加简洁的导数形式。

1.实例：最小二乘位姿估计

在笔者目前的经历中，典型的直接求导就是最小二乘位姿估计，在传统的摄影测量理论当中，对于位姿变量是直接求导到横滚、俯仰、偏航角+摄站位置（XYZ）的，比较直接；而视觉SLAM理论当中，虽然没有直接对位姿矩阵求导（采用了李代数方法进行问题转化），但本质上只是换了一种情况进行位姿矩阵更新（左/右扰动），因而对李代数（6维向量）求导，仍然是直接对优化变量求导。在这个位姿估计问题中：目标函数为重投影误差，那么我们要求像素坐标对李代数的求导关系。而李代数是隐含在位姿变换的T里面的，因此我们先记变换到相机坐标系（又叫归一化坐标系、图像坐标系）下的坐标为（X’,Y’,Z’); 它和像素坐标系之间的变换公式如下：

重申一遍，（X’,Y’,Z’）是李代数对应李群（位姿矩阵）的函数，由此可以使用链式法则，让（X’,Y’,Z’）对李代数进行求导。我们前向传播的得到投影为reproj[0]和reproj[1]，那么误差的形式就可以写成：

于是我们根据链式法则可以得到如下的形式：

第一项是容易的，可以轻松地根据关系直接得到：

后面那个李代数的怎么做呢？也是有一个经典结论的:

两式相乘，就可以得到对于重投影误差关于李代数的求导形式：

在程序之中，给出片段如下：对应另一篇博文中的“小循环”。代码（示例）：

typedef Eigen::Matrix<double,6,6> Matrix6d;
typedef Eigen::Matrix<double,6,1> Vector6d;
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double fx = K.at<float>(0,0);
double fy = K.at<float>(1,1);
double cx = K.at<float>(0,2);
double cy = K.at<float>(1,2);
//下面开始小循环
for(int i=0;i<pts_3d.size();i++){
	Eigen::Vector3d pc = pose * pts_3d[i];
	double inv_z = 1/0/pc[2];
	double inv_z2 = inv_z * inv_z;
	Eigen::Vector2d proj(
		fx * pc[0] /pc[2] + cx,
		fy * pc[1] /pc[2] + cy,
	);
	Eigen::Vector2d e = pts_2d[i]-proj;
	Eigen::Matrix<double,2,6> J;
	J << -fx * inv_z, 0, fx*pc[0]/inv_z2, fx * pc[0] *pc[1]*inv_z2,
		-fx-fx*pc[0] * pc[0]*inv_z2, fx*pc[1]*inv_z2,0,-fy*inv_z,
		fx*pc[1]*inv_z2,fy+fy*pc[1]*inv_z2,-fy*pc[0]*pc[1]*inv_z2,fy*pc[0]*inv_z2;
	H += J.transpose() * J;
	b += - J.transpose() * e;
}

二、清奇思路：不直接求导

这种清奇思路主要体现在很难理清变量之间关系的情况下如何建立导数关系，有点像是数学建模，但是最后捣鼓着就能变成优化问题。笔者认为这种方式更加适合学者们开拓思路。举两个例子，首先是单层光流法，然后是多层光流与直接法位姿估计。

1 实例：追踪光流也能是优化问题？

“光流法”是一种追踪前后帧图像中同一个点的简单方法，根据数学推导，理论上进行求解的是图像之间的运动，但是由于成像时差不知道，我们只能求取到图像之中相同像素的位移。具体到图上，就是使用cv::line函数，把前后得到的匹配点作为顶点在第二张图上画出来（是不是大跌眼镜），就可以形成我们需要的“光流图”了，看上去像是特征点运动起来了。
——————————
要在第二张图片上找到第一张图片上的点，还得是优化方法，这是一个很难的思考问题。首先，我们只知道要求解的是第一张图片上提取出的特征点在第二张图片上的位置，换言之，待优化求解变量是像素位移。但是，和像素位移相关的变量是什么呢？连这个问题都不清楚，更不要说与之相关的导数关系了。这里借助泰勒展开的数学推导很值得借鉴。
——————————
首先，从单层光流法的假设出发，两张图片上相同像素亮度一样（暂且不管对不对）：

而这里涉及到了增量，可以用泰勒展开又得到一个式子：

这样一来就有意思了，得到了

注意，我们要求解的是像素运动，那么可以把dt除掉：

我们似乎找到了一个可以最小二乘的问题、一个类似于高斯牛顿优化的形式（注意，这将给之后的讨论埋下伏笔），当然，最小二乘的核心是“多余观测量”，现在有很多的提取出来的特征点，假设每一个特征点周围的w * w像素，具有相同的运动（一开始笔者不太理解，但是实际上，如果只有平行于相平面的平移，那么所有的像素运动都应该是一样的，但如果有复杂的运动，例如旋转和错切，那么不同的像素运动肯定会不一样，所以光流法的结果在复杂变换下往往是横七竖八的线，至于我们常看到的径向收敛形式的汽车轨迹光流追踪，则是在光心方向运动得到的结果，在那种情况下虽然有规律，但很明显所有像素的运动都不一样）
——————————
因此可以得到如下的式子：

这样一来，结合笔者之前写的优化心得，大概可以勾勒出这个最小二乘问题的求解循环写法了：对于每一个提取出的特征点，首先是对于迭代次数的大循环不用说，然后是针对数据点的小循环，数据点是什么，实际上就是w * w个像素具有相同约束这一条件，error是什么呢？其实就是加上偏移之后的像素坐标和没有加偏移的像素坐标之间的差值，回顾我们的假设条件，光流的两个顶点应当保持一样的灰度，因此对于每一个提取出的特征点，都要遍历它邻域的 w * w 个像素，优化如下问题：

而之所以反复强调“对于某一个提取出的特征点”，是因为我们可能提取出了数百个特征点，对于每个特征点的邻域，都要进行以上的最小二乘法求解其邻域内的运动。因此实际上应该是3层循环（第一层是遍历特征点）。然而数百个循环是难以忍受的，我们在工程实现中往往采用opencv并行计算方法进行处理。
——————————
下面回顾关于求导的问题，即像素运动速度（无时间信息时表现形式为位移）关于某些待优化变量的导数。附加一点要说的，最小二乘直接求导的话，可以只用其中的一次误差项来求导，也就是I(i,j)-I(i+dx,j+dy)直接对[dx,dy]求导，得到的是“位于ij处的、x方向和y方向上的像素梯度”，恰好是矩阵式中左边的部分。而（i,j）处的像素梯度，在图像中使用像素差分代替，i.e.,dx=f(x+1,y)-f(x-1,y);dy=f(x,y+1,x,y-1);（笔者曾经认为图像差分只会用于边缘提取等任务，看来真的是孤陋寡闻）给出 代码如下（示例）：

OpticalFlowTracker tracker(img1,img2,kp1,kp2,success,inverse,has_initial);
parallel_for_(Range(0,kp1.size()),
	std::bind(&OpticalFlowTracker::calculateOpticalFlow,&tracker,placeholder::_1));
	//上述代码指的是用序列管理的方式并行计算一张图像中的所有特征点对应的图像运动
	//这里bind函数的作用是将1号位的参数固定
void OpticalFlowTracker::calculateOpticalFlow(Const Range &range){
	int half_patch_size = 4;
	int iteration = 10;
	for(size_t i=range.start;i<start.end;i++){
		//这里是配合前面的Range管理
		auto kp = kp1[i];
		double dx = 0, dy = 0;
		if(has_initial){
			dx = kp2[i].pt.x - kp1[i].pt.x;
			dy = kp2[i].pt.y - kp1[i].pt.y;
		}
		double cost = 0, lastCost = 0;
		for(int iter=0;iter<iteration;iter++){
			Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
			Eigen::Vector2d b = Eigen::Vector2d::Zero();
			Eigen::Vector2d J;
			//calculate cost and Jacobian
			for(int x =-half_patch_size;x<half_patch_size;x++)
				for(int y=-half_patch_size;y<half_patch_size;y++){
					double error = getPixelValue(img1,kp.pt.x+x,kp.pt.y+y)-
								   getPixelValue(img2,kp.pt.x+x+dx,kp.pt.y+y+dy);
					J = -1.0*Eigen::Vector2d(
						0.5*(getPixelValue(img2,kp.pt.x+dx+x+1,kp.pt.y+y+dy)
							-getPixelValue(img2,kp.pt.x+dx+x-1,kp.pt.y+y+dy)),
						0.5*(getPixelValue(img2,kp.pt.x+dx+x,kp.pt.y+y+dy+1)
							-getPixelValue(img2,kp.pt.x+dx+x,kp.pt.y+y+dy-1))
					);
					H += J * J.transpose();
					b += -error * J;
					cost += error * error;
				}
			Eigen::Vector2d update = H.ldlt().solve(b);
		}
	}
}
	

为简洁起见，案例当中没有考虑反向光流的情况（参数中的inverse）。需要注意以下几点
——————————
（1）为什么求梯度的时候全都是img2？
我们在最小二乘式子当中很难看出来，需要理解这样一个事实，即“图像1中的像素经过变换，变换为图像2中的像素，这时候，图像2中的w * w小窗口内的所有像素都满足同样的运动”。读者可能会问，那么问什么不假设图像1中的小窗口内像素都具有相同的运动呢？这是因为，图像1中的像素没有加dx，dy，事实上没有经过前向传播，我们只对经过前向传播的变量进行优化。这样说还是太绕了，直接看我们最终优化的式子，可以看出，图像1的I(x,y)是没有dx和dy的，自然也就不会对这两个待优化变量计算出jacobian,只有图像2的像素位置有优化变量，才会产生Jacobian与导数。
——————————
（2）关于一些转置与维度的问题？
我们看到前面的Ix和Iy所组成的矩阵式1×2的，但是Eigen库提供Eigen::Vector2d这样现成的类型，尽管我们可以自己定义Eigen::Matrix这样的类型，但是有违程序设计的统一性。我们顾虑矩阵的维度主要是顾及到矩阵相乘的维度要求，但是大可不必。例如，我们把原本为行向量的J写成列向量的Vector2d，但是求解H的时候我们就倒过来呗，原本是J.transpose()*J,换成J *J.transpose()就好了。

总结

本帖会不断的进行更新，加入更多的求导与优化思路，技术的发展永无止境，希望能对自己、对读者的思考有所启发。