四、【python计算机视觉编程】照相机模型与增强现实

（一）针孔照相机模型

针孔照相机模型（有时称为射影照相机模型）是计算机视觉中广泛使用的照相机模型。对于大多数应用来说，针孔照相机模型简单，并且具有足够的精确度。这个名字源于一种类似暗箱机的照相机，该照相机从一个小孔采集射到暗箱内部的光线，如图所示。

比较基础简单的投影变换有正交变换和透视变换。正交变换就是物体上的点全都平行地投射到投影面，没有远近的区别，即没有透视效果。 透视变换正好相反，被投影物体处于一个四棱台区域中，物体被投影到离相机较近的平面上。相机被抽象为一个点，而投影点是物体上的点和相机的连线与投影平面的交点。由于投影的路径不再相互平行，因此会产生透视效果。

针孔照相机数学模型：
首先，描述了基本的小孔成像过程：

图中，X轴是针孔所在坐标系，Y轴为成像平面坐标系，P为空间一点，小孔成像使得P点在图像平面上呈现了一个倒立的像。

这幅图是前一幅图的俯视图，由三角相似关系可以得到：
$$\frac{-y_1}{f}=\frac{x_1}{x_3}$$ $$\frac{-y_2}{f}=\frac{x_2}{x_3}$$
写成矩阵的形式：
$$\left(\begin{array}{c}{y}_1\\ y_2\end{array}\right)=-\frac{f}{x_3}\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)$$
为了简化公式除去负号，把成像平面移到了和物体（P点）相同的一边，这样相似关系中就没有负号。

上图中$Y{1}^{\prime }$为移动后的成像平面，这与移动前的比例关系是等效的。

$$\frac{y_1}{f}=\frac{x_1}{x_3}$$ $$\frac{y_2}{f}=\frac{x_2}{x_3}$$
写成矩阵形式：
$$\left(\begin{array}{c}{y}_1\\ y_2\end{array}\right)=\frac{f}{x_3}\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)$$

（1）照相机矩阵

照相机矩阵可分解为：$P=K[R|t]$，其中R是描述照相机方向的旋转矩阵，t是描述照相机中心位置的三维平移向量，内标定矩阵K描述照相机的投影性质。
标定矩阵仅和照相机自身的情况相关，可以写成如下形式：
$$K=\left[\begin{array}{ccc}\alpha f& s& c_x\\ 0& f& c_y\\ 0& 0& 1\end{array}\right]$$平面和照相机中心间的距离为焦距f。当像素数组在传感器上偏斜的时候，需要用到倾斜参数s。在大多数情况下，s可以设置为0，那样上述公式可以写成：
$$K=\left[\begin{array}{ccc}\alpha f& 0& c_x\\ 0& f& c_y\\ 0& 0& 1\end{array}\right]$$ 其中，$f_x=\alpha f_y$。纵横比例参数α是在像素元素非正方形的情况下使用的。通常情况下，可以默认α=1，则标定矩阵写成如下形式：
$$K=\left[\begin{array}{ccc}f& 0& c_x\\ 0& f& c_y\\ 0& 0& 1\end{array}\right]$$ 除焦距之外，标定矩阵中剩余的唯一参数为光心的坐标$c=[c_x,c_y]$，也就是光线坐标轴和图像平面的交点。因为光心通常在图像的中心，并且图像的坐标是从左上角开始计算的，所以光心得坐标常接近于图像宽度和高度的一半。

（2）三维点的投影

首先创建照相机类，用来处理对照相机和投影建模所需要的全部操作代码：

from scipy import linalg

class Camera(object):
    """表示针孔照相机的类"""
    
    def __init__(self,P):   #注意：需要左右各需要两个下划线
        """初始化P=K[R|t]照相机模型"""
        self.P = P
        self.K = None   #标定矩阵
        self.R = None   #旋转
        self.t = None   #平移
        self.c = None   #照相机中心
        
    def project(self,X):
        """X（4*n的数组）的投影点，并且进行坐标归一化"""
        
        x = dot(self.P,X)
        for i in range(3):
            x[i]/=x[2]
        return x

     #该函数是一种矩阵因子分解方法，称为RQ因子分解。其结果不是唯一的，结果存在符号二义性。
    def factor(self):
        """将照相机矩阵分解为K、R、t，其中，P=K[R|t]"""
        
        #分解前3*3的部分
        K,R = linalg.rq(self.P[:,:,3])
        
        #将K的对角线元素设为正值
        T = diag(sign(diag(K)))
        if linalg.det(T) < 0:
            T[1,1]*=-1
        self.K = dot(K,T)
        self.R = dot(T,R)  #T的逆矩阵为其自身
        self.t = dot(linalg.inv(self.K),self.P[:,3])
        
        return self.K,self.R,self.t
    #计算照相机的中心
    def center(self):
        """计算并返回照相机的中心"""
        
        if self.c is not None:
            return self.c
        else:
            #通过因子分解计算c
            self.factor()
            self.c = -dot(self.R.T,self.t)
            return self.c

