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

Posted 2023-02-19

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

【英文标题】：Correctly interpreting the Pose (Rotation and Translation) after 'recoverPose' from Essential matrix in OpenCV

【发布时间】：2019-06-26 11:13:48

【问题描述】：

我一直在努力从基本矩阵中正确解释恢复姿势的结果。这是基于How do I estimate positions of two cameras in OpenCV?中发布的原始代码

以下是我正在使用的高级步骤： 1.检测两张图片中的ORB特征 2.使用BFMatcher匹配特征 3. 跨两个图像查找Essential 4.recoverPose 即。来自两个图像的 R,T 5. 使用 R、T 对好的特征（从 recoveryPose 屏蔽）进行三角测量，以创建 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 的图反映了完全不同的东西。

Image matching results

Camera Pose Plot

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

那么是什么给了？？？

python脚本和测试图片可以在here找到。

【参考方案1】：

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

【讨论】：

因为我已经不扭曲使它们标准化的点。

【参考方案2】：

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

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

【讨论】：

行kp1_match_ud = cv2.undistortPoints(np.expand_dims(kp1_match,axis=1),K,D) 不扭曲坐标并使其正常化。因此，当使用未失真的点时，您不需要 K 矩阵。此外， findessentialMat 不将失真系数 D 作为输入，这就是我们必须这样做的原因。希望能解决这个问题。