OPEN-VINS学习笔记（一）

OPEN-VINS代码学习（一）

UpdaterHelper::get_feature_jacobian_representation

全局XYZ

// 全局XYZ表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D) {
    H_f.resize(3, 3);
    H_f.setIdentity();
    return;
  }

作为规范参数化，三维点特征的全局位置仅由其在全局参考坐标系中的xyz坐标给出：
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda })\\ & =\left[\begin{array}{c}{}^{G}x\\ {}^{G}y\\ {}^{G}z\end{array}\right]\\ \text{where}\mathbf{\lambda }& ={}^G{\mathbf{p}}_f={\left[\begin{array}{ccc}{}^{G}x& {}^{G}y& {}^{G}z\end{array}\right]}^{\top }\end{array}$$
很明显，相对于特征参数的雅可比矩阵是：
$$\begin{array}{rl}\frac{\partial \mathbf{f}(\cdot )}{\partial \mathbf{\lambda }}& ={\mathbf{I}}_{3\times 3}\end{array}$$
全局逆深度

  // 全局逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH) {

    // 获取线性化特征点
    Eigen::Matrix<double, 3, 1> p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double g_rho = 1 / p_FinG.norm();
    double g_phi = std::acos(g_rho * p_FinG(2));
    // double g_theta = std::asin(g_rho*p_FinG(1)/std::sin(g_phi));
    double g_theta = std::atan2(p_FinG(1), p_FinG(0));
    Eigen::Matrix<double, 3, 1> p_invFinG;
    p_invFinG(0) = g_theta;
    p_invFinG(1) = g_phi;
    p_invFinG(2) = g_rho;

    // 获取逆深度方位角
    double sin_th = std::sin(p_invFinG(0, 0));
    double cos_th = std::cos(p_invFinG(0, 0));
    double sin_phi = std::sin(p_invFinG(1, 0));
    double cos_phi = std::cos(p_invFinG(1, 0));
    double rho = p_invFinG(2, 0);

    // 构造雅可比矩阵
    H_f.resize(3, 3);
    H_f << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    return;
  }

三维点特征的全局逆深度表示是通过以下方式给出的（类似于球坐标）：
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda })\\ & =\frac{1}{\rho }\left[\begin{array}{c}\cos (\theta )\sin (\varphi )\\ \sin (\theta )\sin (\varphi )\\ \cos (\varphi )\end{array}\right]\\ \text{where}\mathbf{\lambda }& ={\left[\begin{array}{ccc}\theta & \varphi & \rho \end{array}\right]}^{\top }\end{array}$$
相对于特征参数的雅可比矩阵可以计算为：
$$\frac{\mathbf{f}(\cdot )}{\partial \mathbf{\lambda }}=\left[\begin{array}{ccc}-\frac{1}{\rho }\sin (\theta )\sin (\varphi )& \frac{1}{\rho }\cos (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\cos (\theta )\sin (\varphi )\\ \frac{1}{\rho }\cos (\theta )\sin (\varphi )& \frac{1}{\rho }\sin (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\sin (\theta )\sin (\varphi )\\ 0& -\frac{1}{\rho }\sin (\varphi )& -\frac{1}{{\rho }^2}\cos (\varphi )\end{array}\right]$$
锚定的XYZ

// 假设我们对于这个特征有一个锚定位姿
  assert(feature.anchor_cam_id != -1);

  // 对于锚定相机，我们需要考虑其定位和方向
  Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();
  Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();
  Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();
  Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();
  Eigen::Vector3d p_FinA = feature.p_FinA;

  // 如果我正在进行前端雅可比（FEJ）优化，我应该优化锚定状态（我们是否应该优化标定？）
  // 如果需要的话，还应该获取特征的前端雅可比位置
  if (state->_options.do_fej) {
    // 在全局坐标系中的“最佳”特征
    Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
    // 使用前端雅可比（FEJ）方法将最佳结果转换到我们的锚定坐标系中
    R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot_fej();
    p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos_fej();
    p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;
  }
  Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();

  // 锚定姿态的雅可比矩阵
  Eigen::Matrix<double, 3, 6> H_anc;
  H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));
  H_anc.block(0, 3, 3, 3).setIdentity();

  // 将锚定姿态的雅可比矩阵添加到我们的返回向量中
  x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
  H_x.push_back(H_anc);
  }