编写实验代码：

# -*- coding: utf-8 -*-

from PIL import Image
from numpy import *
from pylab import *
import sys
import camera
import matplotlib 
import matplotlib.pyplot as plt
from matplotlib.font_manager import FontProperties
font = FontProperties(fname=r"c:\windows\fonts\simsun.ttc", size=14) 

#im = np.array(Image.open('house.000.pgm').convert('L'))

points = loadtxt('house.p3d').T
points = vstack((points,ones(points.shape[1])))
P = np.hstack((np.eye(3),np.array(([0],[0],[-10]))))

figure()
subplot(121)
plot(x[0],x[1],'*')
show()


#创建变换
r = 0.05*random.rand(3)
rot = camera.rotation_matrix(r)

cam = camera.Camera(P)
x = cam.project(points)

#绘制投影
#subplot(131)
#imshow(im)

for t in range(20):
    cam.P = np.dot(cam.P,rot)
    x = cam.project(points)
    subplot(122)
    plot(x[0],x[1],'+')
    # 注意对于旋转，t向量是保持不变的
    show()

效果图：（原3D图）

（图像视图中投影后的点）

发现很奇怪的是，绘制的图中含有“方框”，因此搜索了一下原因：是因为“负号”没办法显示。下面给出两种方案：

1. 在配置区加入：plt.rcParams['axes.unicode_minus'] = False
2. 在F:\Anaconda\Lib\site-packages\matplotlib\mpl-data\matplotlibrc中，改成如下即可。

#axes.unicode_minus  : True      改成→    axes.unicode_minus  : False

（3）照相机矩阵的分解

针孔相机模型是基于透视变换的相机模型。公式为：
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\cdot \left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$简化公式为：$s\cdot m^{\prime }=A\cdot [R|t]\cdot M^{\prime }$
其中$m^{\prime }$为屏幕uv坐标，$A$为相机内参，$[R|t]$为相机外参，$M^{\prime }$为物体世界坐标，而$s$为物体在相机坐标系中的z坐标，这是公式推导过程中产生的参数。这里提到的相机内参是指仅由相机本身决定的参数，也就是对某一相机一旦这个值算好就不用再次进行计算。相对的，外参是和世界坐标系和相机位置有关的。

照相机模型是属于计算机3D视觉中的，而所谓3D视觉就是要建立二维图像和三维场景的联系，比较主要的一个用途是根据二维图片重建三维场景。所以一个自然的想法就是通过各种方式使得可以从三维坐标计算得到屏幕空间坐标或者相反。

公式推导过程：
首先，将世界中的物体映射到屏幕上的整个过程，涉及四个坐标系。第一是世界坐标系，而照相机本身也是世界中的物体，因此在世界坐标系中也有一个位置。还需要一个相对于照相机位置不变的相机坐标系。这个坐标系的原点设在抽象出的针孔相机处，z轴为光轴。而照相机的投影平面存在两个坐标系。投影平面是和照相机相隔焦距距离并垂直于光轴的平面，一般位于z轴的正方向。这两个坐标系最大的区别是一个像前两个坐标系一样使用浮点数，其原点位于光轴和和投影平面的交点处，x轴、y轴与相机坐标系对应轴平行，如图所示。

将三维空间物体投影到这样一个平面坐标系上是很直观的，但是对于计算机的显示来说，不但要求位置以像素为单位表示，而且最好值全是正数，才符合编程的需要。因此就产生了第四个坐标系——uv坐标系（像素坐标系）。这个坐标系的原点在显示屏幕的一个角上，使得显示部分全落在第一象限，同时坐标值是以像素为单位的正数。uv坐标和平面浮点坐标关系如图所示。

