根据相应的 3d 点集计算相机运动

Question

我有一个小问题。我编写了一个程序，它使用相机和深度信息在每一帧中提取一组三维点。这些点在相机坐标系中，即原点在相机中心，x 是水平距离，y 是垂直距离，z 是到相机的距离（沿光轴）。一切都以米为单位。 IE。点 (2,-1,5) 将位于右侧 2 米，下方 1 米，沿相机光轴 5 米。

我在每个时间范围内计算这些点并且也知道对应关系，就像我知道t-1 中的哪个点属于t 中的哪个3d 点一样。

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

---

这是一帧的示例数据集，其中包含相机坐标中点的当前（左）和前一个 3D 位置（右）：

-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 参数使计算更准确，但很多时候转换矩阵显示出相当大的平移运动。正如您在示例数据中看到的那样，移动通常很小。

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

此外，如果我在每个时间范围内都有相机的相对运动，我将如何将其转换为世界坐标？另外，我将如何获得从点 (0,0,0) 开始的绝对位置，以便我拥有每个时间帧的相机位置（和方向）？

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

谢谢！

更新 1：

在@Michael Kupchick 很好的回答之后，我尝试检查OpenCV 中的estimateAffine3D() 函数的工作情况。所以我创建了两个包含 6 个点对的小测试集，它们只有一个平移，而不是一个旋转，并查看了生成的转换矩阵：

测试集 1：

1.5 2.1 6.7 | 0.5 1.1 5.7
6.7 4.5 12.4 | 5.7 3.5 11.4
3.5 3.2 1.2 | 2.5 2.2 0.2
-10.2 5.5 5.5 | -11.2 4.5 4.5
-7.2 -2.2 6.5 | -8.2 -3.2 5.5
-2.2 -7.3 19.2 | -3.2 -8.3 18.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

测试集 2：

1.5 2.1 0 | 0.5 1.1 0
6.7 4.5 0 | 5.7 3.5 0
3.5 3.2 0 | 2.5 2.2 0
-10.2 5.5 0 | -11.2 4.5 0
-7.2 -2.2 0 | -8.2 -3.2 0
-2.2 -7.3 0 | -3.2 -8.3 0

变换矩阵：

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

--> 这给了我两个第一眼看上去就对的变换矩阵...

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

任何人有任何提示或想法为什么它那么糟糕？

Answer 1

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

您要做的是注册已知的 3d，因为对于所有帧都有相同的 3d 点->相机关系从注册计算的转换将是相机运动转换。

为了解决这个问题，您可以使用 PCL。它是 opencv 用于 3d 相关任务的姊妹项目。 http://www.pointclouds.org/documentation/tutorials/template_alignment.php#template-alignment 这是一个关于点云对齐的好教程。

基本上是这样的：

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

http://docs.pointclouds.org/trunk/classpcl_1_1registration_1_1_transformation_estimation_s_v_d.html

你应该至少有3个对应点。

您可以按照教程进行操作或实现自己的 ransac 算法。 这只会为您提供一些粗略的变换估计（如果噪声不太大，可能会非常好），以便获得准确的变换，您应该使用在上一步计算的猜测变换应用 ICP 算法。 ICP在这里描述：

http://www.pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point

这两个步骤应该可以让您准确估计帧之间的转换。

因此，您应该逐步进行成对注册 - 注册第一对帧会得到从第一帧到第二帧 1->2 的转换。将第二个与第三个 (2->3) 注册，然后将 1->2 转换附加到 2->3 等等。这样，您将获得以第一帧为原点的全局坐标系中的变换。