李代数在BA中的应用

首先，给出李代数($so3,se3$)与对应李群($SO3,SE3$)的定义如下：

参考我之前的博客位姿表示方法，$so3$其实就是罗德里格斯表示法。
李代数的作用就是建立起了旋转矩阵和旋转向量之间的指数(对数)关系，这有利于在优化过程中的求导运算。
下面参考https://github.com/izhengfan/ba_demo_ceres中的代码片段，介绍李代数在BA中的应用。

    if(jacobians != NULL)
    {
        if(jacobians[0] != NULL)
        {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J_se3(jacobians[0]);
            J_se3.block<2,3>(0,0) = - J_cam * skew(p);
            J_se3.block<2,3>(0,3) = J_cam;
        }
        if(jacobians[1] != NULL)
        {
            Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor> > J_point(jacobians[1]);
            J_point = J_cam * quaterd.toRotationMatrix();
        }
    }

上面是计算CostFuntion相对于位姿$se3$和特征点的雅可比的代码片段。
这里$J_{cam}$是CostFunction相对于特征点在相机坐标系下坐标的雅可比。那么，接下来就要求特征点在相机坐标系下的坐标相对于位姿$se3$的雅可比：

在BA中，还可以通过四元数表示位姿。但是，四元数的自由度比旋转实际的自由度要多一维，所以在Ceres中要通过

virtual int LocalSize() const {}

给出它的实际自由度。另外在定义雅可比矩阵的时候可以参照与$se3$情况，但需要通过：

bool PoseSE3Parameterization<7>::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor> > J(jacobian);
    J.setZero();
    J.block<6,6>(0, 0).setIdentity();
    return true;
}

将对应的雅可比映射为$se3$的雅可比。注意，此时的Plus函数的增量也是$se3$的维度。
Ceres中给出一个例子来解释这一过程：如果优化空间是一个球面，那么在与球面垂直方向上的搜索是无意义的，应该先将将参数空间投影到球面的正切面(二维平面),再将增量映射回球面。
这一过程其实就是针对优化变量存在关联的情况，将有约束的优化问题转化为无约束的优化问题。

参考：https://blog.csdn.net/u011178262/article/details/85016981
http://ceres-solver.org/nnls_modeling.html