要注意下相机坐标系部分模型和实际相机的关系。实际相机的朝向在相机坐标系中应当是朝向负z方向，也是景物真实所在的方向。

世界坐标系→相机坐标系，这部分使用图形学中基础的旋转平移矩阵的推导：
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M_1\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$其中，带有下标w的代表世界坐标系，而带有下标c的表示相机坐标系，R、t分别表示旋转与平移矩阵。

相机坐标系→投影平面， 使用的是相似三角形的原理。
由图像坐标系与相机坐标系的转化为：$x=\frac{f{X}_c}{Z_c}$ $y=\frac{f{Y}_c}{Z_c}$

用齐次坐标系和矩阵表示上述关系：
$$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$$

投影平面上的坐标系转换，使用浮点数作为单位的坐标系，在此称为浮点平面坐标系。其中，浮点平面坐标系在uv坐标系中的位置是（u₀, v₀）。
由$u=\frac{x}{dx}+u_0$ 和 $v=\frac{y}{dy}+v_0$得到：

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

uv坐标系→世界坐标系：
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{a}_x& 0& u_0& 0\\ 0& a_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=K{M}_1\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& m_{14}\\ m_{21}& m_{22}& m_{23}& m_{24}\\ m_{31}& m_{32}& m_{33}& m_{34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

其中，a_x, a_y分别是图像水平轴和垂直轴的尺度因子。K的参数中只包含焦距、主点坐标等只由相机的内部结构决定，因此称 K 为内部参数矩阵，ax, ay , u0, v0叫做内部参数。M1中包含的旋转矩阵和平移向量是由相机坐标系相对于世界坐标系的位置决定的，因此称M₁为相机的外部参数矩阵，R和t叫做外部参数，M 叫投影矩阵。相机标定就是确定相机的内部参数和外部参数。

补充知识：

1. 内参矩阵（Intrinsic matrix）
   内参矩阵反应了相机自身的属性，各个相机是一不一样的，需要标定才能知道这些参数。图像的点的表示是长度单位，不是像素，由于通常使用的图像是以像素来衡量的，因此还需要将图像坐标系转化为像素坐标系。
2. 外参矩阵（ extrinsic matrix）
   外参矩阵是世界坐标系到相机坐标系的变换。 既然是变换，我们就能够将其表达为$[Rt]$,齐次形式为：
   
   外参矩阵是世界坐标系在相机坐标系下的描述，或者将世界坐标系下的点转换到相机坐标系下。
   也有书上写道： 摄像机的外参数模型，是景物坐标系在摄像机坐标中的描述。

编写代码：

# -*- coding: utf-8 -*-

from PIL import Image
from numpy import *
from pylab import *
import camera

K = array([[1000,0,500],[0,1000,300],[0,0,1]])
tmp = camera.rotation_matrix([0,0,1])[:3,:3]
Rt = hstack((tmp,array([[50],[40],[30]])))
cam = camera.Camera(dot(K,Rt))

print(K,Rt)
print(cam.factor())

实验结果：

需要将K中对角元素设置为大于0，因此在使用linalg.rq函数以后，再使用diag函数获得K对角元素构成的向量，随后通过sign函数返回三个对角元素符号判断（大于0为1，否则为0），再调用diag生成一个对角矩阵，最终使用det计算行列式的值从而完成判断。

（4）计算照相机中心

给定照相机投影矩阵$P$，可以计算出空间上照相机所在位置。照相机的中心C是一个三维点，满足约束$PC=0$。对于投影矩阵为$P=K[R|t]$的照相机，有：
$$K[R|t]C=KRC+Kt=0$$
照相机的中心可以由下述式子来计算：
$$C=-R^{T}t$$，照相机的中心和内标定矩阵$K$无关。

编写代码用来计算照相机的中心：

#计算照相机的中心
    def center(self):
        """计算并返回照相机的中心"""
        
        if self.c is not None:
            return self.c
        else:
            #通过因子分解计算c
            self.factor()
            self.c = -dot(self.R.T,self.t)
            return self.c

（二）照相机标定

简单来说就是从世界坐标系换到图像坐标系的过程，也就是求最终的投影矩阵 $P$ 的过程。
原理：
一般来说，标定的过程分为两个部分：

1. 从世界坐标系转换为相机坐标系，这一步是三维点到三维点的转换，包括 $R$，$t$ （相机外参）等参数。
2. 从相机坐标系转为图像坐标系，这一步是三维点到二维点的转换，包括 $K$（相机内参）等参数。

详见（一）的内容。

步骤：

1. 打印一张棋盘格，把它贴在一个平面上，作为标定物。
2. 通过调整标定物或摄像机的方向，为标定物拍摄一些不同方向的照片。
3. 从照片中提取棋盘格角点。
4. 估算理想无畸变的情况下，五个内参和六个外参。
5. 应用最小二乘法估算实际存在径向畸变下的畸变系数。
6. 极大似然估计法，优化估计，提升估计精度。

一个简单的标定方法：
需要准备一个平面矩形的标定物体（一本书即可）、用于测量的卷尺或直尺，以及一个平面。具体操作步骤如下：

1. 测量你选定矩形标定物体的边长dX和dY；
2. 将照相机和标定物体放置在平面上，使得照相机的背面和标定物体平行，同时物体位于照相机图像视图的中心，你可能需要调整照相机或者物体来获得良好的对齐效果；
3. 测量标定物体到照相机的距离dZ；
4. 拍摄一幅图像来检验该设置是否正确，即标定物体的边要和图像的行和列对齐；
5. 使用像素数来测量标定物体图像的宽度和高度dx和dy。

为什么需要照相机标定呢？
这是由于每个镜头的畸变程度各不相同，通过照相机标定可以校正这种镜头畸变。其实可以认为用这种标定的方式来求解照相机内参和畸变参数，相当于一种照相机校准，然后这些参数就可以用于后面的求解。例如求解新拍的两幅图片相对的 $R$ 和 $t$，求解这个外参用到就是标定得到的相机内参和畸变参数。

畸变参数：
在几何光学和阴极射线管(CRT)显示中，畸变（distortion） 是对直线投影（rectilinear projection）的一种偏移。 简单来说直线投影是场景内的一条直线投影到图片上也保持为一条直线。那畸变简单来说就是一条直线投影到图片上不能保持为一条直线了，这是一种光学畸变（optical aberration）。
畸变一般可以分为两大类，包括径向畸变 和切向畸变。主要的一般径向畸变有时也会有轻微的切向畸变。

径向畸变 的效应有三种，一种是桶形畸变（barrel distortion），另一种是枕形畸变（pincushion distortion），还有一种是两种的结合叫做胡子畸变（mustache distortion），从图片中可以很容易看出区别，具体见下图(图片来自wikipedia)：

切向畸变是由于透镜与成像平面不严格的平行，来自于整个摄像机的组装过程。这种畸变可分为：薄透镜畸变、离心畸变。

畸变还有其他类型的畸变，但是没有径向畸变、切向畸变显著。

（三）以平面和标记物进行姿态估计

下面使用一个例子来演示如何进行姿态估计。

编写实验代码：

# -*- coding: utf-8 -*-
# -*- coding: utf-8 -*-

from pylab import *
from PIL import Image
from numpy import *
# If you have PCV installed, these imports should work
from PCV.geometry import homography, camera
import sift

def my_calibration(sz):
    row, col = sz
    fx = 2555 * col / 2592
    fy = 2586 * row / 1936
    K = diag([fx, fy, 1])
    K[0, 2] = 0.5 * col
    K[1, 2] = 0.5 * row
    return K

def cube_points(c, wid):
    """ Creates a list of points for plotting
        a cube with plot. (the first 5 points are
        the bottom square, some sides repeated). """
    p = []
    # bottom
    p.append([c[0] - wid, c[1] - wid, c[2] - wid])
    p.append([c[0] - wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] - wid, c[2] - wid])
    p.append([c[0] - wid, c[1] - wid, c[2] - wid])  # same as first to close plot

    # top
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])  # same as first to close plot

    # vertical sides
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] - wid])

    return array(p).T



