视觉SLAM的二维和三维点的特征匹配，位姿估计的方法总结
对于视觉，单目经常拿到的数据点是二维，当然也有像双目、kinetic这样的深度相机拿到3d坐标，对于激光一般都是拿到的三维点云坐标。那么对于一些已经提取到的特征点，如何进行匹配，从而估计出传感器的运动，参考视觉十四讲第七讲，自己总结一下，便于以后查看：
首先介绍以下相机的运动过程：世界系、相机系、归一化相机系、像素系
世界坐标系固定下，有一点Pw(x,y,z)，相机在初始时刻在世界坐标系下的位姿已知，可以得到P点在cam0时刻的位姿Pc(x,y,z)，再将相机坐标系转换到归一化相机平面(Z=1)，得到归一化坐标Pc(x/z,y/z,1)齐次坐标，可以得到像素坐标：K*Pc(x/z,y/z,1)。
1、2d-2d：对极几何 ，要转化为3d-2d的问题
图像特征点对的距离筛选标准一般是，汉明距离 < 最小距离的两倍
1）对极约束：
是等式为0的约束（p1、p2是像素坐标），因此E矩阵在不同尺度下是等价的。三个矩阵都会求解出4组解，利用相机深度一定为正（三角化检测），得到第一帧和第二帧间运动的正确解T21（H矩阵还需要其他方式去排除）。
2）基础矩阵E(3x3)：t的反对称矩阵乘以R（外积），E = t^ * R（8点法求解E,SVD去分解出运动）
CVlib：findFundamentalMat()
3）本质矩阵F(3x3)：与E只差相机内参矩阵（光心、焦距eg.），
CVlib：findEssentialMat()、recoverPose()
4）单应矩阵H(3x3)：适用于相机平面与特征平面平行（无人机俯视地面），4组匹配点求解，不能有3个共线的点。
CVlib：findHomoGraphyMat()
5）尺度归一化问题：旋转矩阵R自身是正交矩阵，可以认为就是归一化的，所有认为E的尺度是由于t具有一个尺度带来的，这将导致，单目视觉的尺度不确定性；因此，单目视觉在初始化时，必须带有平移，不能只有旋转；
6）三角化检测：从估计的R，t，建立最小二乘方程，求匹配的特征点的深度；然后以初始化计算的点深度对应的归一化的t，完成尺度确定。点深度为正，完成R，t确定。（三角检测相机运动也必须带有平移）
CVlib：triangulatePoints()
总结：特征点共面、相机发生旋转时，E矩阵出现退化，一般是同时估计F和H，选择重投影误差小的解。2)---4)完成运动估计，6)完成深度估计，并解决尺度问题。对于有多于8对点的求解方式：构建最小二乘超定方程组或者ransac。单目初始化完成后，就可以使用3d-2d来估计运动。构建线性方程组时，一般将待求解的nxn的矩阵，写为
x1的未知向量，进行构建。
2、3d-2d：PnP（至少3对点），一般转换为3d-3d方式
T21求解方式有：
1）DLT：直接线性变换，解线性方程组至少6 对点，多于6对点构建超定方程组，最后对求出的矩阵可以使用QR分解出满足SO(3)的R矩阵和t；
2）P3P：3对点，将2d点求得它的3d坐标，转换到统一坐标系下进行3d-3d的匹配，计算相机运动，因此P3P只是利用三角形相似得到了2d点的3d坐标；
CVlib：solvePnP()、Rodrigues()
3）非线性：BA，最小化重投影误差（将像素坐标与3d点按照当前估计的位姿进行投影构建的误差），高斯牛顿、列-马、Dog-leg等非线性优化方法（SLAM常用的优化库g2o、ceres、gtsam）；
--李群：SO(3)3维、SE(3)6维，对乘法封闭；记三维向量为
,
，也叫李代数，关系如下：
R在t时刻的一阶导数 = ^ * R，反应了R的导数性质，是SO(3)原点附近的正切空间。在t时刻，且有R = exp(^)，T = exp(^)；
--李代数：so(3) 、se(3)对加法封闭；李代数求导：掌握左乘扰动模型。
左扰动
对应的李代数为
，Rp对R求导，R用李代数表示（指数映射），转为Rp对
左扰动
对应的李代数为
，那么：
--泰勒展开： 
设此处
为误差函数，
是雅克比矩阵是误差函数对待优化变量的一阶导数；那么对应于书中推到的误差函数
，主要想优化相机位姿(李代数)和特征点的空间位置，最关键的就是关于这两个变量推到出来的矩阵。
注意：g2o提供的se3的边是旋转在前，平移在后，与李代数的定义相反。因此_jacobianOplusXx的前三列和后三列与推导出来的矩阵要交换；误差的定义：可以是观测值-预测值，也可以是预测值-观测值，此时的雅克比矩阵只需要取相反数；
3、3d-3d：
icp求解方式
非线性优化的具体方法算法，内容比较多，以后再总结。。。
参考：高博视觉十四讲