【VSLAM系列】一：视觉SLAM学习笔记

中英文对照表

中文	英文
计算机视觉	Computer Vision
人工智能	Artificial Intelligence
单目相机	Monocular
双目相机	Stereo
深度相机	RGB-D
视差	Disparity
稀疏	Sparse
稠密	Dense
半稠密	Semi-dense
同时定位与建图（SLAM）	Simultaneous Localization and Mapping
TOF	Time-of-Flight
视觉里程计(VO)	Visual Odometry
前端	Front End
后端优化	Optimization
后端	Back End
回环检测	Loop Closing
建图	Mapping
漂移	Drift
最大后验概率估计MAP	Maximum-a-Posteriori
滤波器	Filter
关键帧	Key frames
度量地图	Metric Map
拓扑地图	Topological Map
路标	Landmark
线性高斯系统(LG)	Linear Gaussian
非线性非高斯系统(NLNG)	Non-Linear Non-Gaussian
卡尔曼滤波器(KF)	Kalman Filter
扩展卡尔曼滤波(EKF)	Extended Kalman Filter
粒子滤波器	Particle Filter
图优化	Graph Optimization
多视图几何	Multiple View Geometry
偏航-俯仰-滚转	yaw-pitch-roll
四元数	Quaternion
矩阵	Matrix
群	Group
轨迹的均方根误差(RMSE)	Root-Mean-Square-Error
相机内参数矩阵K	Camera Intrinsics
相机外参矩阵T	Camera Extrinsics
畸变矫正	Undistort
基线	Baseline
红外结构光	Structuered Light
飞行时间法（ToF）	Time-of-flight
通道	channel
点云库PCL	Point Cloud Library
运动结构恢复（SfM）	Structure from Motion
参数块	Parameter Block
关键点	Key-point
描述子	Descriptor
尺度不变特征变换(SIFT)	Scale-Invariant Feature Transform
FAST基于加速分割测试的特征	Features from Accelerated Segment Test
BRIEF二进制鲁棒独立基本特征	Binary Robust Independent Elementary Feature
ORB	Oriented FAST and Rotation BRIEF
非极大值抑制	Non-maximal suppression
灰度质心法	Intensity Centroid
数据关联	data association
基础矩阵F	Fundamental Matrix
本质矩阵E	Essential Matrix
单应矩阵H	Homography Matrix
尺度不确定性	Scale Ambiguity
随机采样一致性（RANSAC）	Randon Sample Concensus
直接线性变换（DLT）	Direct Linear Transform
迭代最近点（ICP）	Iterative Closest Point
光流法	Optical Flow
直接法	Direct Method
最小化光度误差	Minimize Photometric error
视觉里程计(VO)	visual odometry
有限状态机（FSM）	Finite State Machine
位姿图	Pose Graph
因子图	Factor Graph
动态贝叶斯网络(DBN)	Dynamic Bays Network
有向无环图	Directed Acyclic Graph
全局一致	Global Consistent
感知偏差	Perceptual Alising
感知变异	Perceptual Variability
准确率和召回率	Precision & Recall
词袋模型(BoW）	Bag-of-Words Model
聚类	Clustering
评率-逆文档频率（TF-IDF）	Term Frequency-Inverse Docunment Frequency
随机蕨法	Random Ferns
立体视觉	Stereo Vision
差的绝对值之和(SAD)	Sum of Abssolute Difference
平方差之和（SSD）	Sum of Squared Distence
归一化互相关（NCC）	Normalized Cross Correlation
逆深度	Inverse depthhao
点云	Point Cloud
网格	Mesh
面片	Surfel
体素	Voxel
占用栅格地图	Occupancy Map
八叉树图	Octo-map
并行跟踪与建图(PTAM)	Parallel Tracking and Mapping
SLAM	Simultaneous Localization and Mapping
视觉惯性里程计（VIO）	Visual Inertial Odometry
松耦合	Loosely Coupled
紧耦合	Tightly Coupled

环境配置相关内容

参考博客： https://blog.csdn.net/qq_37769337/article/details/124681853

1）安装OpenCV https://blog.csdn.net/qq_29931565/article/details/120572013

2）ORB_SLAM2环境配置 https://blog.csdn.net/qq_29931565/article/details/120860674

3）安装Eigen

sudo apt-get install libeigen3-dev
sudo updatedb
locate eigen3

4）配置李代数Sophus库

git clone http://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make
sudo mae install

5）C++ fmt库安装

从官网下载，使用sudo make install 安装，注意在CMakeLists.txt中间位置加上

报错

/usr/bin/ld: /usr/local/lib/libfmt.a(format.cc.o): relocation R_X86_64_PC32 against symbol `stderr@@GLIBC_2.2.5' can not be used when making a shared object; recompile with -fPIC
/usr/bin/ld: final link failed: bad value
collect2: error: ld returned 1 exit status
make[2]: *** [src/CMakeFiles/myslam.dir/build.make:262: ../lib/libmyslam.so] Error 1
make[1]: *** [CMakeFiles/Makefile2:136: src/CMakeFiles/myslam.dir/all] Error 2
make: *** [Makefile:95: all] Error 2

查看报错信息，编译器提示我们重新把fmt这个库带着参数-fPIC编译一遍。具体操作是在fmt库的CMakeLists.txt里加上一句

add_compile_options(-fPIC)

add_compile_options(-fPIC)
或者：
set(CMAKE_POSITION_INDEPENDENT_CODE ON) # 添加-fPIC编译，即：装成一个动态库（Linux为.so扩展文件；windows为.dll扩展文件）

git clone https://github.com/fmtlib/fmt.git
mkdir build
cd build
cmake ..
make
sudo make install 

sudo make install 将文件安装在本机上
make -j4调用4个线程进行编译

6）问题 /usr/bin/ld: 找不到 -lEigen3::Eigen
是Pangolin版本太高导致的，ORB-SLAM 2只支持v0.5版本的Pangolin。卸载之前安装的Pangolin库，安装v0.5版本的即可

7）vscode格式化代码

在VSCode中下载内置的Clang-Format插件
在命令行输入sudo apt-get install clang-format
创建.clang-format文件到工作目录下，配置格式化
Ctrl+Shife+i

8）安装Ceres

https://github.com/ceres-solver/ceres-solver

sudo apt-get install liblapack-dev libsuitesparse-dev  libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

头文件在usr/local/include/ceres下面
库文件在usr/local/lib下面

删除ceres

sudo rm -r /usr/local/lib/cmake/Ceres
sudo rm -rf /usr/local/include/ceres /usr/local/lib/libceres.a
sudo rm -rf /usr/local/lib/cmake/Ceres   
sudo rm -r /usr/local/share/Ceres

9）g2o安装

sudo apt-get install cmake libeigen3-dev libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev-qt5

如果由报错，在CMakelists.txt里面添加以下内容，最新的G2O要用c++14编译

list( APPEND CMAKE_MODULE_PATH /home/jeet/g2o/cmake_modules) 

SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator
 g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core 
 g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba 
 g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense
  g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data
   g2o_types_sim3 cxsparse )
   
   target_link_libraries(pose_estimation  ${OpenCV_LIBS} ${CERES_LIBRARIES} ${G2O_LIBS} Sophus::Sophus)
    
    #找不到cs.h文件，添加以下路径
    include_directories("/usr/include/suitesparse/")
    
    #报错In function`fmt::v8::detail::error_handler::on_error
    find_package(FMT REQUIRED)
    target_link_libraries(pose_estimation   fmt::fmt)#或者Sophus::Sophus

10）cmake版本升级

https://cmake.org/files/下载所需版本的源码。

wget https://cmake.org/files/v3.22/cmake-3.22.1.tar.gz

卸载旧版本

sudo apt remove cmake

tar -xvzf cmake-3.22.1.tar.gz

下载完成后在当前目录解压：cd cmake-3.22.1/
安装依赖：

sudo apt-get install g++ gcc libssl-dev make

configure是一个shell脚本，它可以自动设定源程序以符合各种不同平台上Unix系统的特性，并且根据系统参数及环境产生合适的Makefile文件或是C的头文件(header file)，让源程序可以很方便地在这些不同的平台上被编译连接。

sudo ./configure

配置完成后，编译：

make
sudo make install

最后使用新安装的cmake替换旧版本，其中/usr/local/bin/cmake为新安装的cmake目录。

sudo update-alternatives --install /usr/bin/cmake cmake /usr/local/bin/cmake 1 --force

添加链接 ，查看版本

sudo ln -s /usr/local/bin/cmake /usr/bin 
cmake --version

11）安装GTSAM

https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip

https://bitbucket.org/gtborg

  mkdir build && cd build
  cmake ..
  sudo make install

12）cholmod.h: 没有那个文件或目录/usr/local/include/g2o/solvers/cholmod/linear_solver_cholmod.h:30:10: fatal error:

CMakeLists.txt中添加

include_directories( "/usr/include/suitesparse")

13）安装octomap

https://github.com/OctoMap/octomap

ROS melodic对应19.7版本

安装依赖

sudo apt-get install build-essential cmake doxygen libqt4-dev 
sudo apt-get install libqt4-opengl-dev libqglviewer-dev-qt4

mkdir build 
cd build 
cmake .. 
make

14）CSparse报错

报错内容

CMake Warning (dev) at /homepo/clion-2021.3.3/bin/cmakenux/cmake-3.21/Modules/FindPackageHandleStandardArgs.cmake:438 (message):
  The package name passed to `find_package_handle_standard_args` (CSPARSE)
  does not match the name of the calling package (CSparse).  This can lead to
  problems in calling code that expects `find_package` result variables
  (e.g., `_FOUND`) to follow a certain pattern.

只需要，把cmake_modules文件夹下FindCSparse.cmake文件中的最后一行：

find_package_handle_standard_args(CSPARSE DEFAULT_MSG
  CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

修改为：

find_package_handle_standard_args(CSparse DEFAULT_MSG
  CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

即可

14 DBow3报错

make[2]: *** 没有规则可制作目标“/usr/local/lib/libDBoW3.a”，由“gen_vocab” 需求。 停止。

将静态库改为动态库

# dbow3 
# dbow3 is a simple lib so I assume you installed it in default directory 
set( DBoW3_INCLUDE_DIRS "/usr/local/include" )
set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" )#.a改为.so

15 安装pcl

git clone https://github.com/PointCloudLibrary/pcl.git 
cd pcl 
mkdir release 
cd release
cmake -DCMAKE_BUILD_TYPE=None -DCMAKE_INSTALL_PREFIX=/usr \ -DBUILD_GPU=ON-DBUILD_apps=ON -DBUILD_examples=ON \ -DCMAKE_INSTALL_PREFIX=/usr .. 
make  
sudo make install

相关依赖：

sudo apt-get install clang-format-10

sudo apt-get install libqhull* libgtest-dev
sudo apt-get install freeglut3-dev pkg-config
sudo apt-get install libxmu-dev libxi-dev
sudo apt-get install mono-complete
sudo apt-get install openjdk-8-jdk openjdk-8-jre

问题报错

CMake Error at /usr/lib/cmake/vtk-6.3/vtkModuleAPI.cmake:120 (message):
  Requested modules not available:
 
    vtkRenderingOpenGL2
    。。。

可能因为vtk6.3版本为 vtkRenderingOpenGL，而之前下载的vtk7.1所使用的为 vtkRenderingOpenGL2。可以将其改过来，也可以安装7.1以上版本。打开文件/usr/share/pcl-1.12/PCLConfig.cmake

首先修改权限，将其变为可读写

sudo chmod 666 PCLConfig.cmake

然后通过ctrl+F定位vtkRenderingOpenGL所在位置，并将vtkRenderingOpenGL2改为vtkRenderingOpenGL即可。最后重新进行catkin_make编译

在我们对项目进行编译时，可能会遇到make: *** 没有规则可以创建“/usr/lib/x86_64-linux-gnu/XXXXXX.so”需要的目标，这个问题，导致这个问题的原因大多来源于我们对某个项目的某个依赖库（卸载后）重新进行了源码编译，一般导致两种情况：在原本的共享链接库路径下找不到原本的链接库文件.so, 该文件断开链接

16 卸载安装VTK

卸载：

cd /usr/local/include
sudo rm -f vtk*
cd /usr/local/lib
sudo rm -r libvtk*

安装依赖包：

sudo apt-get install mesa-common-dev
sudo apt-get install libgl1-mesa-dev
sudo apt-get install cmake-curses-gui 

下载VTK文件http://www.vtk.org/download/

cd VTK文件的下载路径
unzip VTK文件
cd解压后的VTK文件夹
mkdir build
cd build
ccmake ../

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SomSHgHY-1667439200478)(/home/jeet/snap/typora/76/.config/Typora/typora-user-images/image-20221024143454094.png)]

c//进行设置，下面这些设置参考了官网的设置以及python的设置
VTK_USE_QTON//OFF的状态下enter就可以调整成ON了，页面下方有提示
BUILD_SHARE_LIBSON
VTK_GROUPS_ViewsON
VTK_WRAP_PYTHONON//用python的时候，这个选项要选成ON
VTK_LEGACY_SILENTON
c//确认设置
c//确认设置
g//生成
$make
$sudo make install

17 CMake Error

CMake Error at /usr/local/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:230 (message):
Could NOT find GTest (missing: GTEST_LIBRARY GTEST_MAIN_LIBRARY)

sudo apt-get install checkx
sudo apt-get install libbz2-dev
sudo apt-get install -y libpcre2-dev
sudo apt-get install -y libmilter-dev

大概是因为GTest虽然安装过，但是好像找不到头文件之类，解决方法如下：

sudo apt-get install libgtest-dev
sudo apt-get install cmake
cd
cd /usr/src/gtest
sudo cmake CMakeLists.txt 
sudo make 
sudo cp *.a /usr/lib

第二讲

make2.1 SLAM简介

环境感知+自主定位

早期：SLAM问题主要概括为概率公式（Probabilistis Formulations）,主要算法包括EKF(Extended Kalman Filters)，最大似然估计，贝叶斯概率估计(Bayesian probability estimation)，马尔科夫链，但是对计算效率和数据关联的准确性要求较高（强依赖于里程计数据）。

SLAM问题的本质：对运动物体自身和周围环境空间不确定性的估计

回环检测（Loop Closure）：VIN(Visual-Inertial Navigation)去除回环检测的SLAM系统

关注环境：1.高动态环境 比如仓库识别，2.需要长期工作的环境，3.高频率回环检测及优化，4.需要对错误回环检测的识别和剔除。如果把工作环境限定在静态、刚体，光照变化不明显、没有人为干扰的场景SLAM系统是相当成熟的。

2.2 视觉相机

	单目视觉(Monocular)	双目视觉(stereo)	RGB-D视觉
特点		· 融合不同摄像头的图像并观察差别，建立特征间的对应关系，获得明显的深度感 · 多融合IMU或IR等传感器	· 基于红外结构光或ToF原理
优点	· 传感器简单，成本低廉	· 静止时可以获得深度，双目相机之间的距离基线Baseline，基线越大，所测量的距离也就越远	· 信息丰富· 深度计算简单
缺点	尺度不确定性	· 计算量大 ·依赖特征点；在特征少的白墙或暗光环境易丢失目标	· 视野窄，噪声大 ·受阳光、墙面反光,无法测量投射材质
应用场景及前景		应用程度最高，落地前景最高
常用相机			Kinect/Kinect V2

2.3 经典SLAM框架：

传感器->前端->后端

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qeKpTcuQ-1667439200479)(C:\Users\Jeet\AppData\Roaming\Typora\typora-user-images\image-20220514164953252.png)]

