我是OpenCV和3D重建的新手 . 我试图从相机拍摄的2张图像中生成点 Cloud . 我使用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
