SLAM總結（一）

SLAM（Simultaneous Localization and Mapping）：同時定位和建圖，定位是定位機體在世界坐標系下的位姿（pose、transformation），單傳感器機體一般指相機光心、激光雷達scan中心、IMU中心、編碼器兩輪軸心，多傳感器一般使用IMU中心，可以避免離心力的影響，位姿包括3個自由度的位置（translation）和3個自由度的姿態（rotation），世界坐標系的原點和姿態可以由第一幀關鍵幀、已有的全域地圖或路標點、GPS（全世界一樣，真正意義的世界坐標）得到，建圖是建立機器人所感知周圍環境的地圖，地圖基本幾何元素是點，點沒有方向，只有3個自由度的位置，可以是稀疏點、稠密點、柵格地圖、八叉樹地圖和拓撲地圖等，地圖的主要作用是定位、導航，導航可拆為導和航，導包括全域規劃和區域規劃，航即規劃完后控制機器人運動，稀疏點一般只能用于定位；柵格地圖、八叉樹地圖可用于定位、導航，稠密地圖可以用于定位，處理后可轉換成柵格地圖或八叉樹地圖用于導航，
總之，常見的SLAM問題是估計由大量離散的6自由度的機體pose（定位）和3自由度點（建圖）構成的n維變數，那么時間和運動范圍共同造成n增大，從而造成CPU和記憶體開銷變大，拓展一下，把“同時定位和建圖”改成“定位或建圖”，那么可以分為下面幾種情況：
1） 機體pose和點完全未知：這種情況便屬于SLAM問題
2）機體pose完全已知和點完全未知：這便是純建圖問題，比如在ORB-SLAM2跑的程序中把關鍵幀對應圖片保存下來，跑完后進行一次全域優化，此時得到完全已知的pose，再利用已知pose和圖片建圖，
3)機體pose完全未知和點完全已知：這便是純定位問題，
4）機體pose完全未知和點部分已知：這些點一般可以作為路標使用，認為它們是沒有累積誤差的，可以利用它們使得每次開機都在同一世界坐標系下和減小累積誤差，
5）理論上有3*3=9中情況，其他情況不太常見就不一一例舉

SLAM框架如下圖所示，整個系統由前端、后端和回環檢測組成，

1）前端：頻率為傳感器的幀率，資料關聯（如特征點匹配、光流法等）、初始化、通過幾何方法或很小范圍優化快速得到當前幀的機體pose的較準確的初值（跟蹤，Tracking），當前幀一般只參照前一幀或前幾幀，故隨著關鍵幀增多累積誤差會變大，
2） 后端：頻率為關鍵幀率，關鍵幀需要在保證跟蹤質量的同時盡量減少幀的數量，幀的時間間隔主要與機器人的運動線速度、角速度（速度越快更容易跟蹤丟失）、視野（距離太近更容易跟蹤丟失）和環境中的特征特征結構（特征點稀疏和拐角更容易丟失）有關，
功能是通過三角化得到新的點在世界坐標系下較準確的位置初值、剔除或合并一些舊的點、添加約束和優化較大范圍的區域視窗中的關鍵幀pose和點位置，可以較長一段時間進行一次全域優化，
3） 回環檢測：頻率不確定，一般遠遠低于前兩個，與回環數量有關，會設定最高頻率使得不在短距離內檢測倍訓，通過資料關聯找到之前到過的地方，發現回環后會先使用相似變換的方法（3D-3D）調整與倍訓幀關聯的關鍵幀的位姿，然后優化回環內的所有關鍵幀和點，最后再進行一次全域優化，相對于普通的全域優化，倍訓檢測后的全域優化更容易收斂，
4） 建圖：一般地，通過前幾步可以得到稀疏點地圖，根據不同需求可以建立其他格式地圖，

SLAM分類按方法分類如下圖，本人的主要研究的是基于關鍵幀的幾何特征法，對多傳感器和多幾何特征的融合、增量式非線性優化興趣比較大，以下內容主要討論基于非線性優化的幾何特征法，

