＜视觉十四讲＞ ch6 非线性优化

状态估计问题

1.1 批量状态估计和最大后验估计

运动方程：
观测方程：
观测方程由针孔模型给定，对应到图像上的像素位置，观测方程可以写成(from ch5):其中，K为相机内参，s为像素点的距离，xk是相机的位姿，yj是路标。w和v都是噪声分量，满足0均值的高斯分量：
在噪声的影响下，通过带噪声的数据z和u推断位姿x和地图y（以及概率分布），即状态估计问题。
处理状态贵问题有两种方法：滤波器（渐进式/增量式），非线性优化（批量式），SLAM使用得是滑动窗口估计法，对当前时刻附近的一些轨迹进行优化。
后验分布P(x,y|z,u): 已知输入数据u(所有时刻的输入), z(所有时刻的观测数据), 求状态x, y的条件概率分布。
贝叶斯法则：直接求解后验概率是非常困难的。该方法是最大化似然和先验的乘积，从而最大化后验概率，从而得到某个状态的最优估计。 （我理解最可能的状态）
补充：argmax f(x,y)是指当f(x,y)取得最大值时，变量x,y的取值
最大后验估计：

1.2 最小二乘法的引出

这里用到了两个知识点：
①由于z, u相互独立可以将式子分解；
②用数学推导求解；

首先讨论P（z|x, y)：
考虑高斯分布
经过一些数学推导：
等价于最小化噪声项（重投影误差）的二次形式。 （二次形式称为马氏距离）
据此引出并推导P（z, u | x, y):
注意右侧第一项没y，是因为运动方程本身就没y。
关于u的最大似然和关于z的最大似然是类似的，得到P（z, u | x, y)最大似然估计方法：
即把最大似然估计转化为最小二乘问题。
理解：当视觉里程计得到x和y，带入运动模型和观测模型时不会完美成立（存在误差），所做工作的目的是对数据进行调整，进而使得误差变小，也得到整体数据的最大似然估计。

1.3 举个例子

离散时间系统：
根据以上推导，最终的最小二乘目标函数为：

该系统是一个线性系统，可以用向量的形式求解。

2.非线性最小二乘

上一节是将SLAM问题转换成最大似然估计问题，然后又转换成最小二乘问题。本节来学习如何求解最小二乘问题。
求解某个函数的最小值，高考教我们求导数，然后带进去看哪个最小。当然，现在不是高考题，这个题不是那个题。
首先来看个normal的迭代方法：我的理解是就是找个增量让F(x)不断下降。

2.1 一阶和二阶梯度法

虽然我没学过数值分析，但把F(x) Taylor展开：
一阶梯度法(最速下降法)：发现如果直接取增量x的方向为-J方向，就可以令其趋于最小；
二阶梯度法(牛顿法)：二阶导数保留：J + Hx = 0 –> Hx = -J。
这两种算法不得行，主要原因：最速下降法太贪心，反而容易增加迭代次数，收敛变慢；牛顿法得求出H，难度大。
所以有以下两种算法。

2.2 高斯牛顿法

上个是把F(x)展开，这个是把f(x)Taylor展开，再取范数的平方：
后和牛顿法一样，再对增量x求导，其实我觉得不需要展开也能求导，是这样的：
这里的H可不是海塞矩阵啦，是一阶导数雅可比矩阵的近似矩阵（也不晓得为啥整得名差不多）。然后不断迭代，和牛顿法类似。
缺点：还是需要求H的逆，但这个H能不能逆都不一定，即便能求出来，这个增量x也太大了。

2.3 列文伯格-马夸尔特方法（LM方法）

补充：Taylor展开只在离展开点近得地方近似比较好，于是划分出一个“信赖区域”，不让他跑到近似性不好的区域。
该近似值通过以下方式判断：
我的理解是p是J(x)的真实程度，若p是1，则说明很真，很近似，很好；若p小于1, 则说明实际减小的值少于近似减小的值，需要缩小近似范围（以实际为准），反之，p比较大，实际比预计的大，则扩大近似范围。
说明：上述阈值是经验值。列文伯格把D取为单位阵I，直接把增量x约束在秋内，马夸尔特把D取成非负对角阵（实际通常是J * J^T的对角元素平方根），这使得在梯度小的维度上约束范围大些。
步骤2中的公式右侧有一个不等式约束，即使用拉格朗日乘子将约束项置于目标函数中：
求导，导数为0，得：
说明：这个H和高斯牛顿法的H是一样的，不是海塞矩阵哦。
增量x的系数矩阵比单纯H矩阵有更强的正定性，H在这个系数的占比权重越大，二阶近似效果越好，越接近高斯牛顿法；H在这个系数的占比权重越小，二阶近似差，越接近一阶（快速）牛顿法。（D^T * D 在列文伯格这等于I）
SLAM问题中，通常选择高斯牛顿或者列文伯格-马夸尔特方法中的一个，问题性质好就用高斯牛顿法，问题太病态了就用后者。

