传统ORB-SLam中位姿优化中雅克比矩阵讲解

由于之前的鱼眼orbslam只有单目部分，所以在优化时也只是用了单目位姿优化和三维坐标点优化，并没有将双目的优化添加进去，不知道是否对结果有影响；

这里添加双目的优化部分，主要是将添加雅克比矩阵；

orbslam中的优化部分使用了g2o库，具体的详细讲解可以参考https://zhuanlan.zhihu.com/p/58521241 讲解的很详细

这里不加赘述，主要从单目的雅克比矩阵讲解，延伸到双目的雅克比矩阵，以及带有鱼眼EUCM模型的鱼眼双目的雅克比矩阵的推导

(雅克比矩阵即为误差函数关于优化项的导数，所以depends on 优化项的个数， 雅克比矩阵的个数也不同)

1. 单目的雅克比矩阵

首先以单目位姿优化为例， 因为只对位姿做优化，所以这里采用一元边BaseUnaryEdge​ 进行边的定义，如下：

BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

误差函数e的公式不在推导，可以参见SLAM十四讲P163的讲解，这里假设空间点P 经过位姿变换后在相机坐标系下的坐标为P' =[X',Y',Z']T
根据相机的投影公式，将坐标P‘ 转换到像素坐标系下公式如图：

 

 接下来要计算的误差函数就是将 实际的特征点的二维坐标 与该投影值u,v 进行比较，求差，因为理想状态下两者应该是完全一致的，但是由于位姿的误差等，导致二者不同，因此误差就是需要经二者的差值最小化，也就是最小化重投影误差函数；

首先考虑误差函数e关于扰动量的导数，利用链式法则可以写成如下形式：

因为上面u，v的公式已经明确，那么e关于P’ 的导数就可以写成

（公式1）

同时，第二项P‘ 关于扰动的导数按照之前的推导为：

（这里需要注意，在g2o中的形式一般为[-P'^ , I ], 需要在写雅克比矩阵的时候注意一下）

这里P '^ 为三维坐标P‘ 坐标的反对成矩阵，因此 —P'^ 为在反对成矩阵的基础上添加负号即可；

             |0   z    -y|

-P’^ =  |z   0    -x|

            |-y   x    0|

最终的误差关于扰动量的导数公式如下：

该公式也就对应了ｇ２０库中types_six_dof_expmap.cpp 文件中　EdgeSE3ProjectXYZOnlyPose类　对应的linearizeOplus() 中的雅克比矩阵

  //这里要注意的就是ｇ２０中是Ｐ'^在前，Ｉ矩阵在后
  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;

因为只优化位姿，所以这里定义了一元边，如果是将位姿和三维坐标点一起优化，这里需要定义二元边

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
其中的参数分别是　定义误差函数的维数，坐标形式，顶点０的类型(这里是三维点)，顶点１的类型(这里为相机的位姿)

定义完成二元边后，只需要添加误差函数ｅ关于三维空间点Ｐ（ＸＹＺ）的导数即可，根据SLAM 十四讲的推导，这里只需啊哟对之前Ｐ’的求导乘以Ｒ矩阵即可，如下：

具体的代码部分可以参考 g2o 库中types_six_dof_expmap.cpp 文件中EdgeSE3ProjectXYZ::linearizeOplus()　函数：只需要添加一个雅克比项即可，但是需要注意的这里要先添加对三维点的优化，再添加对位姿的优化：

Matrix tmp;
  tmp(0,0) = fx;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*fx;

  tmp(1,0) = 0;
  tmp(1,1) = fy;
  tmp(1,2) = -y/z*fy;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

２．　普通相机双目优化公式：

普通相机的双目优化时，在误差项求解时，除了要优化左相机中的特征点的测量值和观测值之间的差值，还包括右目中对应特征点的横坐标的观测值与测量值之间的关系；

因此测量值相关公式由两个变成三个，如下：

公式改变带来的主要是公式1中误差函数关于相机坐标系下的坐标点的导数，雅克比矩阵也由最开是2x6变成3x6的矩阵

只需要按照这个顺序依次求导带入即可，对应的相关代码为：g2o 库中types_six_dof_expmap.cpp 文件中　EdgeStereoSE3ProjectXYZ::linearizeOplus() 函数对应的雅克比矩阵的下半部分：

  _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;
  _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;
  _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);
  _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);
  _jacobianOplusXj(2,4) = 0;
  _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;

同理，对坐标点的优化也要相应的变成3x3；