---
# System prepended metadata

title: 視覺SLAM十四講 心得
tags: [SLAM]

---

---
title: '視覺SLAM十四講 心得'
disqus: hackmd
---

視覺SLAM十四講 心得
===


## Table of Contents

[TOC]

## 第2講 ： 初識SLAM
### 經典視覺slamd框架
![](https://i.imgur.com/WkhHl0j.png)
### slam 整體概念
![](https://i.imgur.com/mGsHiL1.png)



## 第6講 : 非線性優化 ＆ 第10講 ： 後端
---
### MAP -> MLE -> LSQ
### Kalman filter
### G-N & L-M 
### Ceres & g2o 
### 整合
![](https://i.imgur.com/YwST2Nv.png)


## 第10講 ： 後端：BA & 第5講 ： 投影模型 & graph SLAM
---
### 投影模型
![](https://i.imgur.com/H2frk0y.png)
### BA
![](https://i.imgur.com/YHoQPA4.png)

### 整合
![](https://i.imgur.com/4eBFP56.png)

### Hessian Matrix
#### 稀疏性

#### 邊緣化
##### Margalization
1. * [SLAM中的marginalization 和 Schur complement](https://blog.csdn.net/heyijia0327/article/details/52822104)
##### FEJ 
1. FEJ in EKF
    * use the first time estimate measurement for Jacobian
2. FEJ in Margalization
    * use the linearization point for Jacobian in the timestamp when margalization
    * we keep using linearilzation point ζ0(margalization point) and use this fixed Jacobian during G-N method instead of recaculating Jacobian in every interative timestamp
    ![](https://i.imgur.com/olyoxyL.png)

3. *  [SLAM中的First-Estimates Jacobian](https://www.zhihu.com/question/52869487)
    * [EKF中的consistency](https://www.zhihu.com/question/59784440/answer/170886408)
    
##### consistency
1. consistency
    * a state estimator is consistent if the estimation errors 
      *    (i) are zero-mean, and 
      *    (ii) have covariance matrix smaller or equal to the one calculated by the filter.

2. Observability
      * unobservable state in 2D SLAM : 3 -> 2
        * since in SLAM only *relative* measurements between the robot and landmark are available, the *global position and orientation* of the state vector cannot be determined.
      * unobservable state in VSLAM   : 7 -> 6
      * unobservable state in VISLAM  : 4 -> 3
      * unobservable DOM uncertainty will grow unbounded, so when inconsistency happened, it violate the consistency of a filter(have covariance matrix smaller or equal to the one calculated by the filter)
3. nonexistent information->reduce uncertainty in global yaw -> reduce all state uncertainty -> filter inconsistency -> filter unrealiable
4. 对于一组状态[x,y]，将x边缘化计算y的分布时，需要选择y的状态点进行线性化，假设是在y0处。之后对状态进行扩充变为[y,z]并进行了状态更新（观测到新的landmark，同时有新的相机pose），这时重新构建正规方程Hx=b时还要继续进行线性化。*但y的状态有可能从y0已经更新到y1，导致线性化点的不同*。这就造成：先验分布对应y0，对H贡献部分称H0；新的[y,z]进行线性化时对应y1，对H贡献部分称H1。正规方程实际是 ==(H0+H1)xb==。这同样会造成inconsistency，来源同样是能观性的改变。借用DSO作者的图（见下方附图），对同一个非线性函数在不同状态点线性化时，H零空间会退化，而H零空间维数与能观自由度个数有关（这里参考VSLAM或视觉惯性导航系统能观性相关的文献），这种退化导致能观性改变，进而会影响consistency。

5. 
![](https://i.imgur.com/OL91N2T.png)

6. *  [Decoupled, Consistent Node Removal and
Edge Sparsification for Graph-based SLAM](https://rss16-representations.mit.edu/papers/BeyondGeometryRSSW16_7_CameraReadySubmission_Eckenhoff.pdf)
    * [A First-Estimates Jacobian EKF for Improving
SLAM Consistency](http://people.csail.mit.edu/ghuang/paper/Huang2008ISER.pdf)





### DSO 中的非線性優化
1. *  [DSO 中的Windowed Optimization](https://blog.csdn.net/heyijia0327/article/details/53707261)
   * [DSO详解 半闲居士](https://zhuanlan.zhihu.com/p/29177540)
   * [DSO之求导篇](https://zhuanlan.zhihu.com/p/77002568)
    * [DSO windowed optimization 代码 (1)](https://www.cnblogs.com/JingeTU/p/8395046.html)    
2.  * [My DSO NOTE](https://hackmd.io/p4eMPxYkRYSK5AHjRGh5sw?both)

## 第3講 ： 三維空間剛體運動
### Content
#### Rotational matrix & Transform matrix & homogenous frame
![](https://i.imgur.com/LrJPOfy.jpg)
```C++=
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
```
```C++=
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
```
#### Axis angle
其實是 so(3) 的幾何空間
```C++=
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
```
#### Euler angle
![](https://i.imgur.com/fxhHsvR.png)
* one rotation => 3 rotation relative to 3 axis
* rpy : yaw-pitch-roll(ZYX) is the most popular one
```C++=
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
```
* really intuitive but have singularity

#### Quaternion
![](https://i.imgur.com/PK80HYy.png)
```C++=
q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
```
==Quternion construct is in w x y z order,
while its coff is in      x y z w order==
### Eigen
#### some note
*  initialization
    * general
 ```C++=
Eigen::Matrix<float, 2, 3> matrix_23;
```                
* special
```C++=
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
```
* manuplilation
 ```C++=
matrix_33.transpose()     // 转置
matrix_33.sum()             // 各元素和
matrix_33.trace()         // 迹
10*matrix_33                // 数乘
matrix_33.inverse()        // 逆
matrix_33.determinant()   // 行列式
 ```
 * 訪問
  ```C++=
for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }
    for(size_t ii = 0; ii < 6; ii++){
        cout << matrix_23(ii) << " ";
    }
 ```
 * ==all have multiply operator overload!==
 *  data type need to be consistent
```C++=
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
```
 
* size need to be correct
 ```C++=
Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
 ```
#### Conversion
![](https://i.imgur.com/hDA0CAx.jpg)

## 第4講 ： 李群李代數
### Why lie algrebra?
![](https://i.imgur.com/aqPQqx2.png)
常常要處理J對位姿T的偏微分，但SO(3),四元數,歐拉角都有約束
## 整合
![](https://i.imgur.com/REE84xE.jpg)
## DSO 中李代數運算
![](https://i.imgur.com/ovKqLvV.png)
 ： 在原本位姿上左乘微小擾動（更新量）,其中xi_hat 為將delta_T 對應的李代數透過指數映射回SE(3)

## 第5講 ： 相機與圖像
### 整合

## 第7講 ： 前端（1）- 特徵點法
## 第8講 ： 前端（2）- 直接法
### 整合
### DSO 中的前端

###### tags: `SLAM` 