对极几何姿态估计：对极线看起来不错但姿态错误
Epipolar geometry pose estimation: Epipolar lines look good but wrong pose
我正在尝试使用 OpenCV 来估计相机的一个相对于另一个的姿势，使用 SIFT 特征跟踪、FLANN 匹配以及随后的基础和基本矩阵计算。分解基本矩阵后，我检查退化配置并获得 "right" R 和 t。
问题是，他们似乎从来都不对。我包括几个图像对：
- 图像 2 沿 Y 轴旋转 45 度且位置相同 w.r.t。图 1.
图像对
结果
- 图片 2 取自大约。沿负 X 方向几米远，负 Y 方向轻微位移。约相机姿势沿 Y 轴旋转 45-60 度。
图像对
结果
第二种情况下的平移向量似乎高估了 Y 方向的运动而低估了 X 方向的运动。在这两种情况下，转换为欧拉角的旋转矩阵都会给出错误的结果。许多其他数据集也会发生这种情况。我已经尝试在 RANSAC、LMEDS 等之间切换基本矩阵计算技术，现在我正在使用 RANSAC 进行计算，第二次计算仅使用 8 点法的内点。更改特征检测方法也无济于事。对极线似乎是适当的，基本矩阵满足 x'.F.x = 0
我是不是遗漏了什么根本性的错误？如果程序正确理解对极几何，可能会发生什么导致完全错误的姿势？我正在检查以确保点位于两个摄像头前面。任何 thoughts/suggestions 都会很有帮助。谢谢！
编辑：尝试使用相同的技术，将两个不同的校准相机隔开；并计算基本矩阵为 K2'.F.K1，但平移和旋转仍然有很大差距。
Code供参考
import cv2
import numpy as np
from matplotlib import pyplot as plt
# K2 = np.float32([[1357.3, 0, 441.413], [0, 1355.9, 259.393], [0, 0, 1]]).reshape(3,3)
# K1 = np.float32([[1345.8, 0, 394.9141], [0, 1342.9, 291.6181], [0, 0, 1]]).reshape(3,3)
# K1_inv = np.linalg.inv(K1)
# K2_inv = np.linalg.inv(K2)
K = np.float32([3541.5, 0, 2088.8, 0, 3546.9, 1161.4, 0, 0, 1]).reshape(3,3)
K_inv = np.linalg.inv(K)
def in_front_of_both_cameras(first_points, second_points, rot, trans):
# check if the point correspondences are in front of both images
rot_inv = rot
for first, second in zip(first_points, second_points):
first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)
if first_3d_point[2] < 0 or second_3d_point[2] < 0:
return False
return True
def drawlines(img1,img2,lines,pts1,pts2):
''' img1 - image on which we draw the epilines for the points in img1
lines - corresponding epilines '''
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
r,c = img1.shape
img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
for r,pt1,pt2 in zip(lines,pts1,pts2):
color = tuple(np.random.randint(0,255,3).tolist())
x0,y0 = map(int, [0, -r[2]/r[1] ])
x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
cv2.line(img1, (x0,y0), (x1,y1), color,1)
cv2.circle(img1,tuple(pt1), 10, color, -1)
cv2.circle(img2,tuple(pt2), 10,color,-1)
return img1,img2
img1 = cv2.imread('C:\Users\Sai\Desktop\room1.jpg', 0)
img2 = cv2.imread('C:\Users\Sai\Desktop\room0.jpg', 0)
img1 = cv2.resize(img1, (0,0), fx=0.5, fy=0.5)
img2 = cv2.resize(img2, (0,0), fx=0.5, fy=0.5)
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.7*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts2 = np.float32(pts2)
pts1 = np.float32(pts1)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_RANSAC)
# Selecting only the inliers
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_8POINT)
print "Fundamental matrix is"
print
print F
pt1 = np.array([[pts1[0][0]], [pts1[0][1]], [1]])
pt2 = np.array([[pts2[0][0], pts2[0][1], 1]])
print "Fundamental matrix error check: %f"%np.dot(np.dot(pt2,F),pt1)
print " "
# drawing lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(img1,img2,lines1,pts1,pts2)
# drawing lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(img2,img1,lines2,pts2,pts1)
E = K.T.dot(F).dot(K)
print "The essential matrix is"
print E
print
U, S, Vt = np.linalg.svd(E)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
first_inliers = []
second_inliers = []
for i in range(len(pts1)):
# normalize and homogenize the image coordinates
first_inliers.append(K_inv.dot([pts1[i][0], pts1[i][1], 1.0]))
second_inliers.append(K_inv.dot([pts2[i][0], pts2[i][1], 1.0]))
# Determine the correct choice of second camera matrix
# only in one of the four configurations will all the points be in front of both cameras
# First choice: R = U * Wt * Vt, T = +u_3 (See Hartley Zisserman 9.19)
R = U.dot(W).dot(Vt)
T = U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Second choice: R = U * W * Vt, T = -u_3
T = - U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Third choice: R = U * Wt * Vt, T = u_3
R = U.dot(W.T).dot(Vt)
T = U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Fourth choice: R = U * Wt * Vt, T = -u_3
T = - U[:, 2]
# Computing Euler angles
thetaX = np.arctan2(R[1][2], R[2][2])
c2 = np.sqrt((R[0][0]*R[0][0] + R[0][1]*R[0][1]))
thetaY = np.arctan2(-R[0][2], c2)
s1 = np.sin(thetaX)
c1 = np.cos(thetaX)
thetaZ = np.arctan2((s1*R[2][0] - c1*R[1][0]), (c1*R[1][1] - s1*R[2][1]))
print "Pitch: %f, Yaw: %f, Roll: %f"%(thetaX*180/3.1415, thetaY*180/3.1415, thetaZ*180/3.1415)
print "Rotation matrix:"
print R
print
print "Translation vector:"
print T
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()
相机的内参K是怎么得到的？在我看来，基本矩阵的计算是正确的，因为匹配点位于对极线上。但如果矩阵 K 不准确，您可能会得到错误的基本矩阵，从而得到错误的 R 和 t。
有很多事情会导致根据点对应关系对相机姿态的估计不准确。您必须考虑的一些因素：-
(*) 8 点法最小化代数误差 ( x'.F.x = 0)。通常最好找到一个能最小化有意义的几何误差的解决方案。例如，您可以在 RANSAC 实现中使用重新投影误差。
(*) 8点求解基本矩阵的线性算法对噪声敏感。亚像素级精确点匹配、适当的数据归一化和精确的相机校准对于获得更好的结果都很重要。
(*) 特征点定位和匹配会导致噪声点匹配，因此通过求解代数方程 x'Fx 得到的解应该真正用作初始估计，进一步的步骤如参数优化需要应用于优化解决方案。
(*) 某些双视图相机配置可能导致解不明确，因此需要进一步的方法（例如第三视图消歧）以获得可靠的结果。
在书中"Programming Computer Vision with Python"，我们需要使用这样的代码来检查E的排名：
计算第二个相机矩阵（假设P1 = [I 0]）
来自基本矩阵。输出是四个列表
可能的相机矩阵。
确保 E 是等级 2
U,S,V = np.linalg.svd(E)
if np.linalg.det(np.dot(U,V))<0:
V = -V
E = np.dot(U,np.dot(np.diag([1.0,1.0,0.0]),V))
我不确定这是否也可以提高性能。
请告诉我。
扩展@Sammy 的第二点，
(*) The linear algorithm which solves for fundamental matrix from 8
points is sensitive to noise. Sub-pixel accurate point matching,
proper data normalization and accurate camera calibration are all
important for better results.
我建议直接使用 findEssentialMat 通过 openCV 而不是 findFundamentalMat 来计算基本矩阵，因为前者的自由度较小，因此在数值上更稳定。
新的 openCV 版本也支持为 findEssentialMat 提供两种不同的相机矩阵和畸变系数。
我正在尝试使用 OpenCV 来估计相机的一个相对于另一个的姿势，使用 SIFT 特征跟踪、FLANN 匹配以及随后的基础和基本矩阵计算。分解基本矩阵后，我检查退化配置并获得 "right" R 和 t。
问题是，他们似乎从来都不对。我包括几个图像对：
- 图像 2 沿 Y 轴旋转 45 度且位置相同 w.r.t。图 1.
图像对
结果
- 图片 2 取自大约。沿负 X 方向几米远，负 Y 方向轻微位移。约相机姿势沿 Y 轴旋转 45-60 度。
图像对
结果
第二种情况下的平移向量似乎高估了 Y 方向的运动而低估了 X 方向的运动。在这两种情况下，转换为欧拉角的旋转矩阵都会给出错误的结果。许多其他数据集也会发生这种情况。我已经尝试在 RANSAC、LMEDS 等之间切换基本矩阵计算技术，现在我正在使用 RANSAC 进行计算，第二次计算仅使用 8 点法的内点。更改特征检测方法也无济于事。对极线似乎是适当的，基本矩阵满足 x'.F.x = 0
我是不是遗漏了什么根本性的错误？如果程序正确理解对极几何，可能会发生什么导致完全错误的姿势？我正在检查以确保点位于两个摄像头前面。任何 thoughts/suggestions 都会很有帮助。谢谢！
编辑：尝试使用相同的技术，将两个不同的校准相机隔开；并计算基本矩阵为 K2'.F.K1，但平移和旋转仍然有很大差距。
Code供参考
import cv2
import numpy as np
from matplotlib import pyplot as plt
# K2 = np.float32([[1357.3, 0, 441.413], [0, 1355.9, 259.393], [0, 0, 1]]).reshape(3,3)
# K1 = np.float32([[1345.8, 0, 394.9141], [0, 1342.9, 291.6181], [0, 0, 1]]).reshape(3,3)
# K1_inv = np.linalg.inv(K1)
# K2_inv = np.linalg.inv(K2)
K = np.float32([3541.5, 0, 2088.8, 0, 3546.9, 1161.4, 0, 0, 1]).reshape(3,3)
K_inv = np.linalg.inv(K)
def in_front_of_both_cameras(first_points, second_points, rot, trans):
# check if the point correspondences are in front of both images
rot_inv = rot
for first, second in zip(first_points, second_points):
first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)
if first_3d_point[2] < 0 or second_3d_point[2] < 0:
return False
return True
def drawlines(img1,img2,lines,pts1,pts2):
''' img1 - image on which we draw the epilines for the points in img1
lines - corresponding epilines '''
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
r,c = img1.shape
img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
for r,pt1,pt2 in zip(lines,pts1,pts2):
color = tuple(np.random.randint(0,255,3).tolist())
x0,y0 = map(int, [0, -r[2]/r[1] ])
x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
cv2.line(img1, (x0,y0), (x1,y1), color,1)
cv2.circle(img1,tuple(pt1), 10, color, -1)
cv2.circle(img2,tuple(pt2), 10,color,-1)
return img1,img2
img1 = cv2.imread('C:\Users\Sai\Desktop\room1.jpg', 0)
img2 = cv2.imread('C:\Users\Sai\Desktop\room0.jpg', 0)
img1 = cv2.resize(img1, (0,0), fx=0.5, fy=0.5)
img2 = cv2.resize(img2, (0,0), fx=0.5, fy=0.5)
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.7*n.distance:
good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts2 = np.float32(pts2)
pts1 = np.float32(pts1)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_RANSAC)
# Selecting only the inliers
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_8POINT)
print "Fundamental matrix is"
print
print F
pt1 = np.array([[pts1[0][0]], [pts1[0][1]], [1]])
pt2 = np.array([[pts2[0][0], pts2[0][1], 1]])
print "Fundamental matrix error check: %f"%np.dot(np.dot(pt2,F),pt1)
print " "
# drawing lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(img1,img2,lines1,pts1,pts2)
# drawing lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(img2,img1,lines2,pts2,pts1)
E = K.T.dot(F).dot(K)
print "The essential matrix is"
print E
print
U, S, Vt = np.linalg.svd(E)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
first_inliers = []
second_inliers = []
for i in range(len(pts1)):
# normalize and homogenize the image coordinates
first_inliers.append(K_inv.dot([pts1[i][0], pts1[i][1], 1.0]))
second_inliers.append(K_inv.dot([pts2[i][0], pts2[i][1], 1.0]))
# Determine the correct choice of second camera matrix
# only in one of the four configurations will all the points be in front of both cameras
# First choice: R = U * Wt * Vt, T = +u_3 (See Hartley Zisserman 9.19)
R = U.dot(W).dot(Vt)
T = U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Second choice: R = U * W * Vt, T = -u_3
T = - U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Third choice: R = U * Wt * Vt, T = u_3
R = U.dot(W.T).dot(Vt)
T = U[:, 2]
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T):
# Fourth choice: R = U * Wt * Vt, T = -u_3
T = - U[:, 2]
# Computing Euler angles
thetaX = np.arctan2(R[1][2], R[2][2])
c2 = np.sqrt((R[0][0]*R[0][0] + R[0][1]*R[0][1]))
thetaY = np.arctan2(-R[0][2], c2)
s1 = np.sin(thetaX)
c1 = np.cos(thetaX)
thetaZ = np.arctan2((s1*R[2][0] - c1*R[1][0]), (c1*R[1][1] - s1*R[2][1]))
print "Pitch: %f, Yaw: %f, Roll: %f"%(thetaX*180/3.1415, thetaY*180/3.1415, thetaZ*180/3.1415)
print "Rotation matrix:"
print R
print
print "Translation vector:"
print T
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()
相机的内参K是怎么得到的？在我看来，基本矩阵的计算是正确的，因为匹配点位于对极线上。但如果矩阵 K 不准确，您可能会得到错误的基本矩阵，从而得到错误的 R 和 t。
有很多事情会导致根据点对应关系对相机姿态的估计不准确。您必须考虑的一些因素：-
(*) 8 点法最小化代数误差 ( x'.F.x = 0)。通常最好找到一个能最小化有意义的几何误差的解决方案。例如，您可以在 RANSAC 实现中使用重新投影误差。
(*) 8点求解基本矩阵的线性算法对噪声敏感。亚像素级精确点匹配、适当的数据归一化和精确的相机校准对于获得更好的结果都很重要。
(*) 特征点定位和匹配会导致噪声点匹配，因此通过求解代数方程 x'Fx 得到的解应该真正用作初始估计，进一步的步骤如参数优化需要应用于优化解决方案。
(*) 某些双视图相机配置可能导致解不明确，因此需要进一步的方法（例如第三视图消歧）以获得可靠的结果。
在书中"Programming Computer Vision with Python"，我们需要使用这样的代码来检查E的排名：
计算第二个相机矩阵（假设P1 = [I 0]） 来自基本矩阵。输出是四个列表 可能的相机矩阵。
确保 E 是等级 2
U,S,V = np.linalg.svd(E)
if np.linalg.det(np.dot(U,V))<0:
V = -V
E = np.dot(U,np.dot(np.diag([1.0,1.0,0.0]),V))
我不确定这是否也可以提高性能。 请告诉我。
扩展@Sammy 的第二点，
(*) The linear algorithm which solves for fundamental matrix from 8 points is sensitive to noise. Sub-pixel accurate point matching, proper data normalization and accurate camera calibration are all important for better results.
我建议直接使用 findEssentialMat 通过 openCV 而不是 findFundamentalMat 来计算基本矩阵，因为前者的自由度较小，因此在数值上更稳定。
新的 openCV 版本也支持为 findEssentialMat 提供两种不同的相机矩阵和畸变系数。