opencv - 从OpenCV的Essential矩阵正确解释 'recoverPose'之后的姿势(旋转和翻译)

Question

我一直在努力尝试正确地解释Essential矩阵中restorePose的结果。这基于How do I estimate positions of two cameras in OpenCV?中发布的原始代码

这是我正在使用的高级步骤:
1.在两个图像中检测ORB功能
2.使用BFMatcher匹配特征
3.在两个图像上查找findEssential
4. restorePose，即来自两个图像的R，T
5.使用R，T划分好特征(从restorePose中屏蔽)，以创建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曲线反射(reflect)出完全不同的情况。

Image matching results

Camera Pose Plot

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

那有什么呢？

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

Answer 1

我通过对旋转矩阵求逆来解决此问题，因为recoverPose函数定义了点移动方向而不是摄像机移动方向的旋转和平移。有关更多信息，请参见this post。