运动模型和观测模型

1. 传感器信息读取。在视觉SLAM中主要为相机图像信息的读取和预处理。

   从传感器数据中提取相关特征(Relevent featyres)，在某些观测和路标之间建立约束(data association)，关键帧之间的相似特征点约束

2. 视觉里程计（Visual Odometry，VO）。视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端（Front End）。

3. **后端优化（**Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端（Back End）。**主要指处理SLAM过程的噪声问题：**如何从带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性多大，称为最大后验概率估计MAP

4. 回环检测（Loop Closing）。回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。开关量约束降低回环检测的错误率。

5. 建图（Mapping）。

1）前端视觉里程计(Visual Odometry, VO)，

主要是估计视角之间的变换，或者运动参数，它不需要输出制图（mapping）的结果，仅有视觉输入的姿态估计；1.旋转矩阵、平移矢量 2.四元数 3.欧拉角 特征点法，直接法

相机运动+目标位置：第一是要根据摄像头回传的图像计算相机的帧间运动，第二就是估计目标点大致的空间位置坐标

研究内容：特征提取，特征匹配，角点特征

图像特征点由两部分构成：关键点（Keypoint）、描述子（Descriptor）。 关键点指的是该特征点在图像中的位置，有些还具有方向、尺度信息；描述子通常是一个向量，按照人为的设计的方式，描述关键点周围像素的信息。通常描述子是按照外观相似的特征应该有相似的描述子设计的。ORB和SIFT

