【OV】基于滑窗的残差构建（包含投影误差），目标函数构造，优化滑动窗口中的状态变量（如位姿、速度等）。)舒尔补方法实现SchurVINS::Solve3

1. 原理解析

在 SchurVINS::Solve3 中，协方差矩阵的更新是通过舒尔补方法（Schur Complement Method）完成的，该方法用于有效地消除系统中某些变量（如特征点的三维坐标），从而减少计算复杂度并优化求解效率。

1.1 问题背景

考虑多视图几何优化中的代价函数：

$$\text{Cost}=\frac{1}{2}\Vert r{\Vert }^2,$$

其中 $r$ 是残差向量，包含投影误差，定义为：

$$r=z-h(x,p),$$

• ( z ): 特征观测值，
• ( h(x, p) ): 系统状态 ( x ) 和特征点位置 ( p ) 的投影模型，
• ( x ): 系统的状态变量（包括位姿、速度、偏置等），
• ( p ): 特征点的三维位置。

目标是通过优化 ( x ) 和 ( p ) 来最小化代价函数。

---

1. 2. 舒尔补基本原理

如果系统状态分为两部分 ( x ) 和 ( p )，优化问题的增量方程可以写成如下形式：

$$\begin{array}{rl}{H}_{xx}\Delta x+H_{xp}\Delta p& =b_x\\ H_{px}\Delta x+H_{pp}\Delta p& =b_p\end{array}$$

• ( H ) 是赫塞矩阵，
• ( b ) 是梯度向量。

为了消除 $\Delta p$（特征点变量的增量），使用舒尔补方法得到关于$\Delta x$ 的方程：

$$\left(H_{xx}-H_{xp}{H}_{pp}^{-1}{H}_{px}\right)\Delta x=b_x-H_{xp}{H}_{pp}^{-1}{b}_p.$$

更新后的系统形式为：

• 等效赫塞矩阵：

$$H_{xx}^{\prime }=H_{xx}-H_{xp}{H}_{pp}^{-1}{H}_{px},$$

• 等效梯度：

$$b_x^{\prime }=b_x-H_{xp}{H}_{pp}^{-1}{b}_p.$$

这种方式避免了直接优化 $p$，而是通过消除 $p$ 来减少系统维度，极大地提高了优化效率。

---

1.3. 代码实现中的关键部分

在代码中，通过以下步骤完成协方差矩阵更新和舒尔补计算：

(1) 初始化变量

初始化特征点相关的协方差矩阵和梯度向量：

curr_pt->V.noalias() += jf.transpose() * jf;  // pt 2 pt
curr_pt->gv.noalias() += jf.transpose() * r;  // pt grad
feature->W.noalias() = jx.transpose() * jf;  // pos 2 pt

这里：

• $V$: 特征点对应的赫塞矩阵 $H_{pp}$，
• $gv$: 特征点梯度 $b_p$，
• $W$: 交叉项 $H_{xp}$。

(2) 舒尔补消元

在遍历所有特征点后，对每个点计算 ( H_{pp}^{-1} ) 并完成舒尔补操作：

pt->Vinv = pt->V.inverse();  // 计算 H_pp 的逆
const Matrix6o3d WVinv = iti->second.W * pt->Vinv;  // 计算 H_xp * H_pp^{-1}

计算等效赫塞矩阵和梯度：

Matrix6d p2p_schur = WVinv * itj->second.W.transpose();  // posi 2 posj schur
Amtx.block(pi_bias, pj_bias, 6, 6) -= p2p_schur;
Bvct.segment(pi_bias, 6) -= WVinv * pt->gv;  // 更新梯度

(3) 状态更新

最终，通过等效赫塞矩阵和梯度，调用 StateUpdate 完成状态更新：

StateUpdate(Amtx, Bvct);

---

1.4. 优化的效率