我们可以将一个3D点特征表示为某个“锚定”坐标系中（例如某个IMU本地坐标系,$\left\{\begin{array}{c}{I}_a\\ G\end{array}\mathbf{R},{{}^G\mathbf{p}}_{I_a}\right\}$），通常是对应于第一次检测到该特征的相机帧的IMU姿态。
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda },\begin{array}{c}{I}_a\\ G\end{array}\mathbf{R},{{}^G\mathbf{p}}_{I_a},{}_I^C\mathbf{R}{,}^C{\mathbf{p}}_I)\\ & ={}_G^{I_a}{\mathbf{R}}^{\top }{{}_I^C\mathbf{R}}^{\top }(\mathbf{\lambda }-{}^C{\mathbf{p}}_I)+{}^G{\mathbf{p}}_{I_a}\\ \text{where}\mathbf{\lambda }& ={}^{C_a}{\mathbf{p}}_f={\left[\begin{array}{ccc}{}^{C_a}x& {}^{C_a}y& {}^{C_a}z\end{array}\right]}^{\top }\end{array}$$
由于锚定位姿参与了这种表示法，它的雅可比矩阵可以计算为：
$$\begin{array}{rl}\begin{array}{r}\frac{\partial \mathbf{f}(\cdot )}{{\partial }_G^{I_a}\mathbf{R}}\end{array}& ={-}_G^{I_a}{\mathbf{R}}^{\top }\lfloor \begin{array}{c}{}_I^C{\mathbf{R}}^{\top }{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)\times \end{array}\rfloor \\ \begin{array}{r}\frac{\partial \mathbf{f}(\cdot )}{{\partial }^G\mathbf{p}{I}_a}\end{array}& ={\mathbf{I}}_{3\times 3}\end{array}$$
此外，如果进行外部标定，还需要以下相对于IMU-相机外参的雅可比矩阵:

// 获取标定的雅可比矩阵（用于锚点克隆）
  if (state->_options.do_calib_camera_pose) {
    Eigen::Matrix<double, 3, 6> H_calib;
    H_calib.block(0, 0, 3, 3).noalias() = -R_CtoG * skew_x(p_FinA - p_IinC);
    H_calib.block(0, 3, 3, 3) = -R_CtoG;
    x_order.push_back(state->_calib_IMUtoCAM.at(feature.anchor_cam_id));
    H_x.push_back(H_calib);
  }

$$\begin{array}{rl}\frac{\partial \mathbf{f}(\cdot )}{{\partial }_I^C\mathbf{R}}& ={-}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\lfloor {(}^{C_a}{\mathbf{p}}_f-{}^C{\mathbf{p}}_I)\times \rfloor \\ \frac{\partial \mathbf{f}(\cdot )}{{\partial }^C{\mathbf{p}}_I}& ={-}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\end{array}$$

// 如果我们正在进行锚定的XYZ特征标定
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) {
    H_f = R_CtoG;
    return;

对于特征状态的雅可比矩阵可以表示为：
$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathbf{\lambda }}{=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }$$
锚定的逆深度

 // 如果我们正在进行完整的逆深度标定
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double a_rho = 1 / p_FinA.norm();
    double a_phi = std::acos(a_rho * p_FinA(2));
    double a_theta = std::atan2(p_FinA(1), p_FinA(0));
    Eigen::Matrix<double, 3, 1> p_invFinA;
    p_invFinA(0) = a_theta;
    p_invFinA(1) = a_phi;
    p_invFinA(2) = a_rho;

    // 使用锚定的逆深度
    double sin_th = std::sin(p_invFinA(0, 0));
    double cos_th = std::cos(p_invFinA(0, 0));
    double sin_phi = std::sin(p_invFinA(1, 0));
    double cos_phi = std::cos(p_invFinA(1, 0));
    double rho = p_invFinA(2, 0);
    // assert(p_invFinA(2,0)>=0.0);

    // 锚定的三维位置相对于逆深度参数的雅可比矩阵
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