相机运动估计:1）PnP通过匹配特征点在图像中的坐标，求解相机运动时的旋转矩阵和平移矩阵。2）光流法通过获取的像素点在窗口内的运动计算相机本身的运动

2）后端优化 (Optimization):

前端视觉里程计能给出一个短时间内的轨迹和地图，但会产生误差累积，考虑长时间内的最优轨迹和地图，这时后端优化就出现了。得到全局一致的轨迹和地图；状态预测

①滤波器模型

​ a. 马尔可夫性、b. 线性高斯系统、c. 卡尔曼滤波

② 非线性系统和扩展卡尔曼滤波器（EKF）

​ 使用相机内参模型及李代数表示位

③BA与图优化

Bundle Adjustment(BA)，是指从视觉重建中提取出最优的3D模型和相机参数

非线性优化后端方案，滤波器后端方案

1. 基于滤波器：KF，EKF将系统线性化，用高斯分布近似观测噪声，并利用卡尔曼滤波进行状态更新
2. 基于非线性优化：图优化，因子图：给定初值之后不断迭代更新图，主要利用的是g2o库

关键帧 (key frames)机制，即不用精细处理每一幅图像，而是把几个关键图像串起来优化其轨迹和地图。

3）闭环检测 (Loop Closing):

指机器人在地图构建过程中, 通过视觉等传感器信息检测是否发生了轨迹闭环, 即判断自身是否进入历史同一地点;