通过舒尔补消去特征点 $p$，原本的优化问题中需要优化的变量大幅减少，使得优化问题从高维变为低维。同时，计算过程也更加稀疏化，便于数值优化库（如 Ceres Solver）高效求解。

总之，代码中通过舒尔补方法避免了对特征点的直接优化，大幅降低了计算复杂度，且保留了状态估计的准确性。

2. 流程总结

这个 SchurVINS::Solve3() 接口主要实现的是基于Schur补操作的视觉惯性导航系统（Visual-Inertial Navigation System, VINS）中窗口状态优化的部分。以下从功能和算法流程两个角度进行分析：

---

2.1 功能说明

1. 功能描述：

   • 该函数的核心任务是利用局部窗口中的观测数据，通过Schur补简化优化问题，优化滑动窗口中的状态变量（如位姿、速度等）。
   • 该过程处理特征点（local_pts）与滑动窗口中帧的观测关系，构建雅可比矩阵与残差向量，然后基于最小二乘优化更新状态。

2. 主要功能模块：

   • 检查状态窗口：确保滑动窗口中至少包含两个状态（以便进行相对位姿的优化）。
   • 计算点与相机的雅可比和残差：
     • 遍历局部特征点并计算其残差。
     • 对关键帧中的观测点，计算其投影到图像平面上的雅可比矩阵。
   • Huber鲁棒核：对残差施加Huber核，减小异常值对优化的影响。
   • Schur补操作：基于Schur补方法，消除特征点的影响，将优化变量维度降低到滑动窗口内的状态变量。
   • 更新系统状态：构造归一化的系统信息矩阵和梯度向量，通过线性求解的方式优化滑动窗口状态。

---

2.2 算法流程

1. 初始化和预处理

• 检查窗口状态数量，确保包含至少两个状态（state_size < 2 时直接退出）。
• 提取窗口状态的最小、最大索引，并初始化信息矩阵 Amtx 和梯度向量 Bvct。

2. 遍历局部特征点

对于每个局部特征点（local_pts）：

• 检查点状态：
  • 过滤掉无效点（如不在当前窗口内的点，或者点状态异常）。
• 计算残差：
  • 根据特征点的三维位置（Pw）和观测相机的位姿，计算投影误差（残差 r）。
• Huber核：
  • 应用Huber鲁棒核函数减少异常值的影响。
• 计算雅可比矩阵：
  • 计算点到相机投影的雅可比矩阵 dr_dpc。
  • 计算与状态变量相关的雅可比 jx 和与特征点位置相关的雅可比 jf。

3. 信息矩阵和梯度向量更新

• 对于每个点的观测：
  • 累加状态与状态之间的二阶信息（jx.transpose() * jx）到信息矩阵 Amtx。
  • 累加梯度信息到 Bvct。

4. Schur补消元特征点

• 对于每个特征点：
  • 计算点的信息矩阵逆（Vinv）以及其对状态变量的影响。
  • 利用Schur补操作，将点的影响整合到状态之间的信息矩阵中，减少优化变量维度。

5. 状态更新

• 利用构造好的 Amtx 和 Bvct，调用 StateUpdate 函数执行优化更新。
• 更新后的状态包括滑动窗口内的位姿、速度等变量。

---

