目的： 因為雜訊！為了減少雜訊帶來的影響。
現實世界點p，在相機座標系下座標p(x,y,z),落在投影平面上的座標為(u,v)，一般把(u,v)寫成齊次座標；
所謂齊次座標，是為了方便進行矩陣的運算，在向量上面加乙個維度，一般把增加的維度預設成1;
焦距fx,fy,相機座標系和成像平面的平移cx,cy都作為內參寫進k（內參矩陣）；
z (u
,v,1
)=kp
−−−−
−−
−z (u,v,1) = k p-------
z(u,v,
1)=k
p−−−
−−−−
*
把p換成世界座標系下的pw表示：p = r pw + t = t pwz puv = k t pw
因為p=(x,y,z), *式兩邊同除以z ，得puv = k t pw 『
其中，t pw『=（x，y，z）/z=（x/z，y/z，1），這個叫做歸一化座標；相機前z=1的平面稱為歸一化平面。
群： 集合 + 運算李群是連續的群，能夠在空間內連續運動；例如：特殊正交群：so(3): 旋轉矩陣 + 矩陣乘法
特殊歐氏群：se(3):變換矩陣 + 矩陣乘法
李代數：每個李群都有對應的李代數反對稱矩陣的定義就是at=兩個向量a,b的叉積，可以寫成a的反稱矩陣乘以向量b
旋轉矩陣的微分是乙個反對稱(也叫斜對稱)矩陣左乘它本身
−a
a^t = -a
at=−a
a ⃗×
b⃗=a
∧b
⃗\vec \times \vec= a^ \, \vec
a×b=a∧
b^ 把向量變成其對應的反對稱矩陣
優化問題的目標就是找到一組引數，使得目標函式取極值；
比如slam的最小重投影誤差就是讓目標函式j對t求導，因為旋轉矩陣對加法運算不封閉，我們將對李代數求導來間接的對變換矩陣求導了。我們要求解這個最佳的位姿t，一般採用迭代優化，每次迭代都更新乙個位姿的增量delta，使得目標函式最小。這個delta就是通過誤差函式對t微分得到的。
矩陣的微分r˙=
\dot =
r˙=[0−
ϕ3ϕ2
ϕ30−
ϕ1−ϕ
2ϕ10
]\begin 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end
⎣⎡0ϕ3
−ϕ2
−ϕ
30ϕ
1ϕ
2−ϕ
10
⎦⎤r(t
)r(t)
r(t)
= φ∧
φ^φ∧
r (t
)r(t)
r(t)
r =e
xp(φ
∧)
r = exp(φ^)
r=exp(
φ∧)
向量φ=（φ1，φ2，φ3）反應了r的導數性質，故稱它在so(3)上的原點 φ0 附近的正切空間上。這個φ正是李群大so(3)對應的李代數小so(3)。
即：李群空間的任意乙個旋轉矩陣r都可以用李代數空間的乙個向量的反對稱矩陣的指數來近似。
對於任意反對稱矩陣，都能找到乙個與之對應的向量， a^ = a（倒^）∨則 a ∨=
aa^\vee=a
a∨=a
\vee
∨把矩陣變成向量；
exp(ξ^)是指 指數對映，即矩陣的指數；可以用泰勒展開；討論觀測方程：在xk處對路標yj進行了一次觀測，對應到影象上的畫素zk,j；
通過帶雜訊資料推斷位姿和地圖，構成了乙個狀態估計問題。即求p(x|z)；
p(x|z)叫後驗概率，一般是我們求解的目標；p(z|x) 是條件概率，又叫似然，一般是通過歷史資料統計得到。一般不把它叫做先驗概率，但從定義上也符合先驗定義。
利用貝葉斯公式，p(x|z)=p(z|x) p(x)/p(z)即「在什麼樣的狀態下，最有可能產生現在觀測到的資料」因為直接求後驗概率是困難的，假設不知道位姿是什麼，也就沒有了先驗概率p(x),
那問題就轉化為找到乙個x,使p(z|x)最大。
p(z|x)~n(h,q)
滿足高維高斯分布
化簡後，得到乙個誤差的和函式，即最小二乘問題(min(||f(x)^2||2))
問題轉變成了求極值的問題：
參考：我們將原問題找f(x)的最小值（此處x為n維向量，不是一維的），變為找到f（x+δx)的最小值，再依此迭代多次，逼近f(x)的最小值。此時，x變為已知，而未知是δx的大小（當然也可以理解為方向，因為n維向量表示了乙個方向）。也就是說，我們希望找到乙個δx，使得函式f（x+δx）在點x處取得最小值。
最速下降法：利用||f(x)||2
2的泰勒展開，保留一階，得到解，delta x=- jt(x)
即：沿著梯度反方向前進可以得到最快的下降；
高斯牛頓法： f(x+δx) ≈ f(x)+ j(x)δx,問題轉化為求arg min∣∣f
(x)+
j(x)
δx∣∣
22
||f(x)+j(x)δx||^2_ 2
∣∣f(x)
+j(x
)δx∣
∣22
對δx求導，另為0;得到下列增量方程：
j(x)t j(x) δx= - j(x)t f(x)列文伯格-馬誇爾特法：給δx加上置信區間定義左邊的係數為h，右邊的係數為g
得 hδx=g
非線性優化
問題的一般表達 x 是 n 維實向量 x x x t in r n,s是 r n 的子集，f 0 x f m x 是x相關的實值函式。我們所要解決的主要是以下最小問題的變體 min f 0 x s.t.f j x 0,j 1.m,x in s,其中 可以是 geq,leq or f 0 是問題的目標...
matlab 非線性優化
求解非線性問題 min z f x s.t.c x 0,ceqx 0,ax b,aeqx beq,lb x ub.x,fval,exitflag,output,lambda,grad,hessian fmincon fun,x0,a,b,aeq,beq,lb,ub,nonlcon,options,p...
非線性優化 高翔
高斯牛頓法 include include include include include using namespace std intmain 開始gauss newton 迭代 int iterations 100 迭代次數 double cost 0,lastcost 0 本次迭代的cost...