前言：最速下降法，在SLAM中，作為一種很重要求解位姿最優值的方法，缺點很明顯：迭代次數太多，盡管Newton法（保留目標函數的二階項Hessian矩陣）改善了“迭代次數過多”這一缺點，但是Hessian矩陣規模龐大（參考：特征匹配點成百對），計算較為困難。Gaussian-Newton法在Newton原有基礎上，用的是一階雅克比的轉置*一階雅克比 JTJ 來近似 Hessian， 但是，這里的近似替換 JTJ並不一定正定。一般，主流非線性優化方法：Levenberg-Marquadt、Dog-leg較為常用，這里不作介紹。
一、梯度下降相關數學概念
個人理解：
是方向偏導數，我們不僅要使得梯度下降，還要以最快的步伐下降。
有關負梯度下降為什么是最快的？另一種理解，請參考：https://zhuanlan.zhihu.com/p/24913912
x* 是無約束問題的局部最優解 =》 x*是其目標函數的駐點
（回想起SVM當中求解有約束目標函數最優值，利用拉格朗日乘子法進行轉換。）
例如：在 y = x^3的曲線當中，x = 0處是 極大值點，也不是極小值點，稱作：鞍點。
理解定理3：我們翻開書，看到如下概念：
性質
（1）如果一個函數f(x)在某個區間I上有f''(x)（即二階導數）>0恆成立，那么對於區間I上的任意x,y，總有：
f(x)+f(y)≥2f[(x+y)/2]，如果總有f''(x)<0成立，那么上式的不等號反向。
幾何的直觀解釋：如果一個函數f(x)在某個區間I上有f''(x)（即二階導數）>0恆成立，那么在區間I上f(x)的圖象上的任意兩點連出的一條線段，這兩點之間的函數圖象都在該線段的下方，反之在該線段的上方。
（2）判斷函數極大值以及極小值。
結合一階、二階導數可以求函數的極值。當一階導數等於0，而二階導數大於0時，為極小值點。當一階導數等於0，而二階導數小於0時，為極大值點；當一階導數和二階導數都等於0時，為駐點。
（3）函數凹凸性。
設f(x)在[a,b]上連續，在（a,b)內具有一階和二階導數，那么，
（1）若在（a,b)內f''(x)>0,則f(x)在[a,b]上的圖形是凹的；
（2）若在（a,b)內f’‘(x)<0,則f(x)在[a,b]上的圖形是凸的。
凸優化，參考：https://blog.csdn.net/kebu12345678/article/details/54926287
二、例題講解
參考：https://blog.csdn.net/wangdingqiaoit/article/details/23454769
准備相關腳本函數：
1 function [ y ] = GDMin2(fx,var,x,e,MAX) 2 % 最速梯度下降法求解函數極小點 3 % 參數描述------------------------------ 4 % fx 符號表達式 如fx = (x1-2)^4+(x1-2*x2)^2； 5 % var 符號變量列表 如：syms x1 x2;var= [x1;x2]; 6 % x 起始位置 7 % e 精度控制 8 % MAX 最大迭代次數控制 9 % ------------------------------ 10 11 if nargin < 5 12 MAX = 10;%設置默認最大迭代次數 13 end 14 precision = 3;%顯示精度控制 15 16 %開始循環迭代 17 %direction存貯搜索方向 18 %step 存貯步長 19 20 bfound = 0; 21 for k=1:1:MAX 22 direction = getNextDirecrion(fx,var,x); 23 disp('------------------------------'); 24 fprintf('d[%d]=:',k); 25 disp( vpa(direction',precision) ); 26 if normest(direction) <= e 27 y = x; 28 bfound = 1;%得到結果時置為1 29 break; 30 else 31 step = getNextStep(fx,var, x,direction);%計算步長 32 if isempty(step) 33 error('can not find a proper step.'); 34 end 35 %打印求解過程 36 fprintf('X[%d]=:',k); 37 disp( vpa(x',precision) ); 38 fprintf('step(%d)=: ', k); 39 disp( vpa(step,precision) ); 40 disp('------------------------------'); 41 x = x+step*direction;%計算下一個位置 42 end 43 end 44 if bfound == 1 45 disp('min value of:'); 46 disp( vpa( subs(fx,var,y),precision) ); 47 end 48 end 49 50 %根據位置xk,獲取搜索方向 51 function [direction] = getNextDirecrion(fx,var,xk) 52 53 gx = gradient(fx,var); %計算梯度函數 54 direction = -subs(gx,var,xk);%計算搜索方向 55 end 56 57 %根據位置xk和方向dk,獲取搜索步長step 58 %注意符號表達式求導數的根時返回值轉換為double類型 59 function [step] =getNextStep(fx,var,xk,dk) 60 61 syms lambda; 62 phix = subs(fx,var,xk+lambda*dk); 63 phix_diff = diff(phix); 64 step = double(solve(phix_diff,'Real',true));%求取導函數的實數根 65 end
在MATLAB命令行鍵入：
1 clear all; 2 syms x1 x2; 3 X = [x1;x2]; 4 fx = x1 - x2 + 2*x1^2 + 2*x1*x2 + x2^2; 5 x1 = [0;0]; 6 e = 0.3; 7 minVal = GDMin2(fx,X,x1,e)
結果：
------------------------------
d[1]=:[ -1.0, 1.0]
X[1]=:[ 0, 0]
step(1)=: 1.0
------------------------------
------------------------------
d[2]=:[ 1.0, 1.0]
X[2]=:[ -1.0, 1.0]
step(2)=: 0.2
------------------------------
------------------------------
d[3]=:[ -0.2, 0.2]
min value of:
-1.2
minVal =
-4/5
6/5
和手動結算的結果是一樣的在 x 取值 [-0.8, 1.2]的時候，取得滿足精度的值。
我們看前兩步：
畫出其迭代路線：
1 clear all; 2 clc; 3 close all; 4 x = 0:0.1:8 5 fx = (x(1,:) - 4).^2 6 7 8 plot(x, fx); 9 hold on 10 11 %% 標出點 12 plot(3, 1, '-ro'); 13 plot(4, 0, '-ro'); 14 hold on 15 16 %% 連線 17 %% plot([x1,x2...], [y1,y2,...]) 18 plot([3 4],[1 0], '-g'); 19 hold off
兩步就收斂；
現在，如果我們將初始值（startpoints）改為：x = 2；再根據上述求解過程，畫出迭代路線：
可以看到，起始點是 x = 2 比 x = 3的迭代次數多一次，所以選取一個好的初始值十分重要；同樣，在SLAM當中，例如：3D-3D映射；我們在RANSAC框架下，基於PNP/ICP 計算出位姿初始值，這樣一個初始值，在后續BA當中，是一個較為理想的初始值。