Opencv 从相应的三维点集计算摄影机运动

我有点小问题。我编写了一个程序，使用相机和深度信息在每一帧中提取一组三维点。这些点位于摄像机坐标系中，这意味着原点位于摄像机中心，x为水平距离，y为垂直距离，z为与摄像机的距离（沿光轴）。一切都以米为单位。也就是说，点（2，-1,5）右侧两米，下方一米，沿摄像机光轴方向五米

我计算每个时间帧中的这些点，并知道对应关系，就像我知道

t-1

中的哪个点属于

t

中的哪个3d点一样

我现在的目标是在我的世界坐标系中计算相机在每个时间帧中的运动（z向上表示高度）。我想计算相对运动，但也要计算从某个起始位置开始的绝对运动，以可视化相机的轨迹

---

这是一个示例数据集，其中包含摄影机坐标中点的当前（左）和先前三维位置（右）：

-0.174004 0.242901 3.672510 | -0.089167 0.246231 3.646694 
-0.265066 -0.079420 3.668801 | -0.182261 -0.075341 3.634996 
0.092708 0.459499 3.673029 | 0.179553 0.459284 3.636645 
0.593070 0.056592 3.542869 | 0.675082 0.051625 3.509424 
0.676054 0.517077 3.585216 | 0.763378 0.511976 3.555986 
0.555625 -0.350790 3.496224 | 0.633524 -0.354710 3.465260 
1.189281 0.953641 3.556284 | 1.274754 0.938846 3.504309 
0.489797 -0.933973 3.435228 | 0.561585 -0.935864 3.404614 

---

由于我希望尽可能使用OpenCV，我在OpenCV 2.3中找到了

estimateAffine3D（）

函数，该函数获取两个3D点输入向量，并使用RANSAC计算它们之间的仿射变换

作为输出，我得到一个3x4变换矩阵

我已经尝试通过设置RANSAC参数使计算更加精确，但是很多时候trnasformation矩阵显示了相当大的平移运动。正如您在示例数据中看到的，移动通常非常小

所以我想问是否有人对我可以尝试什么有其他想法？OpenCV是否为此提供了其他解决方案

另外，如果我有相机在每个时间段的相对运动，我将如何将其转换为世界坐标？还有，我怎样才能从一个点（0,0,0）开始得到绝对位置，从而得到每个时间帧的相机位置（和方向）

如果有人能给我一些建议，那就太好了

谢谢大家!

更新1:

在@Michael Kupchick给出了一个很好的答案之后，我试图检查OpenCV中的estimateAffine3D（）函数的工作情况。因此，我创建了两个6点对的小测试集，它们只有一个平移，而不是旋转，并查看了生成的变换矩阵：

测试集1:

变换矩阵：

1           -1.0573e-16  -6.4096e-17  1
-1.3633e-16 1            2.59504e-16  1
3.20342e-09 1.14395e-09  1            1

1             4.4442e-17  0   1
-2.69695e-17  1           0   1
0             0           0   0

测试集2:

变换矩阵：

1           -1.0573e-16  -6.4096e-17  1
-1.3633e-16 1            2.59504e-16  1
3.20342e-09 1.14395e-09  1            1

1             4.4442e-17  0   1
-2.69695e-17  1           0   1
0             0           0   0

-->这给了我两个转换矩阵，乍一看是正确的

假设这是正确的，当我在每个时间步中都有这个转换矩阵时，我将如何重新计算它的轨迹

---

有人知道为什么会这么糟糕吗？

这个问题比图像处理更与3d相关

您要做的是注册已知的3d，因为所有帧都有相同的3d点->摄影机关系，因此根据注册计算的变换将是摄影机运动变换

为了解决这个问题，您可以使用PCL。它是opencv的姊妹项目，用于3d相关任务。 这是一个关于点云路线的好教程

基本上是这样的：

对于每对连续帧，3d点对应是已知的，因此可以使用中实现的SVD方法

您应该至少有3个对应点

您可以按照教程进行操作，也可以实现自己的ransac算法。 这只会给您一些粗略的变换估计（如果噪声不是太大，可能会很好），为了获得准确的变换，您应该使用在上一步计算的guess变换应用ICP算法。 ICP的描述如下：

这两个步骤将为您提供帧间转换的准确估计

---

因此，您应该以增量方式进行两两注册—注册第一对帧可以获得从第一帧到第二帧的转换1->2。用第三个（2->3）注册第二个，然后将1->2转换附加到2->3，依此类推。通过这种方式，您将获得以第一帧为原点的全局坐标系中的变换。

谢谢您的回答！我将检查PCL库是如何完成的。但在我看来，ICP算法和我在OpenCV中使用的算法有什么不同？我的意思是肯定两者的工作方式不同，但它们不都返回帧之间的相对旋转和平移吗？本教程给了我一个4x4矩阵T。我用openCV函数得到的数据也尝试过它。我还创建了一个4x4矩阵P，它是方向和位置。我试图通过：P_t=P_t-1*t_t来计算轨迹，不幸的是，这给了我不可能的结果。有什么想法吗？试着检查一下estimateAffine3D函数。生成一组3D点，然后生成具有已知平移（无旋转）的第二个点。尝试注册这些集合，看看结果是否合理。谢谢！我在上面的帖子中添加了一个更新，显示了estimateAffine3D（）的两个简单测试用例。。。它似乎适用于一个测试用例，但不适用于另一个。。。所以我想我应该试着用PCL库来代替。。。或者你还有其他想法吗？你得到的变换矩阵的第一行都是零，这不是很奇怪吗？我的意思是旋转矩阵的对角线应该都是1或一些接近的值，你确定你正确地读取了矩阵吗？哦，对了！我读对了变换矩阵，但其中一个输入向量有错误。所以我把变换矩阵改成现在的样子，这对旋转和平移更有意义。这不是我想要的格式吗？ICP算法不会给我ab吗