相机姿势估计给出错误的结果
Camera pose estimation giving wrong results
我正在尝试根据两个不同图像中的匹配点来估计相对相机运动。很像这里描述的：
但是估计的平移和旋转没有意义。
我使用合成输入来确保所有点都有效且位置完美。
10 x 10 x 10 点均匀分布在一个立方体中。
（用蓝色正面、红色背面、较浅的顶部和较暗的底部绘制的立方体）
zeroProjection
相机在立方体前面，指向正面。
rotate90projection
立方体左侧的相机指向左侧面。
我绘制了两个投影。您可以轻松地通过视觉验证相机是否已平移 90 度并在两个投影之间的 x-z 平面中沿对角线移动。
在代码中，旋转（以度为单位）给出为 (0, -90, 0)
平移为(0.7071, 0, 0.7071)，相机移动距离正好为1。
然后我在 2d 点集上执行 findEssentialMat() 和 recoverPose() 以获得平移和旋转估计。
我希望看到与生成图像相同的平移和旋转，但估计完全错误：
rotation estimate: (-74.86565284711004, -48.52201867665918, 121.26023708879158)
translation estimate: [[0.96576997]
[0.17203598]
[0.19414426]]
如何恢复实际的 (0, -90, 0), (0.7071, 0, 0,7071) 转换？
显示两个立方体图像并打印出估计值的完整代码：
import cv2
import numpy as np
import math
def cameraMatrix(f, w, h):
return np.array([
[f, 0, w/2],
[0, f, h/2],
[0, 0, 1]])
n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)
def cube(x=0, y=0, z=0, radius=1):
c = np.zeros((n * n * n, 3), dtype=np.float32)
for i in range(0, n):
for j in range(0, n):
for k in range(0, n):
index = i + j * n + k * n * n
c[index] = [i, j, k]
c = 2 * c / (n - 1) - 1
c *= radius
c += [x, y, z]
return c
def project3dTo2dArray(points3d, K, rotation, translation):
imagePoints, _ = cv2.projectPoints(points3d,
rotation,
translation,
K,
np.array([]))
p2d = imagePoints.reshape((imagePoints.shape[0],2))
return p2d
def estimate_pose(projectionA, projectionB):
E, _ = cv2.findEssentialMat(projectionA, projectionB, focal = f)
_, r, t, _ = cv2.recoverPose(E, projectionA, projectionB)
angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
print('rotation estimate:', angles)
print('translation estimate:', t)
def main():
c = cube(0, 0, math.sqrt(.5), 0.1)
rotation = np.array([[0], [0], [0]], dtype=np.float32)
translation = np.array([[0], [0], [0]], dtype=np.float32)
zeroProjection = project3dTo2dArray(c, K, rotation, translation)
displayCube(w, h, zeroProjection)
rotation = np.array([[0], [-90], [0]], dtype=np.float32)
translation = np.array([[math.sqrt(.5)], [0], [math.sqrt(.5)]], dtype=np.float32)
print('applying rotation: ', rotation)
print('applying translation: ', translation)
rotate90projection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, rotate90projection)
estimate_pose(zeroProjection, rotate90projection)
def displayCube(w, h, points):
img = np.zeros((h, w, 3), dtype=np.uint8)
plotCube(img, points)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
if k == ord('q'):
exit(0)
def plotCube(img, points):
# Red back face
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n-1]), (0, 0, 255), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (0, 0, 200), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)
# gray connectors
cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)
# Blue front face
cv2.line(img, tuple(points[0]), tuple(points[n-1]), (255, 0, 0), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
cv2.line(img, tuple(points[0]), tuple(points[n*(n-1)]), (200, 0, 0), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)
main()
原来是我的代码中的一些小错误（比如错误的主要观点）。
下面的工作代码显示了 3 张图片。
首先是一个立方体显示在镜头前。
其次是相同的立方体，但不同的投影。相机已移动 1 个单位并绕所有 3 个轴旋转。
相机平移和旋转是根据两个投影估计的。
第三个显示使用旋转和平移估计投影的立方体。
该代码有效，因为第二张和第三张图片相似。
import cv2
import numpy as np
import math
def cameraMatrix(f, w, h):
return np.array([
[f, 0, w/2],
[0, f, h/2],
[0, 0, 1]])
n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)
def cube(x=0, y=0, z=0, radius=1):
c = np.zeros((n * n * n, 3), dtype=np.float32)
for i in range(0, n):
for j in range(0, n):
for k in range(0, n):
index = i + j * n + k * n * n
c[index] = [i, j, k]
c = 2 * c / (n - 1) - 1
c *= radius
c += [x, y, z]
return c
def project3dTo2dArray(points3d, K, rotation, translation):
imagePoints, _ = cv2.projectPoints(points3d,
rotation,
translation,
K,
np.array([]))
p2d = imagePoints.reshape((imagePoints.shape[0],2))
return p2d
def estimate_pose(projectionA, projectionB):
principal_point = (w/2,h/2)
E, m = cv2.findEssentialMat(projectionA, projectionB, focal = f, pp = principal_point, method=cv2.RANSAC, threshold=1, prob=0.999)
_, r, t, _ = cv2.recoverPose(E, projectionA, projectionB, focal = f, pp = principal_point, mask = m)
angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
return angles, t
def main():
c = cube(0, 0, math.sqrt(.5), 0.1)
rotation = np.array([[0], [0], [0]], dtype=np.float32)
translation = np.array([[0], [0], [0]], dtype=np.float32)
zeroProjection = project3dTo2dArray(c, K, rotation, translation)
displayCube(w, h, zeroProjection)
rotation = np.array([[10], [-30], [5]], dtype=np.float32)
translation = np.array([[math.sqrt(.7)], [0], [math.sqrt(.3)]], dtype=np.float32)
print('applying rotation: ', rotation)
print('applying translation: ', translation)
movedprojection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, movedprojection)
estRot, estTra= estimate_pose(zeroProjection, movedprojection)
print('rotation estimate:', estRot)
print('translation estimate:', estTra)
rotation = np.array([[estRot[0]], [estRot[1]], [estRot[2]]], dtype=np.float32)
translation = np.array([[estTra[0]], [estTra[1]], [estTra[2]]], dtype=np.float32)
estimateProjection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, estimateProjection)
def displayCube(w, h, points):
img = np.zeros((h, w, 3), dtype=np.uint8)
plotCube(img, points)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
if k == ord('q'):
exit(0)
def plotCube(img, points):
# Red back face
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n-1]), (0, 0, 255), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (0, 0, 200), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)
# gray connectors
cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)
# Blue front face
cv2.line(img, tuple(points[0]), tuple(points[n-1]), (255, 0, 0), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
cv2.line(img, tuple(points[0]), tuple(points[n*(n-1)]), (200, 0, 0), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)
main()
我正在尝试根据两个不同图像中的匹配点来估计相对相机运动。很像这里描述的：
但是估计的平移和旋转没有意义。
我使用合成输入来确保所有点都有效且位置完美。
10 x 10 x 10 点均匀分布在一个立方体中。 （用蓝色正面、红色背面、较浅的顶部和较暗的底部绘制的立方体）
zeroProjection 相机在立方体前面，指向正面。
rotate90projection 立方体左侧的相机指向左侧面。
我绘制了两个投影。您可以轻松地通过视觉验证相机是否已平移 90 度并在两个投影之间的 x-z 平面中沿对角线移动。
在代码中，旋转（以度为单位）给出为 (0, -90, 0)
平移为(0.7071, 0, 0.7071)，相机移动距离正好为1。
然后我在 2d 点集上执行 findEssentialMat() 和 recoverPose() 以获得平移和旋转估计。
我希望看到与生成图像相同的平移和旋转，但估计完全错误：
rotation estimate: (-74.86565284711004, -48.52201867665918, 121.26023708879158)
translation estimate: [[0.96576997]
[0.17203598]
[0.19414426]]
如何恢复实际的 (0, -90, 0), (0.7071, 0, 0,7071) 转换？
显示两个立方体图像并打印出估计值的完整代码：
import cv2
import numpy as np
import math
def cameraMatrix(f, w, h):
return np.array([
[f, 0, w/2],
[0, f, h/2],
[0, 0, 1]])
n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)
def cube(x=0, y=0, z=0, radius=1):
c = np.zeros((n * n * n, 3), dtype=np.float32)
for i in range(0, n):
for j in range(0, n):
for k in range(0, n):
index = i + j * n + k * n * n
c[index] = [i, j, k]
c = 2 * c / (n - 1) - 1
c *= radius
c += [x, y, z]
return c
def project3dTo2dArray(points3d, K, rotation, translation):
imagePoints, _ = cv2.projectPoints(points3d,
rotation,
translation,
K,
np.array([]))
p2d = imagePoints.reshape((imagePoints.shape[0],2))
return p2d
def estimate_pose(projectionA, projectionB):
E, _ = cv2.findEssentialMat(projectionA, projectionB, focal = f)
_, r, t, _ = cv2.recoverPose(E, projectionA, projectionB)
angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
print('rotation estimate:', angles)
print('translation estimate:', t)
def main():
c = cube(0, 0, math.sqrt(.5), 0.1)
rotation = np.array([[0], [0], [0]], dtype=np.float32)
translation = np.array([[0], [0], [0]], dtype=np.float32)
zeroProjection = project3dTo2dArray(c, K, rotation, translation)
displayCube(w, h, zeroProjection)
rotation = np.array([[0], [-90], [0]], dtype=np.float32)
translation = np.array([[math.sqrt(.5)], [0], [math.sqrt(.5)]], dtype=np.float32)
print('applying rotation: ', rotation)
print('applying translation: ', translation)
rotate90projection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, rotate90projection)
estimate_pose(zeroProjection, rotate90projection)
def displayCube(w, h, points):
img = np.zeros((h, w, 3), dtype=np.uint8)
plotCube(img, points)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
if k == ord('q'):
exit(0)
def plotCube(img, points):
# Red back face
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n-1]), (0, 0, 255), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (0, 0, 200), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)
# gray connectors
cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)
# Blue front face
cv2.line(img, tuple(points[0]), tuple(points[n-1]), (255, 0, 0), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
cv2.line(img, tuple(points[0]), tuple(points[n*(n-1)]), (200, 0, 0), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)
main()
原来是我的代码中的一些小错误（比如错误的主要观点）。 下面的工作代码显示了 3 张图片。
首先是一个立方体显示在镜头前。 其次是相同的立方体，但不同的投影。相机已移动 1 个单位并绕所有 3 个轴旋转。 相机平移和旋转是根据两个投影估计的。 第三个显示使用旋转和平移估计投影的立方体。
该代码有效，因为第二张和第三张图片相似。
import cv2
import numpy as np
import math
def cameraMatrix(f, w, h):
return np.array([
[f, 0, w/2],
[0, f, h/2],
[0, 0, 1]])
n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)
def cube(x=0, y=0, z=0, radius=1):
c = np.zeros((n * n * n, 3), dtype=np.float32)
for i in range(0, n):
for j in range(0, n):
for k in range(0, n):
index = i + j * n + k * n * n
c[index] = [i, j, k]
c = 2 * c / (n - 1) - 1
c *= radius
c += [x, y, z]
return c
def project3dTo2dArray(points3d, K, rotation, translation):
imagePoints, _ = cv2.projectPoints(points3d,
rotation,
translation,
K,
np.array([]))
p2d = imagePoints.reshape((imagePoints.shape[0],2))
return p2d
def estimate_pose(projectionA, projectionB):
principal_point = (w/2,h/2)
E, m = cv2.findEssentialMat(projectionA, projectionB, focal = f, pp = principal_point, method=cv2.RANSAC, threshold=1, prob=0.999)
_, r, t, _ = cv2.recoverPose(E, projectionA, projectionB, focal = f, pp = principal_point, mask = m)
angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
return angles, t
def main():
c = cube(0, 0, math.sqrt(.5), 0.1)
rotation = np.array([[0], [0], [0]], dtype=np.float32)
translation = np.array([[0], [0], [0]], dtype=np.float32)
zeroProjection = project3dTo2dArray(c, K, rotation, translation)
displayCube(w, h, zeroProjection)
rotation = np.array([[10], [-30], [5]], dtype=np.float32)
translation = np.array([[math.sqrt(.7)], [0], [math.sqrt(.3)]], dtype=np.float32)
print('applying rotation: ', rotation)
print('applying translation: ', translation)
movedprojection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, movedprojection)
estRot, estTra= estimate_pose(zeroProjection, movedprojection)
print('rotation estimate:', estRot)
print('translation estimate:', estTra)
rotation = np.array([[estRot[0]], [estRot[1]], [estRot[2]]], dtype=np.float32)
translation = np.array([[estTra[0]], [estTra[1]], [estTra[2]]], dtype=np.float32)
estimateProjection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
displayCube(w, h, estimateProjection)
def displayCube(w, h, points):
img = np.zeros((h, w, 3), dtype=np.uint8)
plotCube(img, points)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xff
if k == ord('q'):
exit(0)
def plotCube(img, points):
# Red back face
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n-1]), (0, 0, 255), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
cv2.line(img, tuple(points[n*n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (0, 0, 200), 2)
cv2.line(img, tuple(points[n*n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)
# gray connectors
cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)
# Blue front face
cv2.line(img, tuple(points[0]), tuple(points[n-1]), (255, 0, 0), 2)
cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
cv2.line(img, tuple(points[0]), tuple(points[n*(n-1)]), (200, 0, 0), 2)
cv2.line(img, tuple(points[n-1]), tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)
main()