类似于全局逆深度情况，我们可以在锚定坐标系中使用带有方向的逆深度（类似于球面坐标），$\left\{\begin{array}{c}{I}_a\\ G\end{array}\mathbf{R},{{}^G\mathbf{p}}_{I_a}\right\}$，来表示一个三维点特征：
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda },{}_G^{I_a}\mathbf{R},{}^G{\mathbf{p}}_{I_a},{}_I^C\mathbf{R},{}^C{\mathbf{p}}_I)\\ & ={}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }{(}^{C_a}{\mathbf{p}}_f-{}^C{\mathbf{p}}_I)+{}^G{\mathbf{p}}_{I_a}\\ & ={}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }(\frac{1}{\rho }\left[\begin{array}{c}\cos (\theta )\sin (\varphi )\\ \sin (\theta )\sin (\varphi )\\ \cos (\varphi )\end{array}\right]-{}^C{\mathbf{p}}_I)+{}^G{\mathbf{p}}_{I_a}\\ \text{where}\mathbf{\lambda }& ={\left[\begin{array}{ccc}\theta & \varphi & \rho \end{array}\right]}^{\top }\end{array}$$
对于特征状态的雅可比矩阵可以表示为：
$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathbf{\lambda }}{=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\left[\begin{array}{ccc}-\frac{1}{\rho }\sin (\theta )\sin (\varphi )& \frac{1}{\rho }\cos (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\cos (\theta )\sin (\varphi )\\ \frac{1}{\rho }\cos (\theta )\sin (\varphi )& \frac{1}{\rho }\sin (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\sin (\theta )\sin (\varphi )\\ 0& -\frac{1}{\rho }\sin (\varphi )& -\frac{1}{{\rho }^2}\cos (\varphi )\end{array}\right]$$
相对于锚点姿态的雅可比矩阵为：
$$\begin{array}{rl}\begin{array}{r}\frac{\partial \mathbf{f}(\cdot )}{{\partial }_G^{I_a}\mathbf{R}}\end{array}& ={-}_G^{I_a}{\mathbf{R}}^{\top }\lfloor \begin{array}{c}{}_I^C{\mathbf{R}}^{\top }{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)\times \end{array}\rfloor \\ \begin{array}{r}\frac{\partial \mathbf{f}(\cdot )}{{\partial }^G\mathbf{p}{I}_a}\end{array}& ={\mathbf{I}}_{3\times 3}\end{array}$$
锚定的逆深度（MSCKF版本）

// 如果我们正在使用MSCKF版本的逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    Eigen::Matrix<double, 3, 1> p_invFinA_MSCKF;
    p_invFinA_MSCKF(0) = p_FinA(0) / p_FinA(2);
    p_invFinA_MSCKF(1) = p_FinA(1) / p_FinA(2);
    p_invFinA_MSCKF(2) = 1 / p_FinA(2);

    // 使用MSCKF版本的逆深度表示
    double alpha = p_invFinA_MSCKF(0, 0);
    double beta = p_invFinA_MSCKF(1, 0);
    double rho = p_invFinA_MSCKF(2, 0);

    // 锚定的三维位置相对于逆深度参数
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << (1.0 / rho), 0.0, -(1.0 / (rho * rho)) * alpha, 0.0, (1.0 / rho), -(1.0 / (rho * rho)) * beta, 0.0, 0.0,
        -(1.0 / (rho * rho));
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

请注意，在原始的MSCKF论文[32]中使用了更简化的逆深度表示。如果在特征被测量的相机坐标系中表示，这种表示方式不会出现奇异性。
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda },{}_G^{I_a}\mathbf{R},{}^G{\mathbf{p}}_{I_a},{}_I^C\mathbf{R},{}^C{\mathbf{p}}_I)\\ & {=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }({}^{C_a}{\mathbf{p}}_f-{}^C{\mathbf{p}}_I)+{}^G{\mathbf{p}}_{I_a}\\ & {=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\left(\frac{1}{\rho }\left[\begin{array}{c}\alpha \\ \beta \\ 1\end{array}\right]-{}^C{\mathbf{p}}_I\right)+{}^G{\mathbf{p}}_{I_a}\\ \text{where}\mathbf{\lambda }& ={\left[\begin{array}{ccc}\alpha & \beta & \rho \end{array}\right]}^{\top }\end{array}$$
相对于特征状态的雅可比矩阵:
$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathbf{\lambda }}={}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\left[\begin{array}{ccc}\frac{1}{\rho }& 0& -\frac{1}{{\rho }^2}\alpha \\ 0& \frac{1}{\rho }& -\frac{1}{{\rho }^2}\beta \\ 0& 0& -\frac{1}{{\rho }^2}\end{array}\right]$$
锚定逆深度（MSCKF单深度版本）

/// 情景：使用初始方位角估计特征的单个深度值
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double rho = 1.0 / p_FinA(2);
    Eigen::Vector3d bearing = rho * p_FinA;

    // 逆深度参数对锚定的三维位置的雅可比矩阵
    Eigen::Vector3d d_pfinA_drho;
    d_pfinA_drho << -(1.0 / (rho * rho)) * bearing;
    H_f = R_CtoG * d_pfinA_drho;
    return;
  }

