目录
- 前言
- 旋转矩阵，旋转向量，四元数
- 李群李代数
- BCH公式
- 非线性最小二乘
- 一阶和二阶梯度法
-
- 高斯牛顿法
-
- 列文伯格-马夸尔特方法(LM)
前言
读完高翔的视觉SLAM十四讲的总结前七讲
旋转矩阵，旋转向量，四元数
旋转矩阵 R：任何物体的运动都可以用一个3乘3的矩阵乘法来描述，需要明白旋转矩阵本身是个正交矩阵且行列式为1就好了。
参考：https://www.matongxue.com/madocs/228/
这其中对特征值和特征向量以及矩阵乘法的意义有很直观的表现。
旋转向量：用旋转角度和旋转轴四个变量描述旋转。
欧拉角：以XYZ(roll pitch yaw)为轴旋转以及旋转的先后顺序表示物体旋转。
四元数 q：用四个数字表示一次旋转，且旋转之间可以有运算法则。
欧式变换矩阵 T：一个4乘4的矩阵，同时表达了旋转和平移。构成如下：
[
b
1
]
=
[
R
t
0
1
]
[
a
1
]
−
−
−
b
^
=
T
a
^
\left[ \begin{matrix} b \\ 1 \end{matrix} \right]=\left[ \begin{matrix} R & t \\ 0 & 1 \end{matrix} \right]\left[ \begin{matrix} a \\ 1 \end{matrix} \right]---\hat{b}=T\hat{a}
[b1]=[R0t1][a1]−−−b^=Ta^
注：a,b都是3v向量，多加1维就是a,b的齐次坐标，t是一个3v向量表达平移，T存在是为了方便连续的旋转变换，比如c=T1* T2*a。
linux中通过Eigen几何模块可以直接完成以上变量关系之间的转换，Eigen本身是一个矩阵库。总之只需要知道旋转矩阵，旋转向量，四元数，欧式变换矩阵以及欧拉角之间都是可以互相转换，以及所代表的意思。
Eigen实现创建（片段）：
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
Isometry3d T = Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Vector3d(1, 3, 4));
Quaterniond q = Quaterniond(rotation_vector);
李群李代数
总的来说李群李代数就是对旋转矩阵和欧式变换矩阵做了一个总结。
李群：就是对特殊矩阵做了总结，将满足某种性质和某种运算规则的所有矩阵归结为一个总体叫做群
以下为旋转矩阵和欧式变换矩阵的李群的表达。
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(3)=\lbrace R\in R^{3 \times 3 }| RR^T = I,det(R)=1 \rbrace
SO(3)={R∈R3×3∣RRT=I,det(R)=1}
S
E
(
3
)
=
{
T
=
[
R
t
0
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3)=\lbrace T =\left[ \begin{matrix} R &t \\ 0 &1 \end{matrix} \right] \in R^{4 \times 4 }| R \in SO(3),t\in R^3 \rbrace
SE(3)={T=[R0t1]∈R4×4∣R∈SO(3),t∈R3}
李代数：通过李群和李代数的关系，可以用来表述李群的导数。
以下为旋转矩阵和欧式变换矩阵的李代数表达
s
o
(
3
)
=
{
ϕ
∈
R
3
,
Φ
=
ϕ
^
∈
R
3
×
3
}
so(3)=\lbrace \phi \in R^3, \Phi = \hat{\phi} \in R^{3 \times 3} \rbrace
so(3)={ϕ∈R3,Φ=ϕ^∈R3×3}
s
e
(
3
)
=
{
ξ
=
[
ρ
ϕ
]
∈
R
6
,
ρ
∈
R
3
，
ϕ
∈
s
o
(
3
)
,
ξ
^
=
[
ϕ
^
ρ
0
0
]
∈
R
4
×
4
}
se(3)=\lbrace \xi = \left[ \begin{matrix} \rho \\ \phi \end{matrix} \right]\in R^6,\rho\in R^3，\phi\in so(3),\hat{\xi}= \left[ \begin{matrix} \hat{\phi}&\rho \\ 0&0 \end{matrix} \right] \in R^{4 \times 4}\rbrace
se(3)={ξ=[ρϕ]∈R6,ρ∈R3，ϕ∈so(3),ξ^=[ϕ^0ρ0]∈R4×4}
注：上述中
ϕ
^
\hat{\phi}
ϕ^ 的表达方式意思为将该向量转化为反对称矩阵，这是一种表达向量内积的方式。
李群李代数关系
假设R是随t变化的一个函数也是矩阵。
d
d
t
R
(
t
)
=
ϕ
^
R
(
t
)
\frac{d}{dt}R(t)=\hat{\phi} R(t)
dtdR(t)=ϕ^R(t)
R
=
e
x
p
(
ϕ
^
)
R=exp(\hat{\phi })
R=exp(ϕ^)
李群对应的李代数就表示了对应旋转矩阵和旋转向量的关系
BCH公式
表达了李群乘法和李代数加法的关系，方便对旋转矩阵的微分求导操作。
假定某个旋转
R
R
R，对应李代数为
ϕ
\phi
ϕ，左乘一个微小旋转，记作
Δ
R
\Delta R
ΔR,对应李代数
Δ
ϕ
\Delta\phi
Δϕ.
根据BCH近似：
Δ
R
R
=
e
x
p
(
Δ
ϕ
^
)
e
x
p
(
ϕ
^
)
=
e
x
p
(
(
ϕ
+
J
−
1
(
ϕ
)
Δ
ϕ
)
^
)
\Delta R R=exp(\hat{\Delta\phi})exp(\hat{\phi})=exp((\phi+J^{-1}(\phi)\Delta\phi\hat) )
ΔRR=exp(Δϕ^)exp(ϕ^)=exp((ϕ+J−1(ϕ)Δϕ)^)
e
x
p
(
(
ϕ
+
Δ
ϕ
)
^
)
=
e
x
p
(
(
J
Δ
ϕ
)
^
)
e
x
p
(
ϕ
^
)
exp((\phi+\Delta\phi\hat))=exp((J\Delta\phi\hat))exp(\hat\phi)
exp((ϕ+Δϕ)^)=exp((JΔϕ)^)exp(ϕ^)
由此可以推导出扰动模型，左乘的微小旋转就是扰动。
假设一个空间点p，R对它做了旋转，在旋转基础上加入扰动
Δ
R
\Delta R
ΔR。
旋转矩阵对扰动的旋转向量的导数
∂
(
R
P
)
∂
Δ
ϕ
=
−
(
R
p
)
^
{\partial(RP)\over{\partial\Delta\phi}}=-(Rp\hat)
∂Δϕ∂(RP)=−(Rp)^
同理，在
S
E
(
3
)
SE(3)
SE(3)上的李代数求导。
假设
p
p
p经过一次变换
T
T
T，对应李代数
ξ
\xi
ξ,给
T
T
T加入扰动
Δ
T
=
e
x
p
(
δ
ξ
^
)
\Delta T=exp(\delta\hat\xi)
ΔT=exp(δξ^)，设左扰动
δ
ξ
=
[
δ
ρ
,
δ
ϕ
]
T
\delta\xi=[\delta\rho,\delta\phi]^T
δξ=[δρ,δϕ]T。
欧式变换矩阵对扰动变换矩阵的李代数的导数：
∂
(
T
p
)
∂
δ
ξ
=
[
I
−
(
R
p
+
t
)
^
0
0
]
=
(
T
p
)
⨀
{\partial(Tp)\over\partial\delta\xi}=\left[ \begin{matrix} I & -(Rp+t\hat) \\ 0 & 0 \end{matrix} \right]=(Tp)^{\bigodot}
∂δξ∂(Tp)=[I0−(Rp+t)^0]=(Tp)⨀
以上推导过程都不太重要，记录以下矩阵对矩阵的求导顺序：
d
[
a
b
]
d
[
x
y
]
=
[
d
a
d
x
d
a
d
y
d
b
d
x
d
b
d
y
]
{\mathrm{d}\left[ \begin{matrix} a \\ b \end{matrix} \right]\over\mathrm{d}\left[ \begin{matrix} x \\ y \end{matrix} \right]}=\left[ \begin{matrix} \mathrm{d}a\over\mathrm{d}x & \mathrm{d}a\over\mathrm{d}y\\ \mathrm{d}b\over\mathrm{d}x &\mathrm{d}b\over\mathrm{d}y \end{matrix} \right]
d[xy]d[ab]=[dxdadxdbdydadydb]
非线性最小二乘
最简单的如下式，就是
f
(
x
)
f(x)
f(x)是一个非线性函数，找到x使得该函数取最小值。
应用例子就比如，构造函数的物理意义是误差，找到一个值使得误差最小。
m
i
n
F
(
x
)
=
1
2
∣
∣
f
(
x
)
∣
∣
2
(1)
minF(x)={1\over 2}||f(x)||^2\tag1
minF(x)=21∣∣f(x)∣∣2(1)
而因为非线性，函数长的乱七八糟的，所以一般用迭代的方式慢慢找。
具体步骤：
1,给定某个初始值
x
0
x_0
x0
2,寻找
Δ
x
\Delta x
Δx,使得
∣
∣
f
(
x
0
+
Δ
x
∣
∣
2
||f(x_0+\Delta x||^2
∣∣f(x0+Δx∣∣2达到极小值
3,若
Δ
x
\Delta x
Δx足够小，就停止迭代
4,
x
0
=
x
0
+
Δ
x
x_{0}=x_0 +\Delta x
x0=x0+Δx 更新初始值并返回2
接下来的算法都是为了寻找增量
Δ
x
\Delta x
Δx
一阶和二阶梯度法
对上面的(1)式在
x
x
x附近进行泰勒展开。
F
(
x
k
+
Δ
x
k
)
≈
F
(
x
k
)
+
J
(
x
k
)
T
Δ
x
k
+
1
2
Δ
x
k
T
H
(
x
k
)
Δ
x
k
⋅
⋅
⋅
⋅
⋅
⋅
F(x_k+\Delta x_k)\approx F(x_k)+J(x_k)^T\Delta x_k+{1 \over 2}\Delta x_k^T H(x_k) \Delta x_k \cdot\cdot\cdot\cdot\cdot\cdot
F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk⋅⋅⋅⋅⋅⋅
J
(
x
k
)
J(x_k)
J(xk)是
F
(
x
)
F(x)
F(x)的一阶导数（一阶梯度），同理
H
H
H是二阶梯度。
一阶梯度法
保留一阶梯度，都知道一阶导数都代表函数的增长方向，所以只要让
x
0
x_0
x0朝着反方向移动就好了，而移动长度由
Δ
x
\Delta x
Δx大小决定，随着迭代
x
0
=
x
0
+
Δ
x
x_0 = x_0 + \Delta x
x0=x0+Δx，使得
F
(
x
+
Δ
x
)
F(x+\Delta x)
F(x+Δx)越来越小。
二阶梯度法（牛顿法）
保留二阶梯度，可以得到如下函数：
F
(
x
+
Δ
x
)
≈
F
(
x
)
+
J
(
x
)
T
Δ
x
+
1
2
Δ
x
T
H
(
x
)
Δ
x
F(x+\Delta x)\approx F(x)+J(x)^T\Delta x+{1 \over 2}\Delta x^T H(x) \Delta x
F(x+Δx)≈F(x)+J(x)TΔx+21ΔxTH(x)Δx
于是为了找到
Δ
x
\Delta x
Δx使得上述式子变小，只要让右边部分最小就好了。
Δ
x
∗
=
m
i
n
(
J
(
x
)
T
Δ
x
+
1
2
Δ
x
T
H
(
x
)
Δ
x
)
\Delta x^{\ast} = min(J(x)^T\Delta x+{1 \over 2}\Delta x^T H(x) \Delta x)
Δx∗=min(J(x)TΔx+21ΔxTH(x)Δx)
上述式子对
Δ
x
\Delta x
Δx求导数令其为零。化简得到：
J
+
H
Δ
x
=
0
⇒
H
Δ
x
=
−
J
J+H\Delta x = 0 \Rightarrow H \Delta x = -J
J+HΔx=0⇒HΔx=−J
求解以上线性方程得到
Δ
x
\Delta x
Δx。
方法缺点：二阶矩阵很难求，计算量大，且为了解方程要求矩阵逆。
高斯牛顿法
对式子(1)中的
f
(
x
)
f(x)
f(x)进行泰勒展开：
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
T
Δ
x
f(x+\Delta x)\approx f(x)+J(x)^T\Delta x
f(x+Δx)≈f(x)+J(x)TΔx
将这个式子带到
F
(
x
)
F(x)
F(x)中
Δ
x
∗
=
m
i
n
(
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
)
\Delta x^{\ast} = min({1\over 2}||f(x)+J(x)^T\Delta x||^2)
Δx∗=min(21∣∣f(x)+J(x)TΔx∣∣2)
最后化简得到
J
(
x
)
J
T
(
x
)
Δ
x
=
−
J
(
x
)
f
(
x
)
⟹
H
Δ
x
=
g
J(x)J^T(x)\Delta x =-J(x)f(x)\Longrightarrow H\Delta x = g
J(x)JT(x)Δx=−J(x)f(x)⟹HΔx=g
高斯牛顿法的好处是，用
J
(
x
)
J
T
(
x
)
J(x)J^T(x)
J(x)JT(x) 来近似
H
H
H ，但是同样要求逆，而且因为近似的矩阵半正定，所以不知道能不能求逆。
代码实现
求解曲线拟合问题，用最小二乘估计曲线参数，a，b，c。
e
=
m
i
n
1
2
∑
1
n
∣
∣
y
i
−
e
x
p
(
a
x
i
2
+
b
x
i
+
c
)
∣
∣
2
e=min{1\over 2}\sum_1^n||y_i-exp(ax_i^2+bx_i+c)||^2
e=min211∑n∣∣yi−exp(axi2+bxi+c)∣∣2
在看代码之前，已知我们有一组数据，存放在y_data和x_data数组中。
其中x_data为0～100，y_data为x在0~100时所探索到的数据，程序中我们是默认y_data自带干扰，且干扰符合高斯分布。
手写（片段）
for (int iter = 0; iter < 100; iter++) {
Matrix3d H = Matrix3d::Zero();
Vector3d b = Vector3d::Zero();
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i];
double error = yi - exp(ae * xi * xi + be * xi + ce);
Vector3d J;
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);
J[1] = -xi * exp(ae * xi * xi + be * xi + ce);
J[2] = -exp(ae * xi * xi + be * xi + ce);
H += inv_sigma * inv_sigma * J * J.transpose();
b += -inv_sigma * inv_sigma * error * J;
cost += error * error;
}
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
}
用Ceres实现（片段）
先重装载误差函数，也就是设置e函数
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
template<typename T>
bool operator()
(const T *const abc, T *residual)
const {
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
return true;
}
const double _x, _y;
};
double ae = 2.0, be = -1.0, ce = 5.0;
double abc[3] = {ae, be, ce};
ceres::Problem problem;
for (int i = 0; i < N; i++) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>( new CURVE_FITTING_COST(x_data[i], y_data[i]) ) , nullptr, abc );
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
列文伯格-马夸尔特方法(LM)
在高斯牛顿法的基础上给
Δ
x
\Delta x
Δx添加了信赖区间，也就是说
Δ
x
\Delta x
Δx 在信赖区间内，说明变化是可取的，而信赖区间的大小刻画由
\quad
实际下降/近似下降。
ρ
=
f
(
x
+
Δ
x
)
−
f
(
x
)
J
(
x
)
T
Δ
x
\rho = {f(x+\Delta x)-f(x)\over J(x)^T\Delta x}
ρ=J(x)TΔxf(x+Δx)−f(x)
也就是说当
ρ
\rho
ρ大于某阈值时，近似时可行的，再更新迭代的参数。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
