部分三维空间点的坐标

目录

一、三角测量以及三维点的可视化。实验过程

2.1 使用Matplotlib绘制三维数据、    X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[i])    d1 = dot(P1, X)[2]    d2 = dot(P2[i], X)[2]    X_new = sfm.triangulate(x1n_new[:, inliers_new], x3n_new[:, inliers_new], P1, P3[i])    d1_new = dot(P1, X_new)[2]    d3_new = dot(P3[i], X_new)[2]    if sum(d1 > 0) + sum(d2 > 0) > maxres:        maxres = sum(d1 > 0) + sum(d2 > 0)        ind = i        infront = (d1 > 0) & (d2 > 0)        maxres_new = sum(d1_new > 0) + sum(d3_new > 0)        ind_new = i        infront_new = (d1_new > 0) & (d3_new > 0)    X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[ind])    X = X[:, infront]    X_new = sfm.triangulate(x1n_new[:, inliers_new], x3n_new[:, inliers_new], P1, P3[ind_new])    X_new = X_new[:, infront_new]fig = figure()ax = fig.gca(projection='3d')ax.plot(-X[0], X[1], X[2], 'k.')ax.plot(-X_new[0], X_new[1], X_new[2], 'r.')axis('off')cam1 = camera.Camera(P1)cam2 = camera.Camera(P2[ind])cam3 = camera.Camera(P3[ind_new])x1p = cam1.project(X)x2p = cam2.project(X)x3p = cam3.project(X_new)print("相机投影矩阵 P1:\n", P1)print("相机投影矩阵 P2:\n", P2[ind])print("相机投影矩阵 P3:\n", P3[ind_new])print("部分三维点坐标 X:\n", X[:4])print("部分三维点坐标 X_new:\n", X_new[:4])x1p = dot(K, x1p)x2p = dot(K1, x2p)x3p = dot(K2, x3p)figure()imshow(im1)gray()plot(x1p[0], x1p[1], 'o')plot(l1[0], l1[1], 'r.')axis('off')figure()imshow(im2)gray()plot(x2p[0], x2p[1], 'o')plot(l2[0], l2[1], 'r.')axis('off')figure()imshow(im3)gray()plot(x3p[0], x3p[1], 'o')plot(l3[0], l3[1], 'r.')axis('off')show()

（2）实验结果

 a.如图8所示，打印本质矩阵E

b.如图9所示，打印相机投影矩阵，实验中选取了三张不同视图的图片

c.如图10所示，打印部分三维坐标

d.实现可视化，如图11、x1 = homography.make_homog(l1[:, ndx])x2 = homography.make_homog(l2[:, ndx2])print("Shape of x1 before normalization:", x1.shape)print("Shape of x2 before normalization:", x2.shape)inv_K = np.linalg.inv(K)inv_K1 = np.linalg.inv(K1)inv_K2 = np.linalg.inv(K2)#计算相机内参矩阵的逆矩阵，用于将像素坐标转换为归一化图像平面坐标。

2.3 对拍摄物（鼠标）的多视图三维重建

（1）SFM.py文件