void Solve3() {
    // ##### 1. 初始化和预处理 #####
    if (state_size < 2) {
        return; // 确保至少有两个状态变量
    }

    int min_idx = state_indices.front();
    int max_idx = state_indices.back();

    // 初始化信息矩阵和梯度向量
    Eigen::MatrixXd Amtx = Eigen::MatrixXd::Zero(state_dim, state_dim);
    Eigen::VectorXd Bvct = Eigen::VectorXd::Zero(state_dim);

    // ##### 2. 遍历局部特征点 #####
    for (const auto& point : local_pts) {
        // 检查点状态是否有效
        if (!point.isValid() || point.frame_idx < min_idx || point.frame_idx > max_idx) {
            continue;
        }

        // 提取特征点在世界坐标系中的位置
        Eigen::Vector3d Pw = point.world_position;

        // 遍历点的所有观测
        for (const auto& obs : point.observations) {
            int frame_idx = obs.frame_idx;
            const CameraPose& pose = GetCameraPose(frame_idx);

            // 计算投影误差（残差）
            Eigen::Vector2d r;
            bool valid = ComputeReprojectionResidual(Pw, pose, obs.pixel, r);
            if (!valid) continue;

            // 应用 Huber 核
            double weight = ComputeHuberWeight(r.norm());
            r *= weight;

            // 计算雅可比矩阵
            Eigen::Matrix<double, 2, 6> dr_dpc; // 对相机位姿的雅可比
            Eigen::Matrix<double, 2, 3> dr_dpw; // 对点位置的雅可比
            ComputeJacobian(Pw, pose, dr_dpc, dr_dpw);

            // 计算状态变量相关的雅可比 (jx) 和特征点位置的雅可比 (jf)
            Eigen::MatrixXd jx = MapToStateJacobian(dr_dpc, frame_idx, state_dim);
            Eigen::MatrixXd jf = dr_dpw;

            // ##### 3. 更新信息矩阵和梯度向量 #####
            Amtx += jx.transpose() * jx * weight;
            Bvct -= jx.transpose() * r;
        }
    }

    // ##### 4. Schur补消元特征点 #####
    for (const auto& point : local_pts) {
        // 跳过无效点
        if (!point.isValid() || point.frame_idx < min_idx || point.frame_idx > max_idx) {
            continue;
        }

        Eigen::MatrixXd Hf_inv = ComputeFeaturePointInformationInverse(point);
        Eigen::MatrixXd Jfx = ComputeFeatureStateJacobian(point);

        // Schur补消元
        Eigen::MatrixXd temp = Jfx.transpose() * Hf_inv * Jfx;
        Amtx -= temp;
        Bvct -= Jfx.transpose() * Hf_inv * ComputeFeatureResidual(point);
    }

    // ##### 5. 状态更新 #####
    Eigen::VectorXd delta_state = SolveLinearSystem(Amtx, Bvct);
    UpdateState(delta_state);
}

2.3 算法原理

• Schur补的作用：

  • 在SLAM和VINS中，优化问题通常包含大规模稀疏矩阵（包含状态和特征点两部分）。
  • Schur补通过消除特征点相关变量，将大规模稀疏优化问题转化为仅包含状态变量的小规模优化问题，显著降低计算复杂度。

• 投影残差与雅可比：

  • 残差基于三维点投影到图像平面的误差。
  • 雅可比描述了误差对状态和点位置变化的敏感性，用于梯度下降方向的计算。

---

2.4 算法框架总结

1. 初始化状态变量与矩阵。
2. 遍历特征点计算残差和雅可比。
3. 利用Schur补操作降低优化维度。
4. 构造状态信息矩阵并更新滑动窗口中的状态。

3. 代码实现