词袋向量：由于视觉词汇都是预训练的，在不同场景下的泛化能力是一个很重要的问题。除了增加词汇数量的方法外，还可以根据观测模型信息进行在线更新词汇，这种方法不需要对视觉词汇进行预训练，从而获得了对环境的自适应能力

场景描述：方法主要包括：局部特征描述子、全局描述子、局部区域的全局描述子、结合深度信息的场景描述、场景的时变描述。闭环检测与深度学习：当前深度学习与闭环检测的结合点主要在于利用深度神经网络生成更好的场景描述，

决策模型：根据当前场景描述和地图信息识别出可能的闭环。基于拓扑信息、概率模型、序列匹配、利用度量信息、局部特征几何信息

4）建图 (Mapping):

**度量地图（**metric map):强调精确的表示地图中物体的位置关系，通常用稀疏（sparse)和稠密（dense)，稀疏地图只能用于定位不能用于导航
拓扑地图(Topological Map)

间接法首先对测量数据进行预处理来产生中间层，通过稀疏的特征点提取和匹配来实现的，也可以采用稠密规则的光流，或者提取直线或曲线特征来实现。然后计算出地图点坐标或光流向量等几何量，因此间接法优化的是几何误差：

●CNN-SLAM：在LSD-SLAM上将深度估计以及图像匹配改为基于卷积神经网络的方法，并且可以融合语义信息，得到了较鲁棒的效果；

● 剑桥大学开发的PoseNet，是在GoogleNet的基础上将6自由度位姿作为回归问题进行的网络y改进，可以利用单张图片得到对应的相机位姿；

● 《视觉SLAM十四讲》，利用深度神经网络而不是常见的视觉特征来学习原始数据的特征，实现了基于深度网络的回环检测[20]；

● LIFT利用深度神经网络学习图像中的特征点，相比于SIFT匹配度更高

2.4 SLAM的数学描述

运动方程[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IKCp9O9U-1667439200480)(C:\Users\Jeet\AppData\Roaming\Typora\typora-user-images\image-20220514173319595.png)]
xk-1上一次的位置，uk运动传感器数据，wk噪声

观测方程[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ckgcyRQu-1667439200480)(C:\Users\Jeet\AppData\Roaming\Typora\typora-user-images\image-20220514175238762.png)]
yj路标点位置，vk,j观测噪声

SLAM问题[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PGf25jmt-1667439200481)(/home/jeet/snap/typora/57/.config/Typora/typora-user-images/image-20220524150014084.png)]
通过运动测量u和传感器读数z，求解定位问题x和建图估计y，状态估计问题

第三讲

3.1 变换矩阵

T

旋转向量，方向与旋转轴一致，长度等于旋转角，转轴是矩阵R特征值1对应的特征向量

3.2 Eigen库使用

#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#include <Eigen/Geometry>
    Eigen::Matrix<float, 2, 3> matrix_23;
    //初始化赋值
    matrix23<<1,2,3,4,5,6;
  // 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }
    Eigen::Vector3d v_3d;
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 迹
    cout << 10*matrix_33 << endl;               // 数乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.  ideterminant() << endl;    // 行列式
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
 // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
     Eigen::Matrix3d R=rotation_vector.toRotationMatrix();

    // 四元数可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );

  // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
  // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
  // 应该使用 .cast<>()函数显式转换
  Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  q1.normalize();//四元数归一化

//四元数转换为旋转向量
Eigen::AngleAxisd rotation_vector4(quaternion4);
cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI) << " axis is: " << rotation_vector4.axis().transpose() << endl;
//rotation_vector4.angle()表示旋转角度，rotation_vector4.axis().transpose()表示旋转轴

Isometry3d T1w(q1), T2w(q2);//给变换矩阵旋转部分赋值
  T1w.pretranslate(t1);//给变换矩阵平移部分赋值

读取文件中的数据

// path to trajectory file
string trajectory_file = "../trajectory.txt";
ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
         double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }

Eigen::Map

在不进行拷贝的情况下可以使用eigen的map功能进行内存映射。Map类用于通过C++中普通的连续指针或者数组 （raw C/C++ arrays）来构造Eigen里的Matrix类

int array[9];
Eigen::Map<MatrixXi> mat(array,3,3);