import cv2import numpy as npimport matplotlib.pyplot as pltfrom mpl_toolkits.mplot3d import Axes3Dimages = ["D:\\Computer vision\\1118\\picture1.jpg",           "D:\\Computer vision\\1118\\picture2.jpg",          "D:\\Computer vision\\1118\\picture6.jpg",           "D:\\Computer vision\\1118\\picture5.jpg",          "D:\\Computer vision\\1118\\picture10.jpg",          "D:\\Computer vision\\1118\\picture8.jpg",          "D:\\Computer vision\\1118\\picture7.jpg",          "D:\\Computer vision\\1118\\picture9.jpg"]imgs = [cv2.imread(img) for img in images]gray_imgs = [cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) for img in imgs]orb = cv2.ORB_create()keypoints_and_descriptors = [orb.detectAndCompute(img, None) for img in gray_imgs]keypoints = [kp_desc[0] for kp_desc in keypoints_and_descriptors]descriptors = [kp_desc[1] for kp_desc in keypoints_and_descriptors]bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)matches = bf.match(descriptors[0], descriptors[1])good_matches = sorted(matches, key=lambda x: x.distance)[:50]pts1 = np.float32([keypoints[0][m.queryIdx].pt for m in good_matches])pts2 = np.float32([keypoints[1][m.trainIdx].pt for m in good_matches])img_matches = cv2.drawMatches(imgs[0], keypoints[0], imgs[1], keypoints[1], good_matches, None)cv2.imshow('Matches', img_matches)cv2.waitKey(0)cv2.destroyAllWindows()K = np.array([[2394, 0, 932], [0, 2398, 628], [0, 0, 1]])pts1_normalized = cv2.undistortPoints(np.expand_dims(pts1, axis=1), K, None)pts2_normalized = cv2.undistortPoints(np.expand_dims(pts2, axis=1), K, None)F, mask = cv2.findFundamentalMat(pts1_normalized, pts2_normalized, cv2.FM_8POINT)print("基础矩阵F：",F)E = K.T @ F @ Kprint("本质矩阵E：",E)_, R, t, mask = cv2.recoverPose(E, pts1_normalized, pts2_normalized)points4D = cv2.triangulatePoints(np.hstack((np.eye(3), np.zeros((3, 1)))), np.hstack((R, t)), pts1_normalized.T, pts2_normalized.T).reshape(-1, 4)points3D = points4D[:, :3] / points4D[:, 3:]print("3D Points:\n", points3D)valid_points = points3D[~np.any(np.isinf(points3D), axis=1) & ~np.any(np.isnan(points3D), axis=1)]print("有效点数:", valid_points.shape[0])print("最小值:", valid_points.min(axis=0))print("最大值:", valid_points.max(axis=0))ax_lim = (valid_points.max(axis=0) - valid_points.min(axis=0)) * 0.1fig = plt.figure()ax = fig.add_subplot(111, projection='3d')ax.scatter(valid_points[:, 0], valid_points[:, 1], valid_points[:, 2])ax.set_xlabel('X')ax.set_ylabel('Y')ax.set_zlabel('Z')ax.set_xlim(valid_points[:, 0].min() - ax_lim[0], valid_points[:, 0].max() + ax_lim[0])ax.set_ylim(valid_points[:, 1].min() - ax_lim[1], valid_points[:, 1].max() + ax_lim[1])ax.set_zlim(valid_points[:, 2].min() - ax_lim[2], valid_points[:, 2].max() + ax_lim[2])plt.show()

 (2)实验结果

a.如图13，打印基础矩阵

b.如图14，打印本质矩阵

c.如图15，输出三维点坐标

d.如图16、本质矩阵等相关知识点，对某个物体从不同角度拍摄多张照片，利用SFM构建出三维模型，并可视化。

e. 由三维点计算照相机矩阵，如图6所示，以归一化的格式打印估计出的照相机矩阵，使用估计出的照相机矩阵投影这些三维点，绘制投影后的结果如图7所示。x1_new = homography.make_homog(l1[:, ndx3])x3_new = homography.make_homog(l3[:, ndx3_train])print("Shape of x1_new before normalization:", x1_new.shape)print("Shape of x3_new before normalization:", x3_new.shape)x1n = dot(inv_K, x1)x2n = dot(inv_K1, x2)x1n_new = dot(inv_K, x1_new)x3n_new = dot(inv_K2, x3_new)model = sfm.RansacModel()E, inliers = sfm.F_from_ransac(x1n, x2n, model)E_new, inliers_new = sfm.F_from_ransac(x1n_new, x3n_new, model)print("本质矩阵E：",E)P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])P2 = sfm.compute_P_from_essential(E)P3 = sfm.compute_P_from_essential(E_new)ind = 0maxres = 0for i in range(4):#使用三角测量方法从匹配的点对中恢复三维点坐标。三角测量（Triangulation）以及随机抽样一致性（RANSAC）算法。实验小结

一、投影矩阵计算、匹配、本质矩阵（Essential Matrix）、实验过程

2.1 使用Matplotlib绘制三维数据、