void SchurVINS::Solve3() {
    const int state_size = (int)states_map.size();
    if (state_size < 2) {
        LOG(INFO) << "only one window, bypass solve()";
        return;
    }

    const int min_frame_idx = states_map.begin()->second->frame_bundle->getBundleId();
    const int prev_frame_id0 = (++states_map.rbegin())->second->frame_bundle->frames_[0]->id(),
              prev_frame_id1 = (++states_map.rbegin())->second->frame_bundle->frames_[1]->id();
    const int max_frame_idx = states_map.rbegin()->second->frame_bundle->getBundleId();

    const int state_len = state_size * 6;
    const int64_t curr_state_id = curr_state->id;
    Eigen::MatrixXd Amtx = Eigen::MatrixXd::Zero(state_len, state_len);
    Eigen::VectorXd Bvct = Eigen::VectorXd::Zero(state_len);
    Matrix2o3d dr_dpc = Matrix2o3d::Zero();
    Matrix3o6d dpc_dpos = Matrix3o6d::Zero();
    Matrix2o6d jx = Matrix2o6d::Zero();
    Matrix2o3d jf = Matrix2o3d::Zero();
    Eigen::Vector2d r = Eigen::Vector2d::Zero();

    // compute local points jacobian
    int num_obs = 0;
    double total_error = 0.0;
    schur_pts_.clear();
    for (svo::LocalPointMap::iterator pit = local_pts.begin(); pit != local_pts.end(); ++pit) {
        const svo::PointPtr& curr_pt = pit->second;
        if (curr_pt->register_id_ != curr_state_id) {  // init point jacobian
            curr_pt->gv.setZero();
            curr_pt->V.setZero();
            curr_pt->W.setZero();
            curr_pt->register_id_ = curr_state_id;  // avoid init multi times
        }

        if (curr_pt->CheckStatus() == false || curr_pt->CheckLocalStatus() == false
            || curr_pt->CheckLocalStatus(prev_frame_id0, prev_frame_id1, max_frame_idx) == false) {
            // LOG(INFO) << "Solve2 pass invalid point: " << curr_pt->id()
            //           << " feature num : " << curr_pt->local_obs_.size() << " pos: " <<
            //           curr_pt->pos().transpose();
            continue;
        }

        schur_pts_.insert(curr_pt);
        const Eigen::Vector3d& Pw = curr_pt->pos();

        // compute frame local observation
        for (svo::LocalFeatureMap::iterator fit = curr_pt->local_obs_.begin(); fit != curr_pt->local_obs_.end();
             ++fit) {
            const svo::LocalFeaturePtr& feature = fit->second;
            const int curr_frame_idx = feature->frame->bundleId();

            if (curr_frame_idx < min_frame_idx
                || curr_frame_idx > max_frame_idx)  // pass observations on schurvins local sliding window
                continue;

            if (feature->state.expired())
                continue;

            const svo::AugStatePtr& ft_state = feature->state.lock();
            const int& state_idx = ft_state->index;
            const svo::Transformation& T_imu_cam = feature->camera_id == 0 ? T_imu_cam0 : T_imu_cam1;
            const Eigen::Matrix3d& R_i_c = T_imu_cam.getRotationMatrix();
            const Eigen::Quaterniond& q_i_c = T_imu_cam.getEigenQuaternion();
            const Eigen::Vector3d& t_i_c = T_imu_cam.getPosition();
            const Eigen::Matrix3d R_c_w = (ft_state->quat * q_i_c).toRotationMatrix().transpose();
            const Eigen::Vector3d Pi = ft_state->quat.inverse() * (Pw - ft_state->pos);
            const Eigen::Vector3d Pc = q_i_c.inverse() * (Pi - t_i_c);
            // const double level_scale = (1 << feature->level);
            const double level_scale = 1;

            // calc residual
            if (svo::LocalFeature::unit_sphere) {
                r = feature->tan_space * (feature->xyz - Pc.normalized()) * (focal_length / level_scale);
            } else {
                r = (feature->xyz.head<2>() - Pc.head<2>() / Pc.z()) * (focal_length / level_scale);
            }
            total_error += r.norm();
            // LOG(INFO) << "kf residual: " << r.transpose();

            // huber
            const double r_l2 = r.squaredNorm();
            double huber_scale = 1.0;
            if (r_l2 > huberB) {
                const double radius = sqrt(r_l2);
                double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
                huber_scale = sqrt(rho1);
                r *= huber_scale;
            }
            r *= obs_invdev;

            // calc jacobian
            dr_dpc.setZero();
            if (svo::LocalFeature::unit_sphere) {
                double Pc_norm = Pc.norm();
                double inv_norm = 1.0 / Pc_norm;
                double inv_norm3 = 1.0 / (Pc_norm * Pc_norm * Pc_norm);
                Eigen::Matrix3d norm_jacob = Eigen::Matrix3d::Identity() * inv_norm - Pc * Pc.transpose() * inv_norm3;
                dr_dpc = feature->tan_space * norm_jacob * (focal_length * obs_invdev * huber_scale / level_scale);
            } else {
                const double pc22 = Pc[2] * Pc[2];
                dr_dpc(0, 0) = 1 / Pc[2];
                dr_dpc(1, 1) = 1 / Pc[2];
                dr_dpc(0, 2) = -Pc[0] / pc22;
                dr_dpc(1, 2) = -Pc[1] / pc22;
                dr_dpc *= (focal_length * obs_invdev * huber_scale / level_scale);
            }

            ++num_obs;
            dpc_dpos.leftCols(3).noalias()
                = R_i_c.transpose() * Utility::SkewMatrix(Pi) * ft_state->quat.toRotationMatrix().transpose();
            dpc_dpos.rightCols(3) = -R_c_w;

            // dpc_dext.leftCols(3).noalias() = Utility::SkewMatrix(Pc) * R_i_c.transpose();
            // dpc_dext.rightCols(3) = -R_i_c.transpose();

            // jext.noalias() = dr_dpc * dpc_dext;
            jx.noalias() = dr_dpc * dpc_dpos;
            jf.noalias() = dr_dpc * R_c_w;
            // LOG(INFO) << "jext:\n" << jext;
            // LOG(INFO) << "jx:\n" << jx;
            // LOG(INFO) << "jf:\n" << jf;

            ////////////////////////////////////////////////////////////
            // Amtx.block(0, 0, 6, 6).noalias() += jext.transpose() * jext;  // ext 2 ext
            // Matrix6d blk_ext2pos = jext.transpose() * jx;
            // const int pos_bias = 6 + ft->state->index * 6;
            const int pos_bias = state_idx * 6;
            // Amtx.block(0, pos_bias, 6, 6) += blk_ext2pos;                           // ext 2 pos
            // Amtx.block(pos_bias, 0, 6, 6) += blk_ext2pos.transpose();               // pos 2 ext
            Amtx.block(pos_bias, pos_bias, 6, 6).noalias() += jx.transpose() * jx;  // pos 2 pos
            
            // Bvct.segment(0, 6).noalias() += jext.transpose() * r;       // ext grad
            Bvct.segment(pos_bias, 6).noalias() += jx.transpose() * r;  // pos grad

            out << "local_obs_: " << std::setprecision(15) << fit->first << " local_pts: " << pit->first << std::endl;
            out << "frame pose: " << std::setprecision(15) << ft_state->pos.transpose() << std::endl;
            out << "frame rot: "<< ft_state->quat.vec() << std::endl;
            out << "pit->second_pts: " << std::setprecision(15) << pit->second->pos().transpose() << std::endl;
            out << "JX_2: \n" << std::setprecision(15) << jx.transpose() * jx << std::endl;
            out << "JX_r: \n" << std::setprecision(15) << jx.transpose() * r << std::endl;
            out << "Amtx.block: \n" << std::setprecision(15) << Amtx.block(pos_bias, pos_bias, 6, 6) << std::endl;
            out << "Bvct: \n" << std::setprecision(15) << Bvct << std::endl;

            LOG(INFO) << "local_obs_: " << fit->first << "local_pts: " << pit->first;
            LOG(INFO) << "JX_2: \n" << std::setprecision(15) << jx.transpose() * jx;
            LOG(INFO) << "JX_r: \n" << std::setprecision(15) << jx.transpose() * r;

            curr_pt->V.noalias() += jf.transpose() * jf;  // pt 2 pt
            curr_pt->gv.noalias() += jf.transpose() * r;  // pt grad
            // curr_pt->W.noalias() += jext.transpose() * jf;  // ext 2 pt
            feature->W.noalias() = jx.transpose() * jf;  // pos 2 pt
                                                         // LOG(INFO) << "V:\n" << curr_pt->V;
                                                         // LOG(INFO) << "Vinv:\n" << curr_pt->V.inverse();
                                                         // LOG(INFO) << "gv:\n" << curr_pt->gv.transpose();
                                                         // LOG(INFO) << "W:\n" << ft->W;
        }
    }

    // combine point observation on same body
    for (const svo::PointPtr& pt : schur_pts_) {
        pt->state_factors.clear();
        for (svo::LocalFeatureMap::iterator iti = pt->local_obs_.begin(); iti != pt->local_obs_.end(); iti++) {
            if (iti->second->state.expired())
                continue;
            const svo::AugStatePtr& ft_state = iti->second->state.lock();
            svo::StateFactorMap::iterator iter = pt->state_factors.find(ft_state->index);
            if (iter == pt->state_factors.end()) {
                pt->state_factors.emplace(ft_state->index, svo::StateFactor(iti->second->W, ft_state));
            } else {
                iter->second.W += iti->second->W;
            }
        }
    }

    // schur completment
    for (const svo::PointPtr& pt : schur_pts_) {
        pt->Vinv = pt->V.inverse();
        // LOG(INFO) << "pt->V:\n" << pt->V;
        // LOG(INFO) << "pt->Vinv:\n" << pt->Vinv;
        // const Matrix6o3d eWVinv = pt->W * pt->Vinv;
        // LOG(INFO) << "eWVinv:\n" << eWVinv;
        // Amtx.block(0, 0, 6, 6) -= eWVinv * pt->W.transpose();  // ext 2 ext schur
        // Bvct.segment(0, 6) -= eWVinv * pt->gv;                 // ext grad schur
        for (svo::StateFactorMap::iterator iti = pt->state_factors.begin(); iti != pt->state_factors.end(); iti++) {
            if (iti->second.state.expired())
                continue;
            const svo::AugStatePtr& statei = iti->second.state.lock();  // int pi_bias = 6 + statei->index * 6;
            const int pi_bias = statei->index * 6;

            // const Matrix6d e2p_schur = eWVinv * fti->W.transpose();  // ext 2 pos schur
            // Amtx.block(0, pi_bias, 6, 6) -= e2p_schur;               //
            // Amtx.block(pi_bias, 0, 6, 6) -= e2p_schur.transpose();   //

            // LOG(INFO) << "e2p_schur:\n" << e2p_schur;
            // LOG(INFO) << "Amtx:\n" << Amtx;

            const Matrix6o3d WVinv = iti->second.W * pt->Vinv;
            for (svo::StateFactorMap::iterator itj = iti; itj != pt->state_factors.end(); itj++) {
                if (itj->second.state.expired())
                    continue;
                const svo::AugStatePtr& statej = itj->second.state.lock();
                // int pj_bias = 6 + statej->index * 6;
                const int pj_bias = statej->index * 6;

                Matrix6d p2p_schur = WVinv * itj->second.W.transpose();  // posi 2 posj schur
                Amtx.block(pi_bias, pj_bias, 6, 6) -= p2p_schur;
                if (pi_bias != pj_bias) {
                    Amtx.block(pj_bias, pi_bias, 6, 6) -= p2p_schur.transpose();
                }
                // LOG(INFO) << "Amtx:\n" << Amtx;
            }
            Bvct.segment(pi_bias, 6) -= WVinv * pt->gv;  // pos grad schur
            // LOG(INFO) << "Bvct:\n" << Bvct.transpose();
        }
    }

    LOG(INFO) << "Amtx:\n" << std::setprecision(15) << Amtx;
    LOG(INFO) << "Bvct:\n" << std::setprecision(15) << Bvct;
    StateUpdate(Amtx, Bvct);

    LOG(ERROR) << "total local pts: " << local_pts.size() << " schur pts: " << schur_pts_.size();
}