[MSCKF-VIO]零空间投影：消除特征位置不确定性

零空间投影：消除特征位置不确定性

文章简介

本文是MSCKF VIO系列文章的第九篇，详细介绍零空间投影的实现。零空间投影是MSCKF的核心技术之一，通过消除特征位置不确定性的影响，实现高效的状态更新。

目录

1. 零空间投影概述

1.1 问题背景

在MSCKF中，特征位置不是状态向量的一部分。当我们想要使用特征观测来更新状态时，需要消除特征位置不确定性的影响。

1.2 核心思想

对于一个特征在多个相机帧中的观测，测量方程为：

z = h ( x , p ) + n \mathbf{z} = h(\mathbf{x}, \mathbf{p}) + \mathbf{n}z=h(x,p)+n

其中：

• x \mathbf{x}x：系统状态
• p \mathbf{p}p：特征3D位置
• n \mathbf{n}n：测量噪声

线性化后：
δ z ≈ H x δ x + H p δ p + n \delta \mathbf{z} \approx H_x \delta \mathbf{x} + H_p \delta \mathbf{p} + \mathbf{n}δz≈Hx​δx+Hp​δp+n

我们的目标是只更新系统状态x \mathbf{x}x，而不估计特征位置p \mathbf{p}p。通过投影到H p H_pHp​的零空间，可以消除δ p \delta \mathbf{p}δp的影响。

1.3 数学原理

对H p H_pHp​进行SVD分解：
H p = U Σ V T H_p = U \Sigma V^THp​=UΣVT

其中U UU是左奇异向量矩阵。取U UU的后m − 3 m-3m−3列（对应零奇异值）构成零空间基A AA：

A = U : , 4 : m A = U_{:, 4:m}A=U:,4:m​

投影后的测量方程：
A T δ z = A T H x δ x + A T n A^T \delta \mathbf{z} = A^T H_x \delta \mathbf{x} + A^T \mathbf{n}ATδz=ATHx​δx+ATn

由于A T H p = 0 A^T H_p = 0ATHp​=0，特征位置的影响被消除。

2. 代码实现

2.1 零空间投影主函数

// src/msckf_vio.cpp:862-916voidMsckfVio::featureJacobian(constFeatureIDType&feature_id,conststd::vector<StateIDType>&cam_state_ids,MatrixXd&H_x,VectorXd&r){constauto&feature=map_server[feature_id];// 1. 找到有效的相机状态vector<StateIDType>valid_cam_state_ids(0);for(constauto&cam_id:cam_state_ids){if(feature.observations.find(cam_id)==feature.observations.end())continue;valid_cam_state_ids.push_back(cam_id);}// 2. 计算雅可比矩阵大小intjacobian_row_size=4*valid_cam_state_ids.size();// 3. 初始化雅可比矩阵MatrixXd H_xj=MatrixXd::Zero(jacobian_row_size,21+state_server.cam_states.size()*6);MatrixXd H_fj=MatrixXd::Zero(jacobian_row_size,3);VectorXd r_j=VectorXd::Zero(jacobian_row_size);intstack_cntr=0;// 4. 堆叠每个观测的雅可比for(constauto&cam_id:valid_cam_state_ids){Matrix<double,4,6>H_xi=Matrix<double,4,6>::Zero();Matrix<double,4,3>H_fi=Matrix<double,4,3>::Zero();Vector4d r_i=Vector4d::Zero();measurementJacobian(cam_id,feature.id,H_xi,H_fi,r_i);// 找到相机状态在状态向量中的位置autocam_state_iter=state_server.cam_states.find(cam_id);intcam_state_cntr=std::distance(state_server.cam_states.begin(),cam_state_iter);// 填入对应的列H_xj.block<4,6>(stack_cntr,21+6*cam_state_cntr)=H_xi;H_fj.block<4,3>(stack_cntr,0)=H_fi;r_j.segment<4>(stack_cntr)=r_i;stack_cntr+=4;}// 5. 零空间投影JacobiSVD<MatrixXd>svd_helper(H_fj,ComputeFullU|ComputeThinV);MatrixXd A=svd_helper.matrixU().rightCols(jacobian_row_size-3);H_x=A.transpose()*H_xj;r=A.transpose()*r_j;}

2.2 SVD分解

// 对H_fj进行SVD分解JacobiSVD<MatrixXd>svd_helper(H_fj,ComputeFullU|ComputeThinV);// 获取左奇异向量矩阵UMatrixXd U=svd_helper.matrixU();// 取后(m-3)列作为零空间基MatrixXd A=U.rightCols(jacobian_row_size-3);