旋转投影

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
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(100):
        cam.P=np.dot(cam.P,rot)
        x=cam.project(points)
        plt.plot(x[0],x[1],'k.',color='pink')
        plt.show()