1. 比如有个API只接受普通的C++数组，但又要对普通数组进行线性代数操作，那么用它构造为Map类，直接操作Map就等于操作了原始普通数组，省时省力。
2. 再比如有个庞大的Matrix类，在一个大循环中要不断读取Matrix中的一段连续数据，如果你每次都用block operation 去引用数据，太累（虽然block operation 也是引用类型）。于是就事先将这些数据构造成若干Map，那么以后循环中就直接操作Map就行了。
   实际上Map类并没有自己申请一片空内存，只是一个引用，所以需要构造时初始化，或者使用Map的指针。

第四讲

3.1 李群李代数Sophus：

1）SO(3)的使用方法

//可以构造旋转矩阵或四元数：
Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,0)).toRotationMatrix();
Quaterniod q(R);
//接着根据旋转矩阵或四元数构造SO(3)
Sophus::SO3d SO3_R(R);
Sophus::SO3d SO3_q(q);

李代数用法
使用对数映射获得它的李代数

Vector3d so3=SO3_R.log()

向量和反对称矩阵互换

Sophus::SO3d::hat(so3)
Sophus::SO3d::vee(Sophus::SO3d::hat(so3))

增量扰动模型的更新

Vector3d update_so3(1e-4,0,0);
Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R;
SO3_updated.matrix();

2）SE(3)的使用方法

//首先可以构造旋转矩阵或四元数，以及平移向量：
Matrix3d R=AngleAxisd(M_PI/2,Vector3d(1,0,0)).toRotationMatrix();
Quaterniod q(R);
Vector3d t(1,0,0);
//接着根据旋转矩阵或四元数构造SO(3)
Sophus::SE3d SE3_Rt(R,t);
Sophus::SE3d SE3_qt(q,t);

// 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造

李代数用法
使用对数映射获得它的李代数

typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3=SE_3Rt.log();

向量和反对称矩阵互换

Sophus::SE3d::hat(se3)
Sophus::SE3d::vee(Sophus::SE3d::hat(se3))

增量扰动模型的更新

Vector6d update_se3;
update_se3.setZero();
update_se3(0,0)=1e-4d;

Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_Rt;
SE3_updated.matrix();

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)   
include_directories(${Sophus_INCLUDE_DIRS})

关于sophus模板类需要注意的情况（error: missing template arguments before ‘SO3_R’）https://blog.csdn.net/weixin_38275649/article/details/80869340

打开文件读取数据

  string trajectory_file = "../trajectory.txt";
  ifstream fin;
    fin.open(trajectory_file);
    if (!fin) {
        cerr << "open attempt failed!" << endl;
    }
    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
        poses.push_back(p1);
    }

3）轨迹均方根误差RMSE

设真实轨迹(ground-truth)为 T g ,估计轨迹 T e 。它们都以 T W C 的形式存储,计算估计轨迹的误差。假设每一个 T g 都与给定的 T e 对应。那么,对于任意第 i 个位姿,它的误差可定义为:

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZovXNze4-1667439200482)(/home/jeet/snap/typora/57/.config/Typora/typora-user-images/image-20220525172622018.png)]

即两个位姿之差的李代数二范数。于是,可以定义两条轨迹的均方根(Root-Mean-Square-Error, RMSE)
误差为:
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-hdcFgXbp-1667439200483)(/home/jeet/snap/typora/57/.config/Typora/typora-user-images/image-20220525172550523.png)]

double rmse = 0;
    for (size_t i = 0; i < estimated.size(); i++) {
        Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
        double error = (p2.inverse() * p1).log().norm(); // norm()函数用于获取复数的范数
        rmse += error * error;
    }
    rmse = rmse / double(estimated.size());
    rmse = sqrt(rmse);

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d> Trajectory

初始化vector容器时，要加上内存对齐参数。
上述的这段代码才是标准的定义容器方法，一般情况下定义容器的元素都是C++中的类型，所以可以省略，因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。

第五讲

5.1 OpenCV相关知识

cv::Mat是OpenCV定义的用于表示任意维度的稠密数组，OpenCV使用它来存储和传递图像

元素类型定义
由于cv::Mat要用于存储图像，它里面的元素可以是“像素”，对于像素，OpenCV定义了专门的数据格式来描述它们。“像素”的数据格式----命名规则：
基本数据类型 + 通道数
CV_{8U, 16S, 16U, 32S, 32F, 64F}C{1, 2, 3}
例如：CV_8UC3: 三通道、每个通道是unsigned char类型的数据

{8U, 16S, 16U, 32S, 32F, 64F}：
8U:unsigned char ； 16S: short int ； 16U: unsigned short int ； 32F: float ； 64F: double

{1，2，3}代表通道数，彩色图像每个像素需要存储B、G、R（蓝、绿、红）3个信息，需要3个位置， 每一个称为一个“颜色通道”，灰度图像无颜色，只有一个通道，故常为CV_8UC1

1）cv::Mat类

//默认构造函数与值构造函数
cv::Mat img; //默认构造函数

//读取图像
image = cv::imread(argv[1]);
//值构造函数1
cv::Mat img1(480, 640, CV_8UC3); //图像高（行）480-row，宽（列）640-col， 数据类型：CV_8UC3

//值构造函数2
//cv::Scalar(B, G, R)可以表示三通道颜色，这里所示为纯蓝色
cv::Mat img2(480, 640, CV_8UC3, cv::Scalar(255, 0, 0));

//值构造函数3
//效果与上面一样
cv::Size sz3(480, 640);
cv::Mat img3(sz3, CV_8UC3, cv::Scalar(255, 0, 0));

//拷贝构造函数1---都是以静态引用传递参数（const &）
cv::Mat img4(img3);

//拷贝构造函数2---只拷贝感兴趣的区域----由Rect对象定义
//rect左上角（100，100），宽高均为200，（x,y,width,height)
cv::Rect rect(100, 100, 200, 200);
cv::Mat img5(img3, rect);

//拷贝构造函数3--从指定行列构造
//从img3中拷贝0-239行以及0-319行到img6
cv::Range rows(0, 240);
cv::Range cols(0, 320);
cv::Mat img6(img3, rows, cols);

