plot.py

from __future__ import division
import numpy as np
import matplotlib.pyplot as plt
from skimage import io, img_as_float

A = img_as_float(io.imread('lego1.jpg', plugin='pil'))

# noise
n = np.random.random_sample()

# 2D image coordinates with noise added
x = [1545+n, 1614+n, 1746+n, 1872+n, 1992+n, 2112+n, 2220+n, 2328+n, 2427+n, 1308+n, 1071+n, 843+n, 621+n, 408+n, 195+n, 1548+n, 1548+n, 1551+n, 1551+n, 1551+n, 1554+n, 1554+n, 1557+n, 1557+n, 1560+n, 1560+n, 1563+n, 1566+n]
y = [1842+n, 1806+n, 1737+n, 1668+n, 1605+n, 1542+n, 1482+n, 1428+n, 1368+n, 1794+n, 1746+n, 1701+n, 1659+n, 1614+n, 1572+n, 1761+n, 1680+n, 1599+n, 1518+n, 1434+n, 1353+n, 1296+n, 1185+n, 1104+n, 1020+n, 936+n, 852+n, 768+n]

# 3D world coordinates in mm as columns of C
C = np.array([[0, 16, 48, 80, 112, 144, 176, 208, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 64, 96, 128, 160, 192, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9.6, 19.2, 28.8, 38.4, 48, 57.6, 67.2, 76.8, 86.4, 96, 105.6, 115.2, 124.8], [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]])

# Compute the camera matrix P
M = np.zeros((56, 12))
for s in range(0, 28, 2):
    M[s, :] = np.array([0, 0, 0, 0, -C[0, s], -C[1, s], -C[2, s], -C[3, 0], y[s]*C[0, s], y[s]*C[1, s], y[s]*C[2, s], y[s]*C[3, s]]) 
    M[s+1, :] = np.array([C[0, s], C[1, s], C[2, s], C[3, 0], 0, 0, 0, 0, -x[s]*C[0, s], -x[s]*C[1, s], -x[s]*C[2, s], -x[s]*C[3, s]])


U, S, V = np.linalg.svd(M)
P = V.T[:, -1]
P = np.reshape(P, (3, 4))

def decomposeP(P):
        W = np.zeros((3, 3))
        W[0, 2], W[1, 1], W[2, 0] = 1, 1, 1
        
        Qt, Rt = np.linalg.qr(np.transpose(np.dot(W, P[:, 0:3]))) 
        K = np.dot(W, np.dot(np.transpose(Rt), W))
        R = np.dot(W, np.transpose(Qt))
        
        D = np.identity(3)
        if K[0, 0] < 0:
            D[0, 0] = -1
        if K[1, 1] < 0:
            D[1, 1] = -1
        if K[2, 2] < 0:
            D[2, 2] = -1
        K = np.dot(K, D)    
        R = np.dot(D, R)
        
        c = np.dot(np.dot(-np.transpose(R), np.linalg.inv(K)), P[:, 3])  

        return K/K[2,2], R, c

K, R, c = decomposeP(P)

x0 = K[0, 2]
y0 = K[1, 2]
print x0, y0

x = [-1416.16625586, -1416.11894167, -1416.28780727, -1416.780884, -1415.8373409, -1416.26465353, -1416.3405661, -1416.01184316, -1415.93881488, -1416.003185]
y = [1179.44908385, 1179.49687444, 1179.32630825, 1178.8282635, 1179.78131059, 1179.34969477, 1179.27301787, 1179.60505139, 1179.67881433, 1179.61379664]

plt.plot(x, y, 'b.')
plt.axis('equal')
plt.show()