这种特征表示基于MSCKF表示和VINS-Mono中的单深度表示。与VINS-Mono中的实现相比，我们在处理特征的方位角时非常小心。在初始化过程中，我们首先初始化一个完整的三维特征，然后通过边缘化其方位角部分，将深度保留在状态向量中。边缘化的方位角在所有未来的线性化中保持不变。

然后在更新过程中，我们在每个时间步骤执行零空间投影，以消除特征对该方位角的依赖。为了做到这一点，我们需要至少两组$UV$测量数据来执行这个方位角零空间操作，因为在这个过程中我们失去了特征的两个维度。我们可以将特征测量函数定义如下：
$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathbf{\lambda },{}_G^{I_a}\mathbf{R},{}^G{\mathbf{p}}_{I_a},{}_I^C\mathbf{R},{}^C{\mathbf{p}}_I)\\ & {=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }{(}^{C_a}{\mathbf{p}}_f-{}^C{\mathbf{p}}_I)+{}^G{\mathbf{p}}_{I_a}\\ & {=}_G^{I_a}{\mathbf{R}}^{\top }{}_I^C{\mathbf{R}}^{\top }\left(\frac{1}{\rho }\hat{\mathbf{b}}-{}^C{\mathbf{p}}_I\right)+{}^G{\mathbf{p}}_{I_a}\\ \text{where}\mathbf{\lambda }& =\left[\begin{array}{c}\rho \end{array}\right]\end{array}$$
在上述情况中，我们定义了一个方位角$\hat{\mathbf{b}}$，它是初始化后特征的边缘化方位角。在收集到两个测量值之后，我们可以进行零空间投影，以消除相对于这个方位角变量的雅可比矩阵。

完整代码：

