VINS-Fusion代码阅读（四）

视觉约束部分
和前面的IMUFactor类似，ProjectionFactor也是一个继承自ceres::SizedCostFunction<2, 7, 7, 7, 1>的类，同样，类的框架比较简单，重点仍然放在将基类中对应虚函数覆盖了的Evaluate。
（注：factor有因子的含义）
稍微提及一下类中的数据成员：
pts_i和pts_j：具体指什么含义？（分别为第l个路标点在第i, j个相机归一化相机坐标系中的观察到的坐标，${\bar{P}}_l^{c_i}$ 和 ${\bar{P}}_l^{c_j}$）；
tangent_base：正切平面上的任意两个正交基（在构造函数中通过计算？被赋值）；
静态数据成员sqrt_info和sum_t：在何时被赋值的呢？

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>
{
  public:
    ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j);
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
    void check(double **parameters);

    Eigen::Vector3d pts_i, pts_j;
    Eigen::Matrix<double, 2, 3> tangent_base;
    static Eigen::Matrix2d sqrt_info;
    static double sum_t;
};

步入正题，ProjectionFactor类中的Evaluate成员函数：
优化变量：$[p_{b_i}^w,q_{b_i}^w],[p_{b_j}^w,q_{b_j}^w],[p_c^b,q_c^b],{\lambda }_l$
分别对应：[Pi, Qi], [Pj, Qj], [tic, qic], inv_dep_i
（此处的$\lambda$表示什么含义？逆深度inv_dep_i）（$w,b$代表的坐标系具体指什么？）

解析中（27）式：
P l c j = R b c { R w b j [ R b i w ( R c b 1 λ l P ˉ l c i + p c b ) + p b i w − p b j w ] − p c b } P^{c_j}_l=R^c_b\{R^{b_j}_w[R^w_{b_i}(R^b_c\frac{1}{\lambda_l}\bar{P}^{c_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}]-p^b_c\} Plcj​​=Rbc​{Rwbj​​[Rbi​w​(Rcb​λl​1​Pˉlci​​+pcb​)+pbi​w​−pbj​w​]−pcb​}
$\frac{1}{{\lambda }_l}{\bar{P}}_l^{c_i}$ ==>Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
（因此，pts_i表示${\bar{P}}_l^{c_i}$，为第l个路标点在第i个相机归一化相机坐标系中的观察到的坐标）；
$(R_c^b\ast +p_c^b)$ ==>Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
（此处可以看出来，imu与$b$系是一个系？）
$R_{b_i}^w\ast +p_{b_i}^w$ ==>Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
$R_w^{b_j}(\ast -p_{b_j}^w)$ ==>Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
$R_b^c(\ast -p_c^b)$ ==>Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
因此，pts_camera_j表示$P_l^{c_j}$，是由${\bar{P}}_l^{c_i}$计算得来的。

解析中（25）式：
$r_c({\hat{z}}_l^{c_j},X)=\left[\begin{array}{c}\overrightarrow{b_1}\\ \overrightarrow{b_2}\end{array}\right](\frac{P_l^{c_j}}{||P_l^{c_j}||}-{\bar{P}}_l^{c_j})$
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
residual = sqrt_info * residual;

最后，计算Jacobian，解析中（28）式：
注：此处解析上有一些矩阵维数上的错误，应该为$3\times \ast$，而非$2\times \ast$。
以其中一个为例，分析公式与代码对应关系：
$J[0{]}^{2\times 7}=[\frac{\partial r_c}{\partial p_{b_i}^w},\frac{\partial r_c}{\partial q_{b_i}^w}]$
代码中首先定义了一个jaco_i为$3\times 6$，然后用一个reduce$2\times 3$去乘，得到的$2\times 6$的结果作为$J[0]$的左边6列，最后一列为0；具体如下：
Eigen::Matrix<double, 3, 6> jaco_i;
$R_b^c{R}_w^{b_j}$
jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
$-R_b^c{R}_w^{b_j}{R}_{b_i}^w(R_c^b\frac{1}{{\lambda }_l}{\bar{P}}_l^{c_i}+p_c^b)$
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

Eigen::Matrix<double, 2, 3> reduce(2, 3);
reduce = tangent_base * norm_jaco;此处norm_jaco表达什么含义？对应公式？
reduce = sqrt_info * reduce;

Eigen::Map的用法？
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
jacobian_pose_i.rightCols<1>().setZero();

$J[1{]}^{2\times 7}$、$J[2{]}^{2\times 7}$和$J[3{]}^{2\times 1}$的计算类似。

至此，视觉约束暂时告一段落。

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual;

    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;

        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }

        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix<double, 3, 6> jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
#if 1
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
        }
    }
    sum_t += tic_toc.toc();

    return true;
}