第6期 曾毓菁，等：一种融入注意力和预测的特征选择SLAM算法 ·1041· 性。设k+1为获得一个新选择特征集的时刻，则 视投影模型，并且可以进行有效的计算。为此， x中包含前一帧的优化位姿，这是优化后端的最新 必须将像素测量表示为想要估计的未知状态的线 位姿估计（见图2），注意到在前一帧k和当前帧 性函数。假设a是h时刻第I特征在相机坐标 k+1之间将进行特征选择。用k+H[x+1…x+H] 系下的像素观测，它满足： 表示在H时间段上的状态向量，其中+k+H是待 uux(R")'(p"-"n》=0 (3) 优化的预测的状态向量。此外，设Pk+H为与 式中：p”是特征点1在世界坐标系下的位置； x4+H关联的估计误差的协方差矩阵，它的逆矩阵 (Rh,t"h)是相机在h时刻的位姿。该模型的物 称为信息矩阵，表示为P+HP+H。有了这些定 理意义是在相机坐标系下的像素观测方向和测量 义后，logdet度量可以写成 方向共线，如图3所示，其中叉乘度量了其共线性 faet(S)=logdet((S))=logde . (2) 的偏差。在以k+H状态函数的状态空间形式下 形成一个线性系统，把这个问题当作一个最大似 式中：4，是第I个特征的信息矩阵；p是第I个特 然估计问题来解决，这样就能从特征点提供的 征被跟踪到的概率，其可以通过对特征检测得分 k+H中提取出H时间段的信息矩阵Po。 进行归一化得到？，保证信息较多的特征更容易 保留，这是一种期望的行为。 像素坐标系 相机坐标系 共线 状态向量xa 0 k+H 20 Hz 未来时间轴 X“世界坐标系 图2时间轴 图3向量共线 Fig.2 Time line Fig.3 Vector collinear 1.1贪婪算法 将时间段内每个时刻的状态定义为x= 本文使用带有延迟求值的贪婪算法图选择 [pt"(机器人的预测姿态)，其中R=exp(p), k个特征（近似的）最大化logdet度量。作为输入， R=exp(),R"mh=exp(p)exp(),为了之后公式 需要一定数量的特征F供系统选择，每个新特征 点的预测信息矩阵{4eF,以及未来H时间段的 的表达方便，将Rh、h表示为R和t,然后用 这些量参数化线性系统。将式(3)的左边表示为 信息矩阵2k+H。为了保持对特征跟踪的一致性， f(0,),并用全微分公式将其线性化： 需要对之前确定的有用的特征进行跟踪。当在未 来帧中检测到这些特征时，它们将自动传递给优 fe.0=fe.0+eg-9+ 亦kant-) (4) d 化后端。 算法1带有延迟求值的贪婪算法 lon=-Uu. 1)输入2k+H,d,1=1,2…,N和K,令S#=; Uu(Rpr。 2)for循环i=1,2,…,k; 第1特征在时间段H内的贡献为4，可以表 3)计算fS#U{)上界，[U1,U2,…,Uw]=(2k+H 示为 A=AA (5) 41,42,…,4w 4)根据上界值对特征点进行降序排序，F= Uu(RiP) -U sort(U,U2,…,Uw): A= (6) 5)初始化，fmx=-1,lnx=-1; U+m(R+HP) -U+HY 6)for循环1eF; 最后将其用在贪婪算法中。 7)如果U,<fma,则跳出循环； 2实验分析 8)如果f(S#U0>fm,则fs=fS#U0,Imax=l: 9)S=SUImaxo 为了测试本文的方法，使用EuRoC数据集四 1.2视觉特征信息矩阵 该数据集是使用一个配备了视觉惯性)传感器 本文使用线性侧量模型，它简化了非线性透 的AscTech Firefly六旋翼飞行器采集的，其包含k+1 xk bxk:k+H ∆ = [ xkbxk+1 ···bxk+H ] bxk+1:k+H Pk:k+H bxk:k+H Ωk:k+H ∆ =P −1 k:k+H 性。设 为获得一个新选择特征集的时刻，则 中包含前一帧的优化位姿，这是优化后端的最新 位姿估计 (见图 2)，注意到在前一帧 k 和当前帧 k + 1 之间将进行特征选择。用 表示在 H 时间段上的状态向量，其中 是待 优化的预测的状态向量。此外，设 为与 关联的估计误差的协方差矩阵，它的逆矩阵 称为信息矩阵，表示为 。有了这些定 义后，logdet 度量可以写成 fdet(S ) = log det(Ωk:k+H(S)) = log det   ∑ l∈S pl∆l   (2) ∆l l pl 式中： 是第 个特征的信息矩阵； 是第 l 个特 征被跟踪到的概率，其可以通过对特征检测得分 进行归一化得到[8] ，保证信息较多的特征更容易 保留，这是一种期望的行为。 k−s+1 k k+1 ... k+H 20 Hz 未来时间轴 状态向量 xk: k+H 图 2 时间轴 Fig. 2 Time line 1.1 贪婪算法 κ F {∆l}l∈F Ω¯ k:k+H 本文使用带有延迟求值[17] 的贪婪算法[18] 选择 个特征 (近似的) 最大化 logdet 度量。作为输入， 需要一定数量的特征 供系统选择，每个新特征 点的预测信息矩阵 ，以及未来 H 时间段的 信息矩阵 。为了保持对特征跟踪的一致性， 需要对之前确定的有用的特征进行跟踪。当在未 来帧中检测到这些特征时，它们将自动传递给优 化后端。 算法 1 带有延迟求值的贪婪算法 Ω¯ k:k+H ∆l l = 1,2,··· ,N κ S # 1) 输入 ， ， 和 ，令 = ϕ ； 2) for 循环 i = 1,2,··· , κ ； f(S # ∪{l}) [U1,U2,··· ,UN] = ( Ω¯ k:k+H, ∆1,∆2,··· ,∆N) 3) 计算 上界， ； F ↓ = sort(U1,U2,··· ,UN) 4) 根据上界值对特征点进行降序排序， ； 5) 初始化， fmax = −1，lmax = −1 ； l ∈ F ↓ 6) for 循环 ； 7) 如果 Ul < fmax，则跳出循环； f(S # ∪l) > fmax fmax = f(S # 8) 如果 ，则 ∪l)，lmax = l ； S # = S # 9) ∪lmax。 1.2 视觉特征信息矩阵 本文使用线性测量模型，它简化了非线性透 uhl h l 视投影模型，并且可以进行有效的计算。为此， 必须将像素测量表示为想要估计的未知状态的线 性函数。假设 是 时刻第 特征在相机坐标 系下的像素观测，它满足： uhl ×( ( R W cam,h )T (p W l − t W cam,h )) = 03 (3) p W l l R W cam,h t W cam,h h xˆ k:k+H xˆ k:k+H 式中： 是特征点 在世界坐标系下的位置； ( , ) 是相机在 时刻的位姿。该模型的物 理意义是在相机坐标系下的像素观测方向和测量 方向共线，如图 3 所示，其中叉乘度量了其共线性 的偏差。在以 状态函数的状态空间形式下 形成一个线性系统，把这个问题当作一个最大似 然估计问题[19] 来解决，这样就能从特征点提供的 中提取出 H 时间段的信息矩阵[20]。 相机坐标系 世界坐标系 像素坐标系 共线 x X Z y z uhl p W l Y O O 图 3 向量共线 Fig. 3 Vector collinear xh = [φt W cam,h ] ∆R = exp(φ ∧ ) bR = exp(ϕ ∧ ) R W cam,h = exp(φ ∧ ) exp(ϕ ∧ ) R W cam,h t W cam,h R t f(φ, t) 将时间段内每个时刻的状态定义为 (机器人的预测姿态)，其中 ， ， ，为了之后公式 的表达方便，将 、 表示为 和 ，然后用 这些量参数化线性系统。将式 (3) 的左边表示为 ，并用全微分公式[21] 将其线性化： f(φ, t) = f( ˆφ, tˆ)+ ∂ f ∂φ| ( ˆφ,tˆ)(φ−φˆ)+ ∂ f ∂t | ( ˆφ,tˆ)(t − tˆ) (4) ∂ f ∂t |( ˆφ,tˆ) = ∂ f ∂t |(0,tˆ) = −Ukl ∂ f ∂φ|( ˆφ,tˆ) = ∂ f ∂φ|(0,tˆ) = Ukl(R pˆ ) ∧ 式中： ， 。 第 l 特征在时间段 H 内的贡献为 ∆l 可以表 示为 ∆l = A TA (5) A =   Ukl(Rˆ k pl) ∧ ··· −Ukl . . . . . . U(k+H)l(Rˆ k+H pl) ∧ ··· −U(k+H)l   (6) 最后将其用在贪婪算法中。 2 实验分析 为了测试本文的方法，使用 EuRoC 数据集[22] ， 该数据集是使用一个配备了视觉惯性[23] 传感器 的 AscTech Firefly 六旋翼飞行器采集的，其包含 第 6 期 曾毓菁，等：一种融入注意力和预测的特征选择 SLAM 算法 ·1041·