#  计算特征
sift.process_image('picture/book1/book_frontal.jpg', 'im0.sift')
l0, d0 = sift.read_features_from_file('im0.sift')

sift.process_image('picture/book1/book_perspective.jpg', 'im1.sift')
l1, d1 = sift.read_features_from_file('im1.sift')


#  匹配特征并计算单应性矩阵
matches = sift.match_twosided(d0, d1)
ndx = matches.nonzero()[0]
fp = homography.make_homog(l0[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
tp = homography.make_homog(l1[ndx2, :2].T)

model = homography.RansacModel()
H, inliers = homography.H_from_ransac(fp, tp, model)

# 计算照相机标定矩阵
K = my_calibration((400, 300))

# 位于边长为0.2 z=0平面的三维点
box = cube_points([0, 0, 0.1], 0.1)

# 投影第一幅图像上底部的正方形
cam1 = camera.Camera(hstack((K, dot(K, array([[0], [0], [-1]])))))
# 底部正方形上的点
box_cam1 = cam1.project(homography.make_homog(box[:, :5]))


# 使用H将点变换到第二幅图像中
box_trans = homography.normalize(dot(H,box_cam1))

# 从cam1和H中计算第二个照相机矩阵
cam2 = camera.Camera(dot(H, cam1.P))
A = dot(linalg.inv(K), cam2.P[:, :3])
A = array([A[:, 0], A[:, 1], cross(A[:, 0], A[:, 1])]).T
cam2.P[:, :3] = dot(K, A)

# 使用第二个照相机矩阵投影
box_cam2 = cam2.project(homography.make_homog(box))



# plotting
im0 = array(Image.open('picture/book/book_frontal.jpg'))
im1 = array(Image.open('picture/book/book_perspective.jpg'))

figure()
imshow(im0)
plot(box_cam1[0, :], box_cam1[1, :], linewidth=3)
title('2D projection of bottom square')
axis('off')

figure()
imshow(im1)
plot(box_trans[0, :], box_trans[1, :], linewidth=3)
title('2D projection transfered with H')
axis('off')

figure()
imshow(im1)
plot(box_cam2[0, :], box_cam2[1, :], linewidth=3)
title('3D points projected in second image')
axis('off')

show()

效果图：

分析：
图中的方框基本能还原出照相机拍摄的角度及位置，只要注意遇到的问题，上面的代码基本可以实现。上面估计方法是，使用平面物体作为标记物，来计算用于新视图投影矩阵。将图像的特征和对齐后的标记匹配，计算出单应性矩阵，然后用于计算照相机的姿态。

运行代码中遇到的问题及解决方案：

这个报错的意思是没有找到该文件，询问了同学才知道，这是因为之前没有正确获取.sift，以下是我找到的解决方案：
在这个网站里 http://www.vlfeat.org/ 下载VLFeat 0.9.20工具包。

具体操作可以参考：http://www.itkeyword.com/doc/6844809060106985x122/python-sift

需要注意的是，
这个路径需要设置好，把这个sift.py文件放在你的当前目录下，修改这个cmmd这个路径！如果win64不行，可以尝试使用win32\sift.py来操作。

（四）增强现实 （AR）

增强现实是将物体和相应信息放置在图像数据上的一系列操作的总称。下面需要用到两个工具包，分别是：PyGame和PyOpenGL。

（1）PyGame和PyOpenGL

pygame可直接使用pip install命令安装，pip install PyGame 。

但PyOpenGL默认安装32位，我们要手动下载64位下载地址然后在下载位置使用pip install （文件名.whl）进行安装。pip install PyOpenGL-3.1.3b2-cp37-cp37m-win_amd64.whl 进行下载，这个根据自己的情况来进行调整。我所使用的电脑是win10，64位，使用的版本是Python3.7。

需要注意的是， 为了使用上面两个工具包，需要在脚本的开始部分载入下面的命令：

from OpenGL.GL import *
from OpenGL.GLU import *
import pygame, pygame.image
from pygame.locals import *

（2）从照相机矩阵到OpenGL格式

openGL使用4×4的矩阵来表示变换。但是照相机与场景的变换分成了两个矩阵，GL_PROJECTION矩阵和GL_MODELVIEW矩阵。GL_PROJECTION矩阵处理图像成像的性质，等价于我们的内标定矩阵$K$。GL_MODELVIEW矩阵处理物体和照相机之间的三维变换关系，对应于我们照相机矩阵中的$R$和$t$部分。

使用函数将照相机参数转换为openGL中的投影矩阵：

def set_projection_from_camera(K):
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    fx = K[0, 0]
    fy = K[1, 1]
    fovy = 2 * math.atan(0.5 * height / fy) * 180 / math.pi
    aspect = (width * fy) / (height * fx)
    near = 0.1
    far = 100.0
    gluPerspective(fovy, aspect, near, far)
    glViewport(0, 0, width, height)

模拟视图矩阵可以表示相对的旋转和平移，该变换将该物体放置在照相机前。模拟视图矩阵是个典型的4×4矩阵，如下所示：
$$\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

使用函数实现如何获得移除标定矩阵后的3×4针孔照相机矩阵，并创建一个模拟视图：

def set_modelview_from_camera(Rt):
    """从照相机姿态中获得模拟视图矩阵"""
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    Rx = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])  #围绕x轴将茶壶旋转90度，使z轴向上
    R = Rt[:, :3]  #获得旋转的最佳逼近
    U, S, V = np.linalg.svd(R)
    R = np.dot(U, V)
    R[0, :] = -R[0, :]  #改变x轴的符号
    t = Rt[:, 3]  #获得平移量
    M = np.eye(4) #获得4*4的模拟视图矩阵
    M[:3, :3] = np.dot(R, Rx)
    M[:3, 3] = t
    M = M.T  #转置并压平以获得列序数值
    m = M.flatten()
    glLoadMatrixf(m)  #将模拟视图矩阵替换为新的矩阵

