我试图通过从图像中提取特征并计算相机姿势来重建相机的轨迹，但是轨迹并不像我预期的那样。对于我的视觉里程计系统，我使用的是英特尔相机 D435i，它可以提取颜色和深度帧。我的代码是用 python 编写的，使用的所有重要函数都来自 OpenCV 库。
我正在使用一个数据集，其中相机只是简单地移动，因此我希望旋转是单位矩阵并且平移向前。但是，这不是我在计算它们时得到的原因。
这是我的方法：
- 读取彩色帧，将它们转换为灰度并应用 CLAHE 方法来减少光对比
- 使用 SIFT 从第 t 帧和第 t+1 帧中提取特征
- 使用 FLANN 匹配它们并应用 Lowe 比率检验过滤异常值
- 使用相机固有矩阵 (K) 和像素深度 (s)（从深度帧中获取）将特征的 2D 像素坐标 (u1p, v1p) 转换为 3D 世界坐标：
归一化：x_y_norm = (K^-1) * [u1p ,v1p, 1] 缩放：x_y_norm_s *= x_y_norm
之后我尝试了两种方法：PNP和基本矩阵
A) PNP： 4. 使用函数cv2.solvePNPRansac()得到旋转向量和平移向量。函数的输入是帧 t（对象点）的归一化和缩放的关键点和帧 t+1（图像点）的归一化关键点。然后使用函数cv2.Rodrigues()计算旋转矩阵
B) 基本矩阵： 4. 使用cv2.findEssentialMat()并将关键点的标准化 xy 坐标作为输入。通过检查奇异值来检查基本矩阵是否计算正确。使用cv2.recoverPose()来获得旋转和平移（这里我也使用了标准化的 xy 坐标作为输入）。
- 计算轨迹如下（假设第二个 camera_pose 是 [R|t] 并且第一个位姿是单位矩阵，如书中多视图几何中所述）：
camera_pose[t+1] = pose[t] * pose[t+1]^-1 （通过将先前和当前位姿相乘来从初始位置获取位姿，如将相对相机旋转和平移与已知位姿相结合中所述）
position = pose[t+1] * [0,0,0,1]（从原点计算的位置）
轨迹[:,t+1] = 位置[0:3]
我希望有人可以帮助我，因为我不知道我的错误在哪里，而且 PnP 方法和基本矩阵方法都不起作用。
编辑：对于这两种方法，我添加了从数据集的前 15 帧中获得的轨迹图片（在 ZX 和 XY 平面中）以及包含旋转矩阵和某些帧的平移向量的文本文件的提取. ZX-Plane 应显示在正方向上形成一条线的点，因为我的数据集仅显示相机直接移动的图像，但显示的是随机轨迹。绿点是旧相机姿势，红点是当前姿势。我还添加了一个在第一帧中提取的特征并与第二帧匹配的示例，它们看起来也不错。