//静态构造函数
cv::Mat img7 = cv::Mat::zeros(480, 640, CV_8UC3); //480行640列，值全为零的数组。
cv::Mat img8 = cv::Mat::ones(480, 640, CV_64FC1); //全1矩阵
cv::Mat img9 = cv::Mat::eye(480, 640, CV_16SC2); //单位矩阵

2）访问元素（访问像素）

//直接访问---模板函数at<>()
//单通道, 尖括号里面的类型照着文章开头介绍的类型对应关系输入
cv::Mat img = cv::Mat::ones(240, 320, CV_32FC1);
float elem = img.at<float>(10, 10);

//多通道---Vec3b代表固定向量类
//注意类型之间的对应，固定向量与Mat的代表字母有一点差异
//UC3 -> 3b
cv::Mat img1(480, 640, CV_8UC3, cv::Scalar(255,255,0));
cv::Vec3b elem = img1.at<cv::Vec3b>(10, 10);
elem_B = elem[0]; //蓝色通道数值---255
elem_G = elem[1]; //绿色通道数值---255
elem_R = elem[2]; //红色通道---0

3）区域访问

//320×320， 3通道白色图像
cv::Mat img(320, 320, CV_8UC3, cv::Scalar(255,255,255));

//访问第1行元素
cv::Mat img_r_0 = img.row(0);

//访问第2列元素
cv::Mat img_c_2 = img.col(1);

//0-160行元素组成的数组
cv::Mat img_r_range = img.rowRange(0, 160);

//0-160列元素组成的数组
cv::Mat img_c_range = img.colRange(0, 160);

4）其他方法

//克隆矩阵
cv::Mat img(480, 640, CV_64FC3);
cv::Mat img1 = img.clone();

//设置元素值
img1.setTo(cv::Scalar(1.0, 2.0, 3.0));

//返回通道数目
size_t num_c = img1.channels();

//返回数组大小
cv::Size sz = img1.size();

//检验数组是否为空,为空返回true
bool e = img1.empty();

求程序花费的时间

   #include <chrono>
   chrono::steady_clock::time_point t2=chrono::steady_clock::now();
   //.....
   chrono::steady_clock::time_point t2=chrono::steady_clock::now();
    chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "Traversal took："<<time_used.count()<<" S"<<endl;

#include <ctime>
  clock_t time_stt = clock(); // 计时
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;

boost::format主要是用来格式化std::string字符串以及配合std::cout代替C语言printf()

可以把参数格式化到一个字符串

boost::format fmt("../%s/%d./%s");
colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
// 表示../color/1.png这个字符串，对应的就是文件名称

5）彩色图读取BGR值

//方法一：
//程序上方读取了一张图片color
color.data[ v*color.step + u*color.channels() ];     //B
color.data[ v*color.step + u*color.channels() +1 ];  //G
color.data[ v*color.step + u*color.channels() +2 ];  //R

//方法二：
//程序上方读取了一张图片color
color.at<cv::Vec3b>(v, u)[0];  //B
color.at<cv::Vec3b>(v, u)[1];  //G
color.at<cv::Vec3b>(v, u)[2];  //R

成员函数step返回该Mat对象一行所占的数据字节数

6）去畸变

OpenCV畸变系数矩阵的默认顺序 [k1, k2, p1, p2, k3]

  //! OpenCV自带的undistort函数，更快速
  cv::Mat K = cv::Mat::eye(3, 3, CV_32FC1); //内参矩阵
  K.at<float>(0, 0) = fx;
  K.at<float>(1, 1) = fy;
  K.at<float>(0, 2) = cx;
  K.at<float>(1, 2) = cy;
  cv::Mat distort_coeffs = cv::Mat::zeros(1, 5, CV_32FC1); //畸变系数矩阵 顺序是[k1, k2, p1, p2, k3]
  distort_coeffs.at<float>(0, 0) = k1;
  distort_coeffs.at<float>(0, 1) = k2;
  distort_coeffs.at<float>(0, 2) = p1;
  distort_coeffs.at<float>(0, 3) = p2;
  cout << "K = " << endl
       << K << endl;
  cout << "distort_coeffs = " << endl
       << distort_coeffs << endl;

  t1 = chrono::steady_clock::now();
  cv::undistort(image, image_undistort2, K, distort_coeffs); //去畸变
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "time = " << time_used.count() << endl;

  // 展示去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::imshow("image_undistort2", image_undistort2);
  cv::waitKey(0);

7) imshow

namedWindow("窗口名",0);//创建窗口,WINDOW_NORMAL可调整窗口大小
imshow("窗口名",要显示的图片);//在创建的窗口中显示图片

cv::circle画圆函数

void cvCircle (CvArr * img,CvPoint center,int radius,CvScalar color,int thickness = 1,int line_type = 8,int shift = 0)

参数列表：

img ：为一个图像类型的指针,指向单通道或多通道的图像。
center ：圆的圆心坐标。类型是CvPoint。它表示一个坐标为整数的二维点，是一个包含int类型成员x和y的简单结构体。
radius ：圆的半径，数值为整数类型。
color ：圆线条的颜色。
CvScalar数据结构，cvScalar(int,int,int )
thickness ：为设置圆线条的粗细，值越大则线条越粗。为负数则是填充效果，thickness参数也可以设置为CV_FILL，其值是-1，其结果是使用与边一样的颜色填充圆内部。
line_type ：线条的类型。可以取8，4，CV_AA三个值,分别代表8邻接连接线，4邻接连接线和反锯齿连接线。默认值为8邻接。为了获得更好地效果可以选用CV_AA(采用了高斯滤波)。

第六讲

6.1 Ceres使用

ceres用途：用来求解特定形式的优化问题