（3）在图像中放置虚拟物体

在OpenGL中，该操作可以通过创建一个四边形的方式来完成，该四边形为整个视图。完成该操作最简单的方式是绘制出四边形，同时将投影和模拟视图矩阵重置，使得每一维的坐标范围在-1到1之间。

使用函数载入一幅图像，然后将其转换成一个OpenGL纹理，并将该纹理放置在四边形上：

def draw_background(imname):
    """使用四边形绘制背景图像"""
    
    #载入背景图像，转为OpenGL纹理
    bg_image = pygame.image.load(imname).convert()
    bg_data = pygame.image.tostring(bg_image, "RGBX", 1)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    #绑定纹理
    glEnable(GL_TEXTURE_2D)
    glBindTexture(GL_TEXTURE_2D, glGenTextures(1))
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, bg_data)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST)
    #创建四方形填充整个窗口
    glBegin(GL_QUADS)
    glTexCoord2f(0.0, 0.0);
    glVertex3f(-1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 0.0);
    glVertex3f(1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 1.0);
    glVertex3f(1.0, 1.0, -1.0)
    glTexCoord2f(0.0, 1.0);
    glVertex3f(-1.0, 1.0, -1.0)
    glEnd()
    #清除纹理
    glDeleteTextures(1)

（4）综合集成