void UpdaterHelper::get_feature_jacobian_representation(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
                                                        std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<Type>> &x_order) {

  // 全局XYZ表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D) {
    H_f.resize(3, 3);
    H_f.setIdentity();
    return;
  }

  // 全局逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH) {

    // 获取线性化特征点
    Eigen::Matrix<double, 3, 1> p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double g_rho = 1 / p_FinG.norm();
    double g_phi = std::acos(g_rho * p_FinG(2));
    // double g_theta = std::asin(g_rho*p_FinG(1)/std::sin(g_phi));
    double g_theta = std::atan2(p_FinG(1), p_FinG(0));
    Eigen::Matrix<double, 3, 1> p_invFinG;
    p_invFinG(0) = g_theta;
    p_invFinG(1) = g_phi;
    p_invFinG(2) = g_rho;

    // 获取逆深度方位角
    double sin_th = std::sin(p_invFinG(0, 0));
    double cos_th = std::cos(p_invFinG(0, 0));
    double sin_phi = std::sin(p_invFinG(1, 0));
    double cos_phi = std::cos(p_invFinG(1, 0));
    double rho = p_invFinG(2, 0);

    // 构造雅可比矩阵
    H_f.resize(3, 3);
    H_f << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    return;
  }

  //======================================================================
  //======================================================================
  //======================================================================

 // 假设我们对于这个特征有一个锚定位姿
  assert(feature.anchor_cam_id != -1);

  // 对于锚定相机，我们需要考虑其定位和方向
  Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();
  Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();
  Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();
  Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();
  Eigen::Vector3d p_FinA = feature.p_FinA;

  // 如果我正在进行前端雅可比（FEJ）优化，我应该优化锚定状态（我们是否应该优化标定？）
  // 如果需要的话，还应该获取特征的前端雅可比位置
  if (state->_options.do_fej) {
    // 在全局坐标系中的“最佳”特征
    Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
    // 使用前端雅可比（FEJ）方法将最佳结果转换到我们的锚定坐标系中
    R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot_fej();
    p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos_fej();
    p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;
  }
  Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();

  // 锚定姿态的雅可比矩阵
  Eigen::Matrix<double, 3, 6> H_anc;
  H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));
  H_anc.block(0, 3, 3, 3).setIdentity();

  // 将锚定姿态的雅可比矩阵添加到我们的返回向量中
  x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
  H_x.push_back(H_anc);

  // 获取标定的雅可比矩阵（用于锚点克隆）
  if (state->_options.do_calib_camera_pose) {
    Eigen::Matrix<double, 3, 6> H_calib;
    H_calib.block(0, 0, 3, 3).noalias() = -R_CtoG * skew_x(p_FinA - p_IinC);
    H_calib.block(0, 3, 3, 3) = -R_CtoG;
    x_order.push_back(state->_calib_IMUtoCAM.at(feature.anchor_cam_id));
    H_x.push_back(H_calib);
  }

  // 如果我们正在进行锚定的XYZ特征标定
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) {
    H_f = R_CtoG;
    return;
  }

  // 如果我们正在进行完整的逆深度标定
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double a_rho = 1 / p_FinA.norm();
    double a_phi = std::acos(a_rho * p_FinA(2));
    double a_theta = std::atan2(p_FinA(1), p_FinA(0));
    Eigen::Matrix<double, 3, 1> p_invFinA;
    p_invFinA(0) = a_theta;
    p_invFinA(1) = a_phi;
    p_invFinA(2) = a_rho;

    // 使用锚定的逆深度
    double sin_th = std::sin(p_invFinA(0, 0));
    double cos_th = std::cos(p_invFinA(0, 0));
    double sin_phi = std::sin(p_invFinA(1, 0));
    double cos_phi = std::cos(p_invFinA(1, 0));
    double rho = p_invFinA(2, 0);
    // assert(p_invFinA(2,0)>=0.0);

    // 锚定的三维位置相对于逆深度参数的雅可比矩阵
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

  // 如果我们正在使用MSCKF版本的逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    Eigen::Matrix<double, 3, 1> p_invFinA_MSCKF;
    p_invFinA_MSCKF(0) = p_FinA(0) / p_FinA(2);
    p_invFinA_MSCKF(1) = p_FinA(1) / p_FinA(2);
    p_invFinA_MSCKF(2) = 1 / p_FinA(2);

    // 使用MSCKF版本的逆深度表示
    double alpha = p_invFinA_MSCKF(0, 0);
    double beta = p_invFinA_MSCKF(1, 0);
    double rho = p_invFinA_MSCKF(2, 0);

    // 锚定的三维位置相对于逆深度参数
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << (1.0 / rho), 0.0, -(1.0 / (rho * rho)) * alpha, 0.0, (1.0 / rho), -(1.0 / (rho * rho)) * beta, 0.0, 0.0,
        -(1.0 / (rho * rho));
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

  /// 情景：使用初始方位角估计特征的单个深度值
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {

    // 获取逆深度表示 (should match what is in Landmark.cpp)
    double rho = 1.0 / p_FinA(2);
    Eigen::Vector3d bearing = rho * p_FinA;

    // 逆深度参数对锚定的三维位置的雅可比矩阵
    Eigen::Vector3d d_pfinA_drho;
    d_pfinA_drho << -(1.0 / (rho * rho)) * bearing;
    H_f = R_CtoG * d_pfinA_drho;
    return;
  }

  // 失败，无效的表示，未编程
  assert(false);
}

