激光SLAM学习
扫描一圈返回一次数据：LOAM（3D）10Hz，100ms； 2D：50Hz，20ms
1）传感器数据处理：激光雷达去畸变，里程计校正
2）前端帧间匹配：激光帧间（算法核心）：提供一个尽可能准确的初始解
ICP，PI-ICP ， NDT， CSM
3）激光回环检测
scan-to-scan:当前帧和历史帧,
scan to map：当前帧和历史子图匹配cartographer,
map to map：当前子图和历史子图匹配
4）后端优化：非线性最小二乘,GN,LM
算法上限由硬件决定，下限由算法决定
发展：
数据预处理--非常重要
轮式里程计标定
激光雷达畸变去除
不同系统的时间同步
应用中的问题：
动态：动态物体、环境变化 ------------适应环境变化
几何结构相似：闭环错误
全局定位：一般GPS提供初始范围
建图操作复杂：路线？大回环套小回环？
里程计：地面材质变化，地面不平，机器人载重改变
坐标定义：T_A_B: B在A中的坐标；B到O坐标系的转换
当三维坐标点发生旋转时，如果采用矩阵运算就会需要考虑“左乘”和“右乘”。若绕静坐标系（世界坐标系）旋转，则左乘，也是变换矩阵*坐标矩阵；若是绕动坐标系旋转（自身建立一个坐标系），则右乘，也就是坐标矩阵*变换矩阵。
正常我们用到的是右乘。
激光雷达建图的问题和挑战：
--------------------------------------------------------------------------------------------------------------------------------------------------------
传感器处理之轮式里程计运动模型及标定：
圆弧运动：
标定：*直径与给定不等，不同场合地面软硬程度不一样，导致直径变化（bl,br变化）
最小二乘：
Ax=b A：m*n
解：https://blog.****.net/qq_33628848/article/details/79606596
解的结构：https://blog.****.net/stranger_man/article/details/80852095
Ax=0的解：https://blog.****.net/lennon_w/article/details/84246450
只有零解时,R(A)=n 特别得 当A是方阵时 |A|≠0。 有非零解时,R(A)<n 特别得 当A是方阵时 |A|=0
齐次线性方程组 有非零解的充要条件是r(A)<n。即系数矩阵A的秩小于未知量的个数。（列满秩）
m>n时，通常无解：b不在A的列空间里，寻找最小二乘解：b在列空间的投影最大
1）A列满秩时(r=n<=m)，Ax=b有0或者1个解：
m<n:无穷多解
2）当A行满秩（r=m<=n）Ax=b的解必定存在且有无穷多个解
但是有一些行是*列，因而*变量个数为n-r=n-m
3）m=n，且A可逆，唯一解：对于任意的b，可逆方阵存在且具有唯一解
4）A不满秩（r<m且r<n）要么无解，要么无穷多解
rankA=rankA|b 无穷多解
否则：无解
rankA=rankA|b，有解条件
里程计标定：
直接线性法：DLT
思路比较简单，直接求线性映射
求得A就是修正矩阵，使用时 [dx,dx,dtheta]' = A[dx,dx,dtheta]'
求解两轮直径和轮间距，以激光雷达观测为真值，假设激光雷达和轮速计同一坐标系，不同坐标系的论文：
开源了：Simultaneous calibration of odometry and sensor parameters for mobile robots
--------------------------------------------------------------------------------------------------------------------------------------------------------------
激光雷达运动畸变去除：
激光雷达分类：http://baijiahao.baidu.com/s?id=1601800649597130555&wfr=spider&for=pc
1）测距原理：
三角测距（双目）：与基线反比，
飞行时间（ToF）：SICK。。。激光雷达，
病态的：杂乱环境下，小的位姿变化导致大的输出变化，所以这个光束模型不怎么用
似然场模型：对障碍进行高斯平滑（膨胀），得分通过查表得到
产生原因：扫描一圈过程中机器人运动了，导致一圈的点不是相对于同一原点（位置，方向：方向影响更大）的
运动畸变极大影响匹配精度，所以第一步就是校正，类似于卷帘相机
去除方法：
1）纯估计方法：类ICP方法
已知匹配点时的求解：
VICP：在匹配过程中考虑机器人的运动速度，但是匀速运动假设
扫描一圈时，知道起点速度，匀速运动，后面每一个点的位置都可以根据这个速度和于第一个点的时间差确定
用轮速计在处理器上处理：单片机上处理导致输出数据有延时
插值：https://blog.****.net/xbinworld/article/details/65660665
二次插值：认为是匀加速运动，更合理一点
跟简单的方法 ：分段函数近似
激光数据：范围和角度
3）两种方法结合：
位置误差的线性假设比位置线性假设更合理！！！