编写实验代码：

import math
import pickle
from pylab import *
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
import pygame, pygame.image
from pygame.locals import *
from PCV.geometry import homography, camera
from PCV.localdescriptors import sift


def cube_points(c, wid):
    """ Creates a list of points for plotting
        a cube with plot. (the first 5 points are
        the bottom square, some sides repeated). """
    p = []
    # bottom
    p.append([c[0] - wid, c[1] - wid, c[2] - wid])
    p.append([c[0] - wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] - wid, c[2] - wid])
    p.append([c[0] - wid, c[1] - wid, c[2] - wid])  # same as first to close plot

    # top
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])  # same as first to close plot

    # vertical sides
    p.append([c[0] - wid, c[1] - wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] + wid])
    p.append([c[0] - wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] - wid])
    p.append([c[0] + wid, c[1] + wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] + wid])
    p.append([c[0] + wid, c[1] - wid, c[2] - wid])

    return array(p).T


def my_calibration(sz):
    row, col = sz
    fx = 2555 * col / 2592
    fy = 2586 * row / 1936
    K = diag([fx, fy, 1])
    K[0, 2] = 0.5 * col
    K[1, 2] = 0.5 * row
    return K


def set_projection_from_camera(K):
    """从照相机标定矩阵中获得视图"""
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    fx = K[0, 0]
    fy = K[1, 1]
    fovy = 2 * math.atan(0.5 * height / fy) * 180 / math.pi
    aspect = (width * fy) / (height * fx)
    near = 0.1  #定义近的和远的裁剪平面
    far = 100.0
    gluPerspective(fovy, aspect, near, far)  #设定透视
    glViewport(0, 0, width, height)