UpdaterHelper::get_feature_jacobian_full

透视投影（方位角）测量模型

// Get current IMU clone state
std::shared_ptr<PoseJPL> clone_Ii = state->_clones_IMU.at(feature.timestamps[pair.first].at(m));
Eigen::Matrix3d R_GtoIi = clone_Ii->Rot();
Eigen::Vector3d p_IiinG = clone_Ii->pos();

// Get current feature in the IMU
Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);

// Project the current feature into the current frame of reference
Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
Eigen::Vector2d uv_norm;
uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

// Distort the normalized coordinates (radtan or fisheye)
Eigen::Vector2d uv_dist;
uv_dist = state->_cam_intrinsics_cameras.at(pair.first)->distort_d(uv_norm);

// Our residual
Eigen::Vector2d uv_m;
uv_m << (double)feature.uvs[pair.first].at(m)(0), (double)feature.uvs[pair.first].at(m)(1);
res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;

考虑在某个时间点$k$从相机图像中检测到一个三维特征点，其在图像平面上的测量值$uv$（即对应的像素坐标）如下所示：
$$\begin{array}{rl}{\mathbf{Z}}_{m,k}& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f),\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t{(}^G{\mathbf{p}}_f,{}_G^{C_k}\mathbf{R}{,}^G{\mathbf{p}}_{C_k})),\mathbf{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t({\mathbf{h}}_r(\mathbf{\lambda },\cdots ),{}_G^{C_k}\mathbf{R}{,}^G{\mathbf{p}}_{C_k})),\mathbf{\zeta })+{\mathbf{n}}_k\end{array}$$
其中，${\mathbf{n}}_k$是测量噪声，通常假设为零均值的白噪声；${\mathbf{Z}}_{m,k}$是归一化的无畸变$uv$测量值；$\zeta$是相机的内参数，如焦距和畸变参数；${}^{C_k}{\mathbf{p}}_f$是特征点在当前相机坐标系 { C k } \left\{C_k\right\} {Ck​}中的位置；${}^G{\mathbf{p}}_f$是特征点在全局坐标系 { G } \left\{G\right\} {G}中的位置；$\left\{{}_G^{C_k}\mathbf{R}{,}^G{\mathbf{p}}_{C_k})\right\}$表示当前相机的姿态（位置和方向）相对于全局坐标系的变换；$\lambda$是特征点的其他参数表示（位置之外的参数），如简单的xyz位置或具有方位角的逆深度。

在上述表达式中，我们将测量函数分解为多个函数的串联，这些函数将状态映射到图像平面上的原始$uv$测量值。值得注意的是，由于我们将进行内外参数的校准，并使用不同的特征表示，上述相机测量模型是通用的。

UpdaterHelper::nullspace_project_inplace

• 目的

零空间投影就是为了去除误差模型对三维点的依赖. 换句话说（不严谨）: 使观测模型与三维点无关, 把三维点边缘化掉

