相机矩阵

from scipy import linalg
import numpy as np
import matplotlib.pyplot as plt
class Camera(object):
    def __init__(self,P):
        self.P=P
        self.K=None#标定矩阵
        self.R=None#照相机旋转
        self.t=None#照相机平移
        self.c=None#照相机中心 
    def project(self,X):
        x=np.dot(self.P,X)
        for i in range(3):
            x[i]/=x[2]
        return x
    def rotationMatrix(self,a):
        R=np.eye(4)
        R[:3,:3]=linalg.expm([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
        return R
    def factor(self):
        K,R=linalg.rq(self.P[:,:3])#分解前半部分   
        T=np.diag(np.sign(np.diag(K)))#将K的对角线元素设为正值
        if(linalg.det(T)<0):
            T[1,1]*=-1
        self.K=np.dot(K,T)
        self.R=np.dot(T,R)
        self.t=np.dot(linalg.inv(self.K),self.P[:,3])
        return self.K,self.R,self.t
if __name__=="__main__":
    points=np.loadtxt('C:/Users/xpp/Desktop/3D/bt.p3d')#载入点
    points=points.swapaxes(0,1)#交换维度以读取   
    points=np.vstack((points,np.ones(points.shape[1])))#齐次坐标# 齐次坐标    
    P=np.hstack((np.eye(3),np.array([[0],[0],[-10]])))#设置照相机参数
    cam=Camera(P)
    x=cam.project(points)
    #绘制投影
    plt.figure()
    plt.plot(x[0],x[1],'k.',color='pink')
    plt.show()
    #创建变换
    r=0.05*np.random.random(3)
    rot=cam.rotationMatrix(r)
    #旋转矩阵和投影
    plt.figure()
    for t in range(20):
        cam.P=np.dot(cam.P,rot)
        x=cam.project(points)
        plt.plot(x[0],x[1],'k.',color='pink')
    plt.show()
    #测试函数
    K=np.array([[1000,1000,300],[0,1000,300],[0,0,1]])
    tmp=cam.rotationMatrix([0,0,1])[:3,:3]
    Rt=np.hstack((tmp,np.array([[50],[40],[30]])))
    cam=Camera(np.dot(K,Rt))
    print(K,Rt)
    print(cam.factor())