大致使用方法：
1、构建代价函数模型，描述如何计算残差
2、构建问题（要给一个初值）
3、配置求解器
4、结果输出

6.2 g2o使用

https://blog.csdn.net/hltt3838/article/details/115021610?ops_request_misc=&request_id=&biz_id=102&utm_term=_vertices%5B0%5D&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduweb~default-0-115021610.142^v11pc_search_result_control_group,157^v13new_3&spm=1018.2226.3001.4449

使用g2o，要在list后面天添加FindG2O.cmake存在的文件夹路径。

list( APPEND CMAKE_MODULE_PATH /home/jeet/g2o/cmake_modules) 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

g2o优化步骤

1）相关头文件

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/robust_kernel.h>

2）CMakeLists.txt文件配置

# g2o 
find_package( G2O REQUIRED )
include_directories( ${G2O_INCLUDE_DIRS} )

SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator
 g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core 
 g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba 
 g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense
  g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data
   g2o_types_sim3 cxsparse )

target_link_libraries( 可执行文件名
    ${G2O_LIBS}）

1）定义顶点

class VertexType: public g2o::BaseVertex<6,Sophus::SE3d>//括号中为顶点维度和类型
{
//_estimate为基类中定义的估计量，类型由派生类给定
 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    bool read ( istream& is );//读物数据
    bool write ( ostream& os ) const;//保存优化后的顶点结果
    virtual void setToOriginImpl()；//重置顶点或者设置顶点初值
    virtual void oplusImpl ( const double* update )//顶点的更新方式
    {
        _estimate = f(update , _estimate);//update怎么更新到_estimate上
    }
}

2）定义边

class EdgeType: public g2o::BaseBinaryEdge<6, SE3d, Vertex1, Vertex2>
//<>中依次为测量值的维度，测量值数据类型，边链接的顶点1的类，2的类
{
    //对应的成员变量
   /* _measurement：存储观测值
    _error：存储computeError() 函数计算的误差
    _vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用         setVertex(int, vertex) 是设定的int 有关（0 或1）*/

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    //===================重载读写函数===========
    bool read ( istream& is ){
    setMeasurement ();//读取测量值，并设置 
    }
     bool write ( ostream& os ) const
    {//_vertices[0]表示其中一个顶点
    VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
    VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
    }
    //=================误差计算===============
     virtual void computeError()
     {
     Sophus::SE3d v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();//获取顶点1的估计值
     Sophus::SE3d v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();//获取顶点2的估计值
     //误差计算方法，_measurement，顶点V1，顶点V2三个参数计算误差
      _error = (_measurement.inverse()*v1.inverse()*v2).log();
     }
    
     //============ 雅可比矩阵计算==============
    virtual void linearizeOplus()
    {
        //先获取顶点
        Sophus::SE3d v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3d v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _jacobianOplusXi = v1  XXXXX;//两个顶点分别写雅克比计算式
        _jacobianOplusXj = v2  XXXXX;
        //一元边计算一个雅克比矩阵，二元边有两个，即误差关于该顶点的导数
    }

}

3）主函数中定义求解器并求解

//====================定义求解器===================
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; // 每个误差项优化变量维度为6，误差值维度为6
typedef g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

    // 梯度下降方法，可以从GN, LM, DogLeg 中选,要包含相关头文件
/*#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>*/
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer; // 图模型
    optimizer.setAlgorithm(solver); // 设置求解器
    optimizer.setVerbose(true);     // 打开调试输出

//================向图中添加顶点=========
 while ( !fin.eof() )//这里演示的是从文件中读取顶点数据并添加
    {
        string name;
        fin>>name;
      VertexSE3LieAlgebra* v = new VertexSE3LieAlgebra();//生成一个顶点
            int index = 0;
            fin>>index;
            v->setId( index );//设置顶点ID
            v->read(fin);
            optimizer.addVertex(v);//添加顶点
            vertexCnt++;
            vectices.push_back(v);
            if ( index==0 )
                v->setFixed(true);//第一个点设置为固定点
        }
//===================添加边==================
    while ( !fin.eof() )
    {
        string name;
        fin>>name;
        // SE3d-SE3d 边
            EdgeSE3LieAlgebra* e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 关联的两个顶点的ID
            fin>>idx1>>idx2;
            e->setId( edgeCnt++ );//设置的ID
            e->setVertex( 0, optimizer.vertices()[idx1] );//设置边连接的一个点
            e->setVertex( 1, optimizer.vertices()[idx2] );//设置边连接的第二个点
            e->read(fin);//边的数据，一般就是传感器测量值
           // edge->setMeasurement(y_data[i]);      // 设置观测数值
           
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆，不设置默认为单位阵

            optimizer.addEdge(e);//添加边
            edges.push_back(e);
        }
        if ( !fin.good() ) break;
    }

//====================开始优化==========================
  optimizer.initializeOptimization();
  optimizer.optimize(10);