对于误差函数的线性展开: $r=HX+n$ , （ $r$ 为residual， $H$ 为Jacobian， $n$ 为nosie）， 需要保证等号右边除了 $HX$ 之外其他项都与 $X$ 无关, 才能将 $H$ 给到EKF 进行更新.

MSCKF 的状态量不包含三维特征点. 但是在构建重投影误差时, 需要三维点的坐标. 令待估计状态量 $X$, 观测对 $X$ 的Jacobian 为 $H_X$ , 观测对三维点$P$ 的Jacobian 为 $H_f$ , 观测噪声为 $n$ , 观测误差的一阶泰勒展开如下:
$$r=z-h(X,P)\simeq H_X\tilde{X}+H_f\tilde{P}+n(1)$$
由此引出一个问题: 上述泰勒近似模型的 $\tilde{P}$与 $X$ 不是无关的， $H_x$ 是不能直接用于EKF更新的.因此,我们想要消除 $H_f\tilde{P}$

• 原理

Brief: 想要消除等式某一项,只需要在等式左右同时乘一个矩阵使得这项为0即可.

• 实现

对于式(1),采用上述方法计算出对应的 $A$ ,对等式两边同时左乘 $A^{\top }$ :
$$r_O=A^T(z-h(X,P))\simeq A^T{H}_X\tilde{X}+A^{T}n=H_O\tilde{X}+n_O(2)$$
然后, 将$r_O$ 作为EKF的residual, $H_O$作为EKF的Jacobian, 进行EKF Update.

式(2)提供了点 $P$ 的所有观测帧之间的共视约束, 它体现了观测 $z$ 能给到的全部有效信息,因此EKF的结果是最优的(除了线性展开带来的误差).

$H_f$是列满秩的, 通常行数 $m$ 大于列数 $n$ (对于 $XYZ$ 表示,$n$=3 ), 它的零空间维度是 $m-n$ .因此,投影后的残差项 $r_O$ 是 $(m-n)\ast 1$ 维的向量.

在代码实现时,其实并不需要显式的求解 $A$ . EKF需要的三个量 $r_O$,$H_O$,$n_O$ 的协方差矩阵 $R_O$ 求解如下:

1. **$r_O$和 $H_O$ : **

$r$ 和 $H_x$ 在 $H_f$ 零空间的投影可以通过Givens Rotation快速计算得出.

2. $R_o$ :

$$R_O=A^{T}RA(3)$$

完整代码：

