# load points
points = np.loadtxt('house.p3d').T
points = np.vstack((points, np.ones(points.shape[1])))
#%%
# setup camera
#P = hstack((eye(3),array([[0],[0],[-10]])))
cam = Camera()
## Test matrix functions
cam.set_K(1460, 1460, 608, 480)
cam.set_width_heigth(1280, 960) #TODO Yue
# cam.set_R(0.0, 0.0, 1.0, 0.0)
cam.set_t(0.0, 0.0, -8.0)
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(140.0)) #TODO Yue
cam.set_P()
print(cam.factor())
#%%
x = np.array(cam.project(points))
#%%
# plot projection
plt.figure()
plt.plot(x[0], x[1], 'k.')
plt.xlim(0, 1280)
plt.ylim(0, 960)
plt.show()
#%%
# create transformation