前端：常用的視覺幾何特征是點，但最近幾年對線、面和物體的研究越來越多，一些人致力于將它們用一個統一的數學運算式表達出來，便于處理；一些人致力于給物體加上語意，結合生活實際賦予它們特性，比如人是經常動的、不能撞的；一些人致力于估計動態物體的pose和運動速度，這些是我們生活中常見的幾何特征，加以利用勢必提高演算法的魯棒性和精度，給物體賦予語意也會使得機器人變得更加智能，

后端：對于非線性優化，關鍵是目標函式（約束、殘差項）、目標函式對變數的Jacobi矩陣和增量的求解方法，增量的求解一般使用LM方法，最后轉化成了求解線性方程AX=b，求解方法包括CSparse、Cholesky、Preconditioned Conjugate Gradient(PCG)-CG改進、QR，求解之前還可以使用schur、plain、reording等方法對系數矩陣A進行處理，加快計算速度

拓展
1）動態SLAM：SLAM一般假設環境是靜止的，那么當空間中存在動態物體時，勢必打破這一假設，當動態物體占據傳感器資料較少一部分時，資料關聯、RANSAC、優化的魯棒核濾去動態物體，因為相對于靜態點來說，動態物體上的點是outlier（外點），但動態物體占據傳感器的大部分時，動態點更像是inlier（內點），以上方法無法將動態物體濾去，從而動態SLAM應運而生，顧名思義，也就是在動態環境中運行的SLAM演算法，它除了估計靜態點的3D位置外，還能估計動態點的3D位置和速度，其中，SLAM_MOT（Muti-object tracking）比較具有代表性，在自動駕駛汽車上應用較多，它除了完成SLAM任務外，還能跟蹤環境中的動態物體，
2)語意SLAM：語意SLAM一般會與CV中的語意分割、物體識別等任務相結合，可以充分利用視覺特征中的語意資訊，賦予環境中的物體語意，使得機器人能夠更好地與環境互動，使得機器人更加智能，
3)Active SLAM：SLAM一般都是在人或者在導航演算法地控制下運動建圖，這樣的話，機器人的運動并不一定有利于定位和建圖，Active SLAM就是通過演算法控制機器人按有利于定位和建圖的軌跡運動，比如機器人主動（active）利用已有地圖，探索未知區域、通過倍訓等減小累積誤差和提高魯棒性，

