从 OpenCV 中的基本矩阵“recoverPose”后正确解释姿势（旋转和平移）

Question

我一直在努力正确解释从基本矩阵中恢复姿势的结果。 这是基于如何估计 OpenCV 中两个摄像头的位置？

以下是我使用的高级步骤： 1. 检测两个图像中的 ORB 特征 2. 使用 BFMatcher 匹配特征 3. 在两个图像中 findEssential 4.recoverPose 即。 来自两个图像的 R,T 5. 使用 R, T 对好的特征（从recoverPose 屏蔽）进行三角剖分以创建 3d 点云（地标） 6. 作为基本事实，我还从图像中提取棋盘角并进行三角剖分它们使用上面计算的 R、T。 棋盘角的良好平面结构表明 R、T 对三角测量是准确的。 7. 绘制一切

import numpy as np
import cv2
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_pose3_on_axes(axes, gRp, origin, axis_length=0.1):
    """Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
    # get rotation and translation (center)
    #gRp = pose.rotation().matrix()  # rotation from pose to global
    #t = pose.translation()
    #origin = np.array([t.x(), t.y(), t.z()])

    # draw the camera axes
    x_axis = origin + gRp[:, 0] * axis_length
    line = np.append(origin, x_axis, axis=0)
    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')

    y_axis = origin + gRp[:, 1] * axis_length
    line = np.append(origin, y_axis, axis=0)
    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')

    z_axis = origin + gRp[:, 2] * axis_length
    line = np.append(origin, z_axis, axis=0)
    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')

img1 = cv2.imread('/home/vik748/data/chess_board/GOPR1488.JPG',1)          # queryImage
img2 = cv2.imread('/home/vik748/data/chess_board/GOPR1490.JPG',1)  

fx = 3551.342810
fy = 3522.689669
cx = 2033.513326
cy = 1455.489194

K = np.float64([[fx, 0, cx], 
                [0, fy, cy], 
                [0, 0, 1]])

D = np.float64([-0.276796, 0.113400, -0.000349, -0.000469]);

print(K,D)

# Convert images to greyscale
gr1=cv2.cvtColor(img1,cv2.COLOR_BGR2GRAY)
gr2=cv2.cvtColor(img2,cv2.COLOR_BGR2GRAY)

#Initiate ORB detector
detector = cv2.ORB_create(nfeatures=25000, edgeThreshold=15, patchSize=125, nlevels=32, 
                     fastThreshold=20, scaleFactor=1.2, WTA_K=2,
                     scoreType=cv2.ORB_HARRIS_SCORE, firstLevel=0)

# find the keypoints and descriptors with ORB
kp1, des1 = detector.detectAndCompute(gr1,None)
kp2, des2 = detector.detectAndCompute(gr2,None)

print ("Points detected: ",len(kp1), " and ", len(kp2))

bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

matches = bf.match(des1,des2)
kp1_match = np.array([kp1[mat.queryIdx].pt for mat in matches])
kp2_match = np.array([kp2[mat.trainIdx].pt for mat in matches])

kp1_match_ud = cv2.undistortPoints(np.expand_dims(kp1_match,axis=1),K,D)
kp2_match_ud = cv2.undistortPoints(np.expand_dims(kp2_match,axis=1),K,D)

E, mask_e = cv2.findEssentialMat(kp1_match_ud, kp2_match_ud, focal=1.0, pp=(0., 0.), 
                               method=cv2.RANSAC, prob=0.999, threshold=0.001)

print ("Essential matrix: used ",np.sum(mask_e) ," of total ",len(matches),"matches")

points, R, t, mask_RP = cv2.recoverPose(E, kp1_match_ud, kp2_match_ud, mask=mask_e)
print("points:",points,"\trecover pose mask:",np.sum(mask_RP!=0))
print("R:",R,"t:",t.T)

bool_mask = mask_RP.astype(bool)
img_valid = cv2.drawMatches(gr1,kp1,gr2,kp2,matches, None, 
                            matchColor=(0, 255, 0), 
                            matchesMask=bool_mask.ravel().tolist(), flags=2)

plt.imshow(img_valid)
plt.show()

ret1, corners1 = cv2.findChessboardCorners(gr1, (16,9),None)
ret2, corners2 = cv2.findChessboardCorners(gr2, (16,9),None)

corners1_ud = cv2.undistortPoints(corners1,K,D)
corners2_ud = cv2.undistortPoints(corners2,K,D)

#Create 3 x 4 Homogenous Transform
Pose_1 = np.hstack((np.eye(3, 3), np.zeros((3, 1))))
print ("Pose_1: ", Pose_1)
Pose_2 = np.hstack((R, t))
print ("Pose_2: ", Pose_2)

# Points Given in N,1,2 array 
landmarks_hom = cv2.triangulatePoints(Pose_1, Pose_2, 
                                     kp1_match_ud[mask_RP[:,0]==1], 
                                     kp2_match_ud[mask_RP[:,0]==1]).T
landmarks_hom_norm = landmarks_hom /  landmarks_hom[:,-1][:,None]
landmarks = landmarks_hom_norm[:, :3]

corners_hom = cv2.triangulatePoints(Pose_1, Pose_2, corners1_ud, corners2_ud).T
corners_hom_norm = corners_hom /  corners_hom[:,-1][:,None]
corners_12 = corners_hom_norm[:, :3]

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_aspect('equal')         # important!
title = ax.set_title('3D Test')
ax.set_zlim3d(-5,10)

# Plot triangulated featues in Red
graph, = ax.plot(landmarks[:,0], landmarks[:,1], landmarks[:,2], linestyle="", marker="o",color='r')
# Plot triangulated chess board in Green
graph, = ax.plot(corners_12[:,0], corners_12[:,1], corners_12[:,2], linestyle="", marker=".",color='g')

# Plot pose 1
plot_pose3_on_axes(ax,np.eye(3),np.zeros(3)[np.newaxis], axis_length=0.5)
#Plot pose 2
plot_pose3_on_axes(ax, R, t.T, axis_length=1.0)
ax.set_zlim3d(-2,5)
ax.view_init(-70, -90)
plt.show()

因此，正如我们从图像 1488 和 1490 中看到的那样，相机正在向左-向上移动，并且向下和向右移动。 然而，第二个位置的 R 和 T 的图反映了完全不同的东西。

图像匹配结果

相机姿势图

我曾尝试使用 R' 和 -(R')*T 进行反转，但这也不能正确绘制。 我尝试了很多不同的组合，但似乎都没有意义。

那给什么？？？

可以在此处找到 python 脚本和测试图像。

Answer 1

我通过取旋转矩阵的逆来解决这个问题，因为recoverPose函数定义了点移动方向的旋转和平移，而不是相机移动的方向。 有关更多信息，请参阅此帖子。

Answer 2

为什么你在cv2.findEssentialMat中使用cv2.findEssentialMat focal = 1.0和pp=(0,0) ，当你已经在 Camera Matrix 'K'定义了fx , fy , cx , cy 。 据我所知，在cv2.findEssentialMat应该使用相同的fx 、 fy 、 cx和cy或'K'本身。 像这样的东西。 如果我错了，请向我解释。 我也在解决同样的问题。

E, mask_e = cv2.findEssentialMat(kp1_match_ud, kp2_match_ud, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=0.001)