使用Opencv和Python从立体视觉生成点云期间使用相同的3D坐标

Question

我是OpenCV和3D重建的新手。我试图从相机拍摄的2张图像中生成点云。我使用this库校准了相机。在这里，我得到了相机的内在参数。我使用ORB特征检测器和描述符。这些是主要图像和我在特征检测后得到的图像。

然后我得到了基本矩阵并用它来得到基本矩阵。我用它来获取外在参数（旋转和平移）。但我到底所有的3D坐标都是一样的。如果我使用不同的P1和P2（硬编码），那么我会得到不同的3D坐标。

Here是代码。

 # Import library

import numpy as np
import cv2
from matplotlib import pyplot as plt
import time


def degeneracyCheckPass(first_points, second_points, rot, trans):
    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

start = time.time();
# Load a pair of images
img1 = cv2.imread('1.jpg',1)          
img2 = cv2.imread('2.jpg',1) 

#camera_matrix:

#cmat is K
zero = np.float32([[0.0], [0.0], [0.0]])
cmat = np.float32([[2610.9791576918024, 0.0, 1035.3907882371566], [0.0, 2611.5091416583214, 479.1873404639389], [0.0, 0.0, 1.0]])
cmat_inv = np.linalg.inv(cmat)
#dist_coefs:
dist = np.matrix('0.10783691621695163 0.0979247249215728 -0.010931168141218273 0.009561950912198006 -1.745839718546563')


# Initiate ORB detector
orb = cv2.ORB_create()

# Find the keypoints and descriptors with ORB
kp1, des1 = orb.detectAndCompute(img1,None)
kp2, des2 = orb.detectAndCompute(img2,None)

# Create BFMatcher object
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

# Match descriptors.
matches = bf.match(des1,des2)


#Ratio test as per Lowe's paper
good = []
pts1 = []
pts2 = []

dist = [m.distance for m in matches]
thres_dist = (sum(dist) / len(dist)) * 0.8
for m in matches:
    if m.distance < thres_dist:
        good.append(m)
        pts2.append(kp2[m.trainIdx].pt)
        pts1.append(kp1[m.queryIdx].pt)

# Draw  matches.
img3 = cv2.drawMatches(img1,kp1,img2,kp2,good, None, flags=2)
plt.imshow(img3),plt.show()

# Get the list of best matches from both the images
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)

# Find fundamental Matrix
F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)

# Select inlier points
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]

# Find essential matrix
#E = K'^T . F . K
E = cmat.T.dot(F).dot(cmat)


# Compute camera pose

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)):
    first_inliers.append(cmat_inv.dot([pts1[i][0], pts1[i][1], 1.0]))
    second_inliers.append(cmat_inv.dot([pts2[i][0], pts2[i][1], 1.0]))


# First choice: R = U * W * Vt, T = u_3
R = U.dot(W).dot(Vt)
T = U[:, 2]

# Start degeneracy checks
if not degeneracyCheckPass(first_inliers, second_inliers, R, T):
    # Second choice: R = U * W * Vt, T = -u_3
    T = - U[:, 2]
    if not degeneracyCheckPass(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 degeneracyCheckPass(first_inliers, second_inliers, R, T):
            # Fourth choice: R = U * Wt * Vt, T = -u_3
            T = - U[:, 2]

T_r = T.reshape((T.shape[0],-1))

# Reconstruct 3D location of matched points
# Get camera projection matrix


P1 = np.hstack((cmat, zero))
P2 = np.dot(cmat, np.hstack((R,T_r))) 


pts1t = pts1.transpose()
pts2t = pts2.transpose()


floatpst1t = np.array([pts1t[0,:].astype(np.float) / 1944.0, pts1t[1,:].astype(np.float) / 2592.0])


floatpst2t = np.array([pts2t[0,:].astype(np.float) / 1944.0, pts2t[1,:].astype(np.float) / 2592.0])


#Demo projection matrix
#P1 = np.eye(4)
#P2 = np.array([[ 0.878, -0.01 ,  0.479, -1.995],
#            [ 0.01 ,  1.   ,  0.002, -0.226],
#            [-0.479,  0.002,  0.878,  0.615],
#            [ 0.   ,  0.   ,  0.   ,  1.   ]])
X = cv2.triangulatePoints(P1[:4], P2[:4], floatpst1t, floatpst2t)
print X
#here I will divide the first three parameters with the fourth one to get the 3d coordinate

finish = time.time();
print(finish - start)

这是X与我计算的P1和P2

的结果

这是演示P1和P2