作為技術篇的開篇之作，簡單敘述一下自己的學習SLAM的經歷以及一些主要的參考文獻，轉眼研究生生活已過一半，從作業到現在，嘗試過開關電源、嵌入式開發、機器學習、影像處理、CV和NLP，最后選擇了SLAM，從《視覺SLAM十四講》入門，然后開啃ORB-SLAM2論文和原始碼，拿下ORB-SLAM2后，差不多一兩個星期可以初步搞定一個開源演算法，從多傳感器融合的VINS-Mono和PL-VIO到lidar演算法Cartographer，感嘆一下，carto這原始碼是我看過最繞的代碼，得像剝洋蔥一層一層扒開，終于知道《C++ primer》是怎么用的了！ 得益于導師的放養，自己能夠到公司實習，通過專案加深對上面幾個演算法的理解，感覺自己遇到了全天下最好的導師，一心為學生著想，經過一些實習面試，發現自己對知識掌握的深度還不太夠，于是決定停下來好好總結一下再去探索新的演算法，因此，決定重啟CSDN，將四個演算法放在一個系列一起總結對比一下，另外也會單獨更詳細結合原始碼決議各個演算法，希望通過這種方式加深自己的理解，也能為SLAM的愛好者提供一些幫助，希望感興趣的朋友點進來看一看，多多指正！以下是本系列（SLAM總結）一些主要參考文獻
1） ORB-SLAM2
[1]ORB-SLAM: Tracking and Mapping Recognizable Features（2014）
[2]ORB-SLAM: A Versatile and Accurate Monocular SLAM System（2015）
[3]ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras（2017）
[4]Fast角點：Faster and Better: A Machine Learning Approach to Corner Detection
[5]Brief描述子：Binary robust independent elementary features
[6]ORB特征：ORB_an efficient alternative to SIFT or SURF
[7]詞袋：Bags of Binary Words for Fast Place Recognition in Image Sequences
詞袋相似度計算（需采集資料集大量特征，《視覺SLAM十四講》P309）
[8]Video google: A text retrieval approach to object matching in videos
[9]Understanding inverse document frequency: on theoretical arguments for idf
[10]圖優化：g2o：A General Framework for Graph Optimization
[11]H分解出t、R：Motion and structure from motion in a piecewise planar environment
[12]E的SVD分解：Multiple View Geometry in Computer Vision
[13]RANSAC：Random sample consensus:a paradigm for model fitting with applications to image analysis and automated cartography
[14]倍訓檢測和重定位方法：Fast Relocalisation and Loop Closing in Keyframe-Based SLAM
[15]2D-2D三維重建（5點法）：An efficient solution to the five-point relative pose problem
[16]重定位時獲得當前幀的初始位姿3D-2D方法（EPnP）： An accurate On solution to the PnP problem
倍訓檢測中相似變換、pose graph optimization：
[17]Scale Drift-Aware Large Scale Monocular SLAM
[18]closed-form solution of absolute orientation using unit quaternions
[19]3D-3D點（倍訓檢測用到）求解R、T: Least-Squares Fitting of Two 3-D Point Sets
2）VINS-Mono（多傳感器）：與1中一樣的論文就不再羅列
[1]VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
[2]VIO：Monocular visual-inertial state estimation for mobile augmented reality
[3]重定位、倍訓檢測和地圖融合：Relocalization, global optimization and map merging for monocular visual-inertial SLAM
[4]EuRoc相機模型及標定方法：Single View Point Omnidirectional Camera Calibration from Planar Grids
[5]初始化：Robust initialization of monocular visual-inertial estimation on aerial robots
[6]時間偏移標定：Online Temporal Calibration for Monocular Visual-Inertial Systems
[7]Harris角點：Good features to track
[8]KLT稀疏光流：An iterative image registration technique with an application to stereo vision
IMU預積分：
[9]Tightly-coupled monocular visualinertial fusion for autonomous flight of rotorcraft MAVs
[10]On-manifold preintegration for real-time visual–inertial odometry
[11]IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation
[12]邊緣化方法（schur）：Sliding window filter with application to planetary landing
3）PL-VIO（多傳感器、點線特征融合）：與1中一樣的論文就不再羅列
[1] PL-VIO: Tightly-Coupled Monocular Visual–Inertial
Odometry Using Point and Line Features
[2] LSD線特征：LSD: A Fast Line Segment Detector with a False Detection Control
[3] LBD線特征描述子：An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency
4）Cartographer
[1] Real-Time Loop Closure in 2D LIDAR SLAM
[2] ICP：Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration
[3] 倍訓優化：Sparse pose adjustment for 2D mapping
5）多幾何特征
多特征：
[1] Unified Representation and Registration of Heterogeneous Sets of Geometric Primitives
[2] StructVIO: Visual-Inertial Odometry With Structural Regularity of Man-Made Environments
[3] Systematic Handling of Heterogeneous Geometric Primitives in Graph-SLAM Optimization
線特征：
[4] The 3D Line Motion Matrix and Alignment of Line Reconstructions?
[5] Building a 3-D Line-Based Map Using Stereo SLAM
面特征：
[6] GPO: Global Plane Optimization for Fast and Accurate Monocular SLAM Initialization
物體：
[7] QuadricSLAM: Dual Quadrics From Object Detections as Landmarks in Object-Oriented SLAM
[8] CubeSLAM: Monocular 3-D Object SLAM
6）相關書籍
[1] 視覺SLAM十四講
[2] 概率機器人
[3] 計算機視覺：多視圖幾何
[4] 機器人學中的狀態估計