import numpy as npfrom matplotlib.pyplot import cla, plotfrom numpy import diag, dot, linspace, mean, sqrt, std, vstack, zerosfrom scipy import linalgfrom torch import det, svdimport torch#计算基本矩阵（Fundamental Matrix）def compute_fundamental(x1,x2):    n=x1.shape[1]    if x2.shape[1]!=n:        raise ValueError("Number of points don't match.")    A=zeros((n,9))    for i in range(n):        A[i]=[x1[0,i]*x2[0,i],x1[0,i]*x2[1,i],x1[0,i]*x2[2,i],              x1[1,i]*x2[0,i],x1[1,i]*x2[1,i],x1[1,i]*x2[2,i],              x1[2,i]*x2[0,i],x1[2,i]*x2[1,i],x1[2,i]*x2[2,i]]        U,S,V=linalg.svd(A)        F=V[-1].reshape(3,3)        U,S,V=linalg.svd(F)        S[2]=0        F=dot(U,dot(diag(S),V))        return F    #计算极点def compute_episole(F):    U,S,V=linalg.svd(F)    e=V[-1]    return e/e[2]#绘制极线def plot_epipolar_line(im,F,x,epipole=None,show_epipole=True):    m,n=im.shape[:2]    line=dot(F,x)    t=linspace(0,n,100)    lt=np.array([(line[2]+line[0]*tt)/(-line[1])for tt in t])    ndx=(lt>=0)&(lt<m)    plot(t[ndx],lt[ndx],linewidth=2)    if show_epipole:        if epipole is None:            epipole=compute_episole(F)        plot(epipole[0]/epipole[2],epipole[1]/epipole[2],'r*')#三角测量def triangle_point(x1,x2,P1,P2):    M=zeros((6,6))    M[:3,:4]=P1    M[3:,:4]=P2    M[:3,4]=-x1    M[3:,5]=-x2    U,S,V=linalg.svd(M)    X=V[-1,:4]    return X/X[3]def triangulate(x1,x2,P1,P2):    n=x1.shape[1]    if x2.shape[1]!=n:        raise ValueError("Number of points don't match.")    X=[triangle_point(x1[:,i],x2[:,i],P1,P2)for i in range(n)]    return np.array(X).T#从点对应关系计算投影矩阵def compute_P(x,X):    n=x.shape[1]    if X.shape[1]!=n:        raise ValueError("Number of points don't match.")    M=zeros((3*n,12+n))    for i in range(n):        M[3*i,0:4]=X[:,i]        M[3*i+1,4:8]=X[:,i]        M[3*i+2,8:12]=X[:,i]        M[3*i:3*i+3,i+12]=-x[:,i]    U,S,V=linalg.svd(M)    return V[-1,:12].reshape((3,4))#从基本矩阵计算投影矩阵def compute_P_from_fundamental(F):    e=compute_episole(F.T)    Te=skew(e)    return vstack((dot(Te,F.T).T,e)).Tdef skew(a):    return np.array([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])#从本质矩阵计算投影矩阵def compute_P_from_essential(E):    U,S,V=linalg.svd(E)    if det(torch.tensor(dot(U, V)))<0:        V=-V    E=dot(U,dot(diag([1,1,0]),V))    Z=skew([0,0,-1])    W=np.array([[0,-1,0],[1,0,0],[0,0,1]])    P2=[vstack((dot(U,dot(W,V)).T,U[:,2])).T,        vstack((dot(U,dot(W,V)).T,-U[:,2])).T,        vstack((dot(U,dot(W.T,V)).T,U[:,2])).T,        vstack((dot(U,dot(W.T,V)).T,-U[:,2])).T]    return P2#归一化八点法计算基本矩阵def compute_fundamental_normalized(x1,x2):        n=x1.shape[1]        if x2.shape[1]!=n:            raise ValueError("Number of points don't match.")                x1=x1/x1[2]        mean_1=mean(x1[:2],axis=1)        S1=sqrt(2)/std(x1[:2])        T1=np.array([[S1,0,-S1*mean_1[0]],[0,S1,-S1*mean_1[1]],[0,0,1]])        x1=dot(T1,x1)                x2=x2/x2[2]        mean_2=mean(x2[:2],axis=1)        S2=sqrt(2)/std(x2[:2])        T2=np.array([[S2,0,-S2*mean_2[0]],[0,S2,-S2*mean_2[1]],[0,0,1]])        x2=dot(T2,x2)        F=compute_fundamental(x1,x2)        F=dot(T1.T,dot(F,T2))        return F/F[2,2]#RANSAC模型 用于拟合基本矩阵class RansacModel(object):    def __init__(self,debug=False):        self.debug=debug    def fit(self,data):        data=data.T        x1=data[:3,:8]        x2=data[3:,:8]        F=compute_fundamental_normalized(x1,x2)        return F        def get_error(self,data,F):        data=data.T        x1=data[:3]        x2=data[3:]        Fx1=dot(F,x1)        Fx2=dot(F,x2)        denom=Fx1[0]**2+Fx1[1]**2+Fx2[0]**2+Fx2[1]**2        err=(diag(dot(x1.T,dot(F,x2))))**2/denom        return err    def F_from_ransac(x1,x2,model,maxiter=5000,match_theshold=1e-6):        import ransac        data=vstack((x1,x2))        F,ransac_data=ransac.ransac(data.T,model,8,maxiter,match_theshold,10,                                    return_all=True)        return F,ransac_data['inliers']

（2）load_vggdata.py文件

from tkinter import NOfrom PIL import Imagefrom matplotlib import figure, pyplotfrom matplotlib.pyplot import imshow,plot,axis, showfrom numpy import genfromtxt, loadtxt, ones,  vstack, whereimport numpy as npimport camerafrom mpl_toolkits.mplot3d import axes3dimport sfmimage1 = Image.open("D:/Computer vision/3d/images/001.jpg")im1 = np.array(image1)image2 = Image.open("D:/Computer vision/3d/images/002.jpg")im2 = np.array(image2)points2D=[loadtxt('D:/Computer vision/3d/2D/00'+str(i+1)+'.corners').T for i in range(3)]points3D=loadtxt('D:/Computer vision/3d/3D/p3d').Tcorr = genfromtxt('D:/Computer vision/3d/2D/nview-corners', dtype='int', invalid_raise=False, usemask=True)P = [camera.Camera(loadtxt(f'D:/Computer vision/3d/2D/00{i+1}.P')) for i in range(3)]def process1():    X=vstack((points3D,ones(points3D.shape[1])))    x=P[0].project(X)    pyplot.figure()    imshow(im1)    plot(points2D[0][0],points2D[0][1],'*')    axis('off')    pyplot.figure()    imshow(im1)    plot(x[0],x[1],'r.')    axis('off')    show()def process2():    fig=pyplot.figure()    ax=fig.gca(projection="3d")    ax.plot(points3D[0],points3D[1],points3D[2],'k.')    show()def process3():    ndx=(corr[:,0]>=0)&(corr[:,1]>=0)    x1=points2D[0][:,corr[ndx,0]]    x1=vstack((x1,ones(x1.shape[1])))    x2=points2D[1][:,corr[ndx,1]]    x2=vstack((x2,ones(x2.shape[1])))    F=sfm.compute_fundamental(x1,x2)    e=sfm.compute_episole(F)    pyplot.figure()    imshow(im1)    for i in range(5):        sfm.plot_epipolar_line(im1,F,x2[:,i],e,False)    axis('off')    pyplot.figure()    imshow(im2)    for i in range(5):        plot(x2[0,i],x2[1,i],'o')    axis('off')    show()def process4():    ndx=(corr[:,0]>=0)&(corr[:,1]>=0)    x1=points2D[0][:,corr[ndx,0]]    x1=vstack((x1,ones(x1.shape[1])))    x2=points2D[1][:,corr[ndx,1]]    x2=vstack((x2,ones(x2.shape[1])))    Xtrue=points3D[:,ndx]    Xtrue=vstack((Xtrue,ones(Xtrue.shape[1])))    Xest=sfm.triangulate(x1,x2,P[0].P,P[1].P)    print(Xest[:,:3])    print(Xtrue[:,:3])    fig1=pyplot.figure()    ax1=fig1.gca(projection='3d')    ax1.plot(Xest[0],Xest[1],Xest[2],'ko')    ax1.plot(Xtrue[0],Xtrue[1],Xtrue[2],'r.')    show()def process5():    corr1=corr[:,0]    ndx3D=where(corr1>=0)[0]    ndx2D=corr1[ndx3D]    x=points2D[0][:,ndx2D]    x=vstack((x,ones(x.shape[1])))    X=points3D[:,ndx3D]    X=vstack((X,ones(X.shape[1])))    Pest=camera.Camera(sfm.compute_P(x,X))    print(Pest.P/Pest.P[2,3])    print(P[0].P/P[0].P[2,3])    xest=Pest.project(X)    pyplot.figure()    imshow(im1)    plot(x[0],x[1],'bo')    plot(xest[0],xest[1],'r.')    axis('off')    show()if __name__ == '__main__':    process1()    process2()    process3()    process4()    process5()

(3)结果展示

a.如图1所示，显示视图与图像点、基础矩阵、17，实现三维坐标的可视化

三、

二、

输出基础矩阵、