三、总结

1、科学问题的初值提供很重要，视觉SLAM主要用ICP，PnP之类的算法提供优化初始值；
2、求解线性增量方程，一般将矩阵分解，像之前ch3提到的QR和Cholesky等方法。

4.令人兴奋的英文符号

4.1 手写高斯牛顿

回一下高斯牛顿过程：将f(x)Taylor展开令导数为0，写出增量方程不断迭代。
待拟合的曲线： y = exp(ax^2 + bx + c) + w， 已知x, y，估计a, b, c，w是噪声。
step:
① 给定初始值[2, -1, 5];
② 为了迭代求出雅可比矩阵和噪声误差（比较简单不需要Ceres / g2o)；
③ 求解增量方程，得到增量x；
④ 迭代直到次数达到上限或者增量x达到理想值。
这里有一点需要注意。这里状态估计中使用的范数是作为内积的信息矩阵。具体推导见这篇文章。我认为这很容易理解。增量方程如下：

代码部分：
我和书里写得不太一样，主要是对M^{-1}的定义上，我只用一个inv，书里用了inv * inv。但结果一样。奇怪[0]

int main(int argc, char **argv) {
   double aa = 1.0, ba = 2.0, ca = 3.0;//真实参数值
   double ae = 2.0, be = -1.0, ce = 5.0;//估计参数值
   int N = 100;  //数据点
   double w_sigma = 1.0;
   double inv_sigma = 1.0 / w_sigma; //set 1/Sigma = W^{-1}
   cv::RNG rng;
   
   //prepare data
   vector<double> x, y;
   for(int i = 0; i < N; i++){
	double temp = i / 100.0;
   	x.push_back(temp);
   	y.push_back(exp(aa * temp * temp + ba * temp + ca) + rng.gaussian(w_sigma));
   }
   
   //Gauss-Newton
   int iteration = 100; //time
   double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
   
   chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
   for(int i = 0; i < iteration; i++) {
   	Matrix3d H = Matrix3d::Zero(); // H = J^T * W^{-1} * J
   	Vector3d g = Vector3d::Zero(); // g = -J^T * W^{-1} * w
   	cost = 0;
   
   	for(int j = 0; j < N; j++){
		double xi = x[j], yi = y[j];
		double error = yi - exp(ae * xi * xi + be * xi + ce);
		Vector3d J;
      		J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      		J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      		J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc   		
      		H += inv_sigma * J * J.transpose();
     		g += -inv_sigma * error * J;
     		
     		cost += error * error;
     	}
     	
     	Vector3d dx = H.ldlt().solve(g);
     	if(isnan(dx[0])) {
     		cout<<"result is nan!"<<endl;
     		break;
     	}   	
     	if(i > 0 && cost >= lastCost){
     		cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      		break;
    	}//notice that i > 0 	
     	ae += dx[0];
     	be += dx[1];
     	ce += dx[2];   	
     	lastCost = cost;   	
     	cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }
  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 << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

result:

xdhu@ubuntu:~/Downloads/slambook2-master/ch6/build$ ./gaussNewton
total cost: 3.19575e+06, 		update: 0.0455771  0.078164 -0.985329	estimated params: 2.04558,-0.921836,4.01467
total cost: 376785, 		update:  0.065762  0.224972 -0.962521		estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6, 		update: -0.0670241   0.617616  -0.907497	estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01, 		update: -0.522767   1.19192 -0.756452		estimated params: 1.52155,1.11267,1.3882
total cost: 174.853, 		update: -0.537502  0.909933 -0.386395		estimated params: 0.984045,2.0226,1.00181
total cost: 102.78, 		update: -0.0919666   0.147331 -0.0573675	estimated params: 0.892079,2.16994,0.944438
total cost: 101.937, 		update: -0.00117081  0.00196749 -0.00081055	estimated params: 0.890908,2.1719,0.943628
total cost: 101.937, 		update:   3.4312e-06 -4.28555e-06  1.08348e-06	estimated params: 0.890912,2.1719,0.943629
total cost: 101.937, 		update: -2.01204e-08  2.68928e-08 -7.86602e-09	estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.000277778 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629