//======================可以保存结果=================
    // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
    // 伪装成 SE3d 顶点和边，让 g2o_viewer 可以认出
    ofstream fout("result_lie.g2o");
    for ( VertexSE3LieAlgebra* v:vectices )
    {
        fout<<"VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for ( EdgeSE3LieAlgebra* e:edges )
    {
        fout<<"EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();

第七讲

7.1特征点法

特征点	思路	优点	缺点
SIFT（Scale-Invariant Feature Transform）	尺度不变特征变换	充分考虑了光照、旋转、尺度的变化	计算量大
FAST关键点Features from Accelerated Segment Test	如果一个像素与它邻域的像素差别较大(过亮或过暗),那它更可能是角点	计算快	只比较像素大小，没有描述子，特征点数量大切不确定，存在尺度问题
BRIEF描述子Binary Robust Independent Elementary Feature	比较相邻像素点直接的值的大小，向量元素为0或1	占用内存小、具备很好的识别性能、运行时间很短	不具备旋转不变性。（当两幅需要匹配的图像旋转角度过大（大概30以上），识别效果就很差、不具备尺度不变性
ORB（Oriented FAST and Rotation BRIEF）		ORB特征在速度方面相较于SIFT、SURF已经有明显的提升的同时，保持了特征子具有旋转与尺度不变性

ORB特征

1)FAST角点提取 2）BRIEF描述子

Hariss角点检测算法基本思想是
使用一个固定窗口在图像上进行任意方向上的滑动，比较滑动前与滑动后两种情况，窗口中的像素灰度变化程度，如果存在任意方向上的滑动，都有着较大灰度变化，那么我们可以认为该窗口中存在角点

尺度不变性：构建图像金字塔，并在图像金字塔上每一层检测角点，
旋转不变性：由灰度质心法实现(以图像灰度值作为权重中心)

特征匹配

单应矩阵：描述处于同一平面上的一些点在两张图像之间的变换关系

1）从两帧相邻图像进行特征查找与匹配

2）由两张图像的匹配点，利用对极几何计算基础矩阵或单应矩阵，从而得到相机旋转矩阵R和t

3）利用R,t和两帧关键点对应的像素坐标，利用三角测量计算深度，从而得到点的世界坐标

4）跟踪失败后，相机回到曾经的位置找到匹配帧，匹配帧具有3D地图点和位姿，利用PnP计算当前帧相较于匹配帧的相机运动从而得到相机位姿。

OpenCV求解PnP

cv::solvePnPRansac(pts3d,pts2d,K,Mat(),rvec,tvec,false,100,4.0,0.9,inliers);

solvePnPRansac函数参数：
[in] pts3d 参考点在世界坐标系下的点集；float or double
[in] pts2d 参考点在相机像平面的坐标下的点集；float or double
[in] K 相机内参矩阵3X3
[in] D 相机畸变矩阵5X1
[out]rvec 旋转矩阵 cv::Mat
[out]tvec 平移向量 cv::Mat
[in] bool值 若使用迭代算法（true），使用解析求解的结果（false）
[in] int值 算法的迭代次数
[in] float值 筛选内点和外点的距离阈值，
[in] float值 此值与迭代次数有关，代表从n个样本中取s个点，N次采样可以使s个点全为内点的概率。
[out]inliers 返回内点的序列，矩阵形式
[in] flags 最小子集的计算模型

第八讲

8.1直接法

一种使用点和线的高效的直接视觉里程计（visual odometry，VO）算法—— EDPLVO

直接法的目标函数优化包含了像素的信息，因此图像的性质直接影响了优化的进行。如果图像间同一像素忽明忽暗，则灰度梯度不再是凸函数，会陷入局部最优解。可以构建图像金字塔，当图像分辨率很小时，图像较为模糊，灰度变化不大，非凸性变弱。因此先在粗糙的图像下计算位姿，不断提高分辨率，找到更精确的位姿。同时直接法假设了灰度不变性，容易受到光照的影响。

优势：
没有特征提取和匹配的时间，不必显式估计每个点的运动，计算规模较小
选择的点只需要有像素梯度，不必是角点
可稠密或半稠密。可以选很多点根据p 的来源，我们可以把直接法进行分类：

1. p 来自稀疏关键点，我们称之为稀疏直接法。通常，使用数百个甚至上千个关键点，并且像LK光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度更快，但只能计算稀疏的重构。
2. p 来自部分像素。如果像素梯度为0，那么整项雅各比矩阵就为0，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方，这称为半稠密直接法，可以重构一个半稠密结构。
3. p 为所有像素，称为稠密直接法。稠密重构需要计算所有像素，因此多数不能在现有的CPU上实时计算，需要GPU的加速。但是，如前面讨论的，像素梯度不明显的点，在运动估计中不会有太大的贡献，在重构时也会难以估计位置。

8.2 视觉+IMU融合SLAM

IMU 适合计算短时间、快速的运动；视觉适合计算长时间、慢速的运动。同时，可利用视觉定位信息来估计 IMU 的零偏，减少 IMU 由零偏导致的发散和累积误差；反之， IMU 可以为视觉提供快速运动时的定位。

松耦合

紧耦合

主要解决问题
相机与IMU的外参匹配，包括旋转和平移
相机和IMU的帧率不同如何实现两者数据上的融合
最小化特征点的重投影误差
如何构建IMU的测量误差函数
求解融合信息后构建的误差函数

8.3 视觉+激光融合SLAM

数据层面：图像数据和点云数据融合

任务层面：前端和回环检测，图像和点云优势互补

发展趋势：1、鲁棒性和可移植性；2、多传感器融合，数据融合

3、语义SLAM；4、软硬件结合，集成传感器

ORB-SLAM2

1）sim3是使用3对匹配点来进行相似变换（similarity transformation）的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度

2）特征点提取中解决某些区域提取特征点过密的问题：
使用四叉树对特征点进行裁剪，四叉树的叶子节点数刚大于需要提取的特征点数，在每个叶子节点上选择一个特征点

弱纹理区域图片特征点特区的少：
先进行图像区域划分，在特征点少的区域降低阈值在进行提取

3）减少错误匹配的方法：
根据旋转主方向，最优匹配与次优匹配距离减小错误匹配
用BoWs语法数缩小匹配范围
在知道深度和相机位姿时通过求3D点的坐标以及在另一个相机位姿下点的投影来缩小范围
在只知道相机位姿的情况下通过极线匹配缩小特征点匹配的范围

2D-2D模型约束与外面剔除
Ransac:
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2Mo4yggp-1667439200484)(/home/jeet/snap/typora/57/.config/Typora/typora-user-images/image-20220707154751128.png)]

在求F模型的误差时用诡异化处理消除点的数量的影响，点的得分为阈值减去误差e