位姿估计 | 点特征空间目标位姿估计及其Python代码

引言

本文接着分享空间目标位姿跟踪和滤波算法中用到的一些常用内容，希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章：位姿跟踪 | 相关内容目录和链接总结（不断更新中~~~）

本文介绍：基于点特征的位姿估计方法流程，并附带部分Python代码。

位姿估计方法流程

1. SUFT点特征检测介绍

SUFT特征匹配主要包括2个阶段：
第一阶段：SUFT特征的生成，即从多幅图像中提取对尺度缩放、旋转、亮度变化无关的特征向量。
第二阶段：SUFT特征向量的匹配。

1） 相关介绍

特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。

关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。

描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。
描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量空间上的距离相近,就可以认为它们是同样的特征点。

常见的点特征检测方法包括：SIFT，SUFT，FAST，ORB等。

• SIFT（尺度不变特征变换,Scale-Invariant FeatureTransform）：充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化，计算量较大，耗时较长。

• SUFT（Speeded Up Robust Feature）：SIFT算法的加速版，采用了Haar特征以及积分图像的概念，大大加快了程序的运行效率。善于处理具有模糊和旋转的图像，但是不善于处理视角变化和光照变化。

• FAST：顾名思义，适当降低精度和鲁棒性，提升计算速度，属于计算特别快的一种特征点(没有描述子)。

• ORB（Oriented FASTand Rotated BRIEF）：改进了 FAST 检测子不具有方向性的问题，采用速度极快的二进制描述子BRIEF，使整个图像特征提取的环节大大加速。具有旋转、尺度不变性的同时，速度方面提升明显。

2）Python代码

这里展示了两种情况：1. 只进行SUFT特征检测，不进行匹配；2. 在SUFT特征检测的基础上进行特征匹配，并展示匹配结果。

a. SUFT特征检测

import cv2 
import numpy as np 

img_in= cv2.imread('***.jpg')

# 参数为hessian矩阵的阈值
surf = cv2.xfeatures2d.SURF_create()

# 找到关键点和描述符
key, desc_query = surf.detectAndCompute(img_in, None)

# 把特征点标记到图片上
img=cv2.drawKeypoints(img_in, key, img_out)

cv2.imshow('img_result', img_out)
cv2.waitKey(0)

b. SUFT特征匹配

import cv2
import numpy as np
 
def drawMatchesKnn(img1_gray,kp1,img2_gray,kp2,goodMatch):
    h1, w1 = img1_gray.shape[:2]
    h2, w2 = img2_gray.shape[:2]
 
    vis = np.zeros((max(h1, h2), w1 + w2, 3), np.uint8)
    vis[:h1, :w1] = img1_gray
    vis[:h2, w1:w1 + w2] = img2_gray
 
    p1 = [kpp.queryIdx for kpp in goodMatch]
    p2 = [kpp.trainIdx for kpp in goodMatch]
 
    post1 = np.int32([kp1[pp].pt for pp in p1])
    post2 = np.int32([kp2[pp].pt for pp in p2]) + (w1, 0)
 
    for (x1, y1), (x2, y2) in zip(post1, post2):
        cv2.line(vis, (x1, y1), (x2, y2), (0,0,255))
 
    cv2.namedWindow("match",cv2.WINDOW_NORMAL)
    cv2.imshow("match", vis)
 
 # 载入图片
img1_gray = cv2.imread("1.jpg")
img2_gray = cv2.imread("2.jpg")

# 特征检测（第一节内容）
surf = cv2.xfeatures2d.SURF_create()
kp1, des1 = surf.detectAndCompute(img1_gray, None)
kp2, des2 = surf.detectAndCompute(img2_gray, None)
 
# 暴力匹配（帧间特征匹配）
bf = cv2.BFMatcher(cv2.NORM_L2)
matches = bf.knnMatch(des1, des2, k = 2)

# 筛选匹配较好的点
goodMatch = []
for m,n in matches:
    if m.distance < 0.4*n.distance:
        goodMatch.append(m)

# 画出匹配图示
drawMatchesKnn(img1_gray,kp1,img2_gray,kp2,goodMatch[:20])
cv2.waitKey(0)
cv2.destroyAllWindows()

3. ICP问题线性计算R|T

在特征匹配的基础上，对目标特征点进行跟踪，输出特征点的序列坐标值。
设特征点数量为$N$，对时间采样序列长度为$K$，$P_i^j,i=1,\dots ,N,j=1,\dots ,K$，表示第$i$个特征点在地$j$时刻的特征位置。

接着将基于点特征的位姿估计问题，转换为ICP问题，利用ICP线性求解目标体坐标系相对于相机坐标系的旋转矩阵$R$和平移矩阵$T$.

具体内容及代码参见历史博文：
位姿估计 | 目标体坐标系相对于相机坐标系的位姿估计及其Python代码

4. OI算法优化计算R|T

利用非线性优化方法提高位姿估计解的精度，构建共线性误差最小化函数$\mathbf{e}$，利用正交迭代算法优化位姿。

具体内容及代码参见历史博文：
位姿测量 | 正交迭代（OI）算法流程及其Python代码

5. KF算法估计下一帧R|T

利用第1-4的步骤可以得到单幅图像目标体坐标系相对于相机坐标系的位姿。
针对视频或者序列图像的位姿估计，可以结合$t$时刻的位姿估计解$R|T(t)$和$t+1$时刻的位姿估计解$R|T(t+1)$，利用卡尔曼滤波优化$t+1$时刻的位姿估计值。

同时，考虑到空间目标（航天器）具有一定的运动规律，可以对空间目标运动进行建模，设计基于卡尔曼滤波的位姿估计方法，进一步优化位姿解。

卡尔曼滤波具体内容及代码参见历史博文：
滤波算法 | 无迹卡尔曼滤波（UKF）算法及其Python实现

总结

总的来说，基于特征的空间目标位姿估计整体流程为：

• 首先进行特征检测，比如点特征检测、直线特征检测等；
• 接着进行帧间特征点匹配，对特征点进行跟踪，持续输出特征点的位置；
• 针对单幅图像中的特征点，利用点在相机坐标系和目标本体系的位置，计算目标本体系相对于相机坐标系的位姿；
• 利用非线性方法对位姿进行优化，输出最终的单幅图像位姿解；
• 考虑多幅图像特征点之间的关系，利用卡尔曼滤波方法优化下一帧位姿解。

以上为简单内容和流程介绍，整体代码内容过多，因此只摘抄了部分代码公开，可能会有些许问题，有需要的可以自行理解和修改。