opencv中的三维姿态估计

Question

我有一个物体，上面有一些传感器，其中一个已知的三维位置相对于彼此固定。我们将此对象称为＆＃34;检测器＆＃34;。 我有一些这样的传感器＆＃39;检测到3D世界空间中的位置。 问题：我如何得到＆＃34;探测器的估计姿势（位置和旋转）＆＃34;在3D世界空间。

我试着研究npn问题，flann和orb匹配并知道异常值，但看起来他们都期望某种类型的摄像头位置。我与相机无关，我想要的只是＆＃34;探测器＆＃34;的姿势。考虑到opencv是一个＆＃34;愿景＆＃34;库，我甚至需要opencv吗？

编辑：并非所有传感器都可能是检测器。这里用浅绿色点表示。

Answer 1

你绝对不需要openCV来估计你的物体在太空中的位置。

这是一个简单的优化问题，您需要最小化与模型的距离。

首先，你需要创建一个物体在太空中的态度模型。

def Detector([x, y, z], [alpha, beta, gamma]):

应返回点的所有位置的列表或数组，其中包含3D空间中的ID。 您甚至可以为每个传感器点创建一个类，并为整个对象创建一个类，该类具有与对象相同的多个传感器属性。

然后，您需要构建一个优化算法，使您的模型适合检测到的数据。 算法应该使用态度 x，y，z，alpha，beta，gamma作为变量。

对于目标函数，您可以使用类似于相应ID的距离总和。

假设您有一个3点对象，您想要适合3个数据点

#Model
m1 = [x1, y1, z1]
m2 = [x2, y2, z2]
m3 = [x3, y3, z3]

#Data
p1 = [xp1, yp1, zp1]
p2 = [xp2, yp2, zp2]
p3 = [xp3, yp3, zp3]

import numpy as np
def distanceL2(pt1, pt2):
    distance = np.sqrt((pt1[0]-pt2[0])**2 + (pt1[1]-pt2[1])**2 + (pt1[2]-pt2[2])**2))

# You already know you want to relate "1"s, "2"s and "3"s
obj_function = distance(m1, p1) + distance(m2,p2) + distance(m3,p3)

现在您需要深入了解优化库以找到最佳算法，具体取决于您需要优化的速度。 由于你的空间点几乎是连通的，所以这不应该太困难。 scipy.optimize可以做到。

要减少问题的大小，请尝试选择“检测到”问题之一。作为参考的点（好像这个度量是可信的），然后找到该位置的obj_function的最小值（只有3个参数可以优化，对应于方向） 然后迭代你拥有的每个点。 一旦达到最佳状态，您可以尝试在其周围寻找更好的位置，并查看是否再次减小距离。

Answer 2

对不起，这有点旧，但永远不会太晚进行对象跟踪：）

OpenCV SolvePNP RANSAC应该适合你。您无需提供初始姿势，只需为rvec和tvec制作空垫即可保存结果。

另外，由于没有相机，只需使用Identity作为相机失真参数。

首次使用空rvec和tvec调用PNP时，请务必设置useExtrinsicGuess = false。保存结果（如果正常），将其用于useExtrinsicGuess = true的下一帧，以便更快地优化功能。 https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnpransac