由三维点计算照相机矩阵

代码

from PIL import Image
import numpy as np
import camera
import sfm
import camera
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

im1 = np.array(Image.open('images/001.jpg'))
im2 = np.array(Image.open('images/002.jpg'))

points2D = [np.loadtxt('2D/00' + str(i + 1) + '.corners').T for i in range(3)]

points3D = np.loadtxt('3D/p3d').T

corr = np.genfromtxt('2D/nview-corners',dtype='int',missing_values='*')

P = [camera.Camera(np.loadtxt('2D/00'+str(i+1)+'.P')) for i in range(3)]


corr = corr[:,0]
ndx3D = np.where(corr>=0)[0]
ndx2D = corr[ndx3D]

x = points2D[0][:,ndx2D]
x = np.vstack( (x,np.ones(x.shape[1])) )
X = points3D[:,ndx3D]
X = np.vstack( (X,np.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)

plt.figure()
plt.imshow(im1)
plt.plot(x[0],x[1],'bo')
plt.plot(xest[0],xest[1],'r')
plt.axis('off')

plt.show()

import numpy as np

def compute_P(x, X):
    """    计算照相机矩阵，根据二维和三维点之间的对应关系
           坐标为齐次坐标
    """
    n = x.shape[1]
    if X.shape[1] != n:
        raise ValueError("Number of points don't match.")

    # create matrix for DLT solution
    M = np.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 = np.linalg.svd(M)

    return V[-1, :12].reshape((3, 4))

A = np.zeros((23,3),dtype=float)
f = open('2Ddata.txt')
lines = f.readlines()
A_row = 0
for line in lines:
    list = line.strip('\n').split(' ')
    A[A_row][0] = float(list[0])
    A[A_row][1] = float(list[1])
    A[A_row][2] = float(list[2])
    A_row+=1
print(A)
A=np.transpose(A)


B = np.zeros((23,4),dtype=float)
f = open('3Ddata.txt')
lines = f.readlines()
B_row = 0
for line in lines:
    list = line.strip('\n').split(' ')
    B[B_row][0] = float(list[0])
    B[B_row][1] = float(list[1])
    B[B_row][2] = float(list[2])
    B[B_row][3] = float(1.00000)
    B_row+=1
print(B)
B = np.transpose(B)


print(A.shape)
print(B.shape)
print(compute_P(A,B))