void UpdaterHelper::nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {

  // 对所有变量应用H_f的左零空间
  // 基于《矩阵计算》第四版（Golub和Van Loan）中的算法
  // 参见第252页，算法5.2.4，了解这两个循环的工作原理
  // 它们使用“matlab”索引表示法，因此我们需要从所有索引中减去1
  Eigen::JacobiRotation<double> tempHo_GR;
  for (int n = 0; n < H_f.cols(); ++n) {
    for (int m = (int)H_f.rows() - 1; m > n; m--) {
      // Givens矩阵G
      tempHo_GR.makeGivens(H_f(m - 1, n), H_f(m, n));
      // 将G乘以每个矩阵中对应的行(m-1, m)
      // Note: 我们只将G应用于非零列[n:Ho.cols()-n-1]，虽然它等效于将G应用于整个列[0:Ho.cols()-1].
      (H_f.block(m - 1, n, 2, H_f.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (H_x.block(m - 1, 0, 2, H_x.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
    }
  }

  // 如果H_f雅可比矩阵是一个3D位置，其最大秩为3，因此左零空间的大小为Hf.rows()-3
  // NOTE: 需要使用eigen3进行评估，因为这里会出现别名问题！
  // H_f = H_f.block(H_f.cols(),0,H_f.rows()-H_f.cols(),H_f.cols()).eval();
  H_x = H_x.block(H_f.cols(), 0, H_x.rows() - H_f.cols(), H_x.cols()).eval();
  res = res.block(H_f.cols(), 0, res.rows() - H_f.cols(), res.cols()).eval();

  // 合理性检查
  assert(H_x.rows() == res.rows());
}

UpdaterHelper::measurement_compress_inplace

• 目的

EKF中的 $H$ 矩阵, 行数由观测数量决定，列数是状态量维度。行数会随着观测数量增长到很大(对于单个点 $P$ , 观测数量为$M_P$,每一个为2维 $uv$ ,则行数为 2$M_P$ )，使得矩阵运算的计算量急速增大.

通常,观测数量远大于状态量维度, $H$ 矩阵行数远大于列数. 通过把residual投影到 $H$ 列空间基向量上实现降维, 可以把 $H$ 行数减少到与列数相同,就可以节省EKF Update中矩阵运算的时间.

对于 $H$ 是胖矩阵 (行数小于列数)的情况,就没有必要进行模型压缩了

• 原理:

QR分解

对 $H$ 矩阵进行QR分解
$$H=[\begin{array}{cc}{Q}_1& Q_2\end{array}]\left[\begin{array}{c}{T}_H\\ 0\end{array}\right](4)$$
其中 $Q_1$ 是$H$列空间的基, $Q_2$ 是 $H$ 零空间的基, $T_H$ 是上三角矩阵.则式(2)可得出:
$$r_O=[\begin{array}{cc}{Q}_1& Q_2\end{array}]\left[\begin{array}{c}{T}_H\\ 0\end{array}\right]\tilde{X}+n_O(5)$$
两边同乘$\left[\begin{array}{c}{Q}_1^T\\ Q_2^T\end{array}\right]$：
$$\left[\begin{array}{c}{Q}_1^T{r}_O\\ Q_2^T{r}_O\end{array}\right]=\left[\begin{array}{c}{T}_H\\ 0\end{array}\right]\tilde{X}+\left[\begin{array}{c}{Q}_1^T{n}_O\\ Q_2^T{n}_O\end{array}\right](6)$$
通过上述方法, 将residual $r_O$ 投影到 $H$ 列空间的基上,可以保留观测的所有有效信息.而 $Q_2^T{n}_O$ 完全是噪声,可以直接抛弃.然后, 将以下残差模型用于EKF Update:
$$r_n=Q_1^T{r}_O=T_H\tilde{X}+n_n(7)$$
其中噪声$n_n$的协方差矩阵为.
$$R_O=Q_1^{T}R{Q}_1(8)$$
$Q_1$的求解方法参考UpdaterHelper::nullspace_project_inplace.

void UpdaterHelper::measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {

  // Return if H_x is a fat matrix (there is no need to compress in this case)
  if (H_x.rows() <= H_x.cols())
    return;

  // Do measurement compression through givens rotations
  // Based on "Matrix Computations 4th Edition by Golub and Van Loan"
  // See page 252, Algorithm 5.2.4 for how these two loops work
  // They use "matlab" index notation, thus we need to subtract 1 from all index
  Eigen::JacobiRotation<double> tempHo_GR;
  for (int n = 0; n < H_x.cols(); n++) {
    for (int m = (int)H_x.rows() - 1; m > n; m--) {
      // Givens matrix G
      tempHo_GR.makeGivens(H_x(m - 1, n), H_x(m, n));
      // Multiply G to the corresponding lines (m-1,m) in each matrix
      // Note: we only apply G to the nonzero cols [n:Ho.cols()-n-1], while
      //       it is equivalent to applying G to the entire cols [0:Ho.cols()-1].
      (H_x.block(m - 1, n, 2, H_x.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
    }
  }

  // If H is a fat matrix, then use the rows
  // Else it should be same size as our state
  int r = std::min(H_x.rows(), H_x.cols());

  // Construct the smaller jacobian and residual after measurement compression
  assert(r <= H_x.rows());
  H_x.conservativeResize(r, H_x.cols());
  res.conservativeResize(r, res.cols());
}