def set_modelview_from_camera(Rt):
    """从照相机姿态中获得模拟视图矩阵"""
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    Rx = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])  #围绕x轴将茶壶旋转90度，使z轴向上
    R = Rt[:, :3]  #获得旋转的最佳逼近
    U, S, V = np.linalg.svd(R)
    R = np.dot(U, V)
    R[0, :] = -R[0, :]  #改变x轴的符号
    t = Rt[:, 3]  #获得平移量
    M = np.eye(4) #获得4*4的模拟视图矩阵
    M[:3, :3] = np.dot(R, Rx)
    M[:3, 3] = t
    M = M.T  #转置并压平以获得列序数值
    m = M.flatten()
    glLoadMatrixf(m)  #将模拟视图矩阵替换为新的矩阵


def draw_background(imname):
    """使用四边形绘制背景图像"""
    
    #载入背景图像，转为OpenGL纹理
    bg_image = pygame.image.load(imname).convert()
    bg_data = pygame.image.tostring(bg_image, "RGBX", 1)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    #绑定纹理
    glEnable(GL_TEXTURE_2D)
    glBindTexture(GL_TEXTURE_2D, glGenTextures(1))
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, bg_data)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST)
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST)
    #创建四方形填充整个窗口
    glBegin(GL_QUADS)
    glTexCoord2f(0.0, 0.0);
    glVertex3f(-1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 0.0);
    glVertex3f(1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 1.0);
    glVertex3f(1.0, 1.0, -1.0)
    glTexCoord2f(0.0, 1.0);
    glVertex3f(-1.0, 1.0, -1.0)
    glEnd()
    #清除纹理
    glDeleteTextures(1)


def draw_teapot(size):
    """在原点处绘制红色茶壶"""
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glEnable(GL_DEPTH_TEST)
    glClear(GL_DEPTH_BUFFER_BIT)
    
    #绘制红色茶壶
    glMaterialfv(GL_FRONT, GL_AMBIENT, [0, 0, 0, 0])
    glMaterialfv(GL_FRONT, GL_DIFFUSE, [0.0, 0.5, 0.5, 0.0])
    glMaterialfv(GL_FRONT, GL_SPECULAR, [0.7, 0.6, 0.6, 0.0])
    glMaterialf(GL_FRONT, GL_SHININESS, 0.25 * 128.0)
    glutSolidTeapot(size)

width, height = 400,300


def setup():
    """设置窗口和pygame环境"""
    pygame.init()
    pygame.display.set_mode((width, height), OPENGL | DOUBLEBUF)
    pygame.display.set_caption("OpenGL AR demo")


# compute features
sift.process_image('picture/book1/book_frontal.jpg', 'im0.sift')
l0, d0 = sift.read_features_from_file('im0.sift')

sift.process_image('picture/book1/book_perspective.jpg', 'im1.sift')
l1, d1 = sift.read_features_from_file('im1.sift')

# match features and estimate homography
matches = sift.match_twosided(d0, d1)
ndx = matches.nonzero()[0]
fp = homography.make_homog(l0[ndx, :2].T)
ndx2 = [int(matches[i]) for i in ndx]
tp = homography.make_homog(l1[ndx2, :2].T)

model = homography.RansacModel()
H, inliers = homography.H_from_ransac(fp, tp, model)

K = my_calibration((400,300))
cam1 = camera.Camera(hstack((K, dot(K, array([[0], [0], [-1]])))))
box = cube_points([0, 0, 0.1], 0.1)
box_cam1 = cam1.project(homography.make_homog(box[:, :5]))
box_trans = homography.normalize(dot(H, box_cam1))
cam2 = camera.Camera(dot(H, cam1.P))
A = dot(linalg.inv(K), cam2.P[:, :3])
A = array([A[:, 0], A[:, 1], cross(A[:, 0], A[:, 1])]).T
cam2.P[:, :3] = dot(K, A)

Rt = dot(linalg.inv(K), cam2.P)

setup()
draw_background('picture/book1/book_perspective.jpg')
set_projection_from_camera(K)
set_modelview_from_camera(Rt)
draw_teapot(0.05)



while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            sys.exit()
pygame.display.flip()  #会在屏幕中绘制该物体

效果图：

遇到的问题：
1.

2.未能实现在图像上显示红色茶壶。

解决方案：
1.这个错误是freeglut和glut共存的缘故，它们都定义了相同的方法，这个是动态链接库的重叠问题，找到你使用的python路径下\OpenGL\DLLS中的glut64.vcX.dll文件，将其余文件删除就可以了。vcX中的X因不同版本的OpenGL而不同。留下方框中的即可。文件的路径是：D:\Program Files\Python\Python36\Lib\site-packages\OpenGL\DLLS