Python计算机视觉编程学习笔记 四 照相机模型与增强现实

本章将尝试对照相机进行建模，并有效地使用这些模型。在之前的章节里，已经讲述了图像到图像之间的映射和变换。为了处理三维图像和平面图像之间的映射，还需要在映射中加入部分照相机产生图像过程的投影特性。本章学习如何确定照相机的参数，以及在具体应用中，如增强现实，如何使用图像间的投影变换。

（一）针孔照相机模型

1.1 照相机模型

相机将三维世界中的坐标点映射到二维图像平面的过程能够用一个几何模型进行描述。这个模型有很多种，其中最简单的称为针孔模型。
在现实生活中，针孔相机是由前方有 一个小洞（针孔）所构成。现实世界中源于某个物体的光线穿过此洞，会在摄像机的底板或图像平面上形成一幅倒立的图像。略为不便的是针孔相机的图像是倒置的。因此，我们换一种方式，对针孔前方图像平面的虚拟图像加以考虑。当然，从物理层面构造这样一种摄像机是不可能的但是在数学层面上这与真实的针孔模型是等价的（除了图像是倒置的以外）。

令成像平面为$\pi$
空间点0是投影中心，它到平面$\pi$的距离是f(焦距)。空间点M在平面$\pi$上的投影（或像）m是以点0为端点并经过点M的射线与平面$\pi$的交点。

摄像机并非总是位于世界坐标系的原点，通常我们可能想要定义一个任意的世界坐标系，这种坐标系可能使用于多个摄像机。为此，我们可以使用刚体变换描述世界坐标系到摄像机坐标系的转换。实际上，摄像机坐标系是未知的，更多的作用是用于数学模型描述。

坐标系约定

1、世界坐标系：Xw,Yw,Zw

2、摄像机坐标系：Xc,Yc,Zc

3、图像坐标系：$[u,v][x,y]$

说明：为了校正成像畸变，用理想图像坐标系$[Xu,Yu]$和真实图像坐标系$[Xd,Yd]$分别描述畸变前后的坐标关系。

世界坐标与相机坐标之间的转换关系：
平移向量t和旋转矩阵R可以用来表示相机坐标系与世界坐标系的关系。所以，假设空间点P在世界坐标系下的齐次坐标是$(Xw,Yw,Zw,1)T$，（这里T是上标转置），在相机坐标下的齐次坐标是$(Xc,Yc,Zc,1)T$,则存在如下的关系：
图像坐标系与相机坐标系变换关系：
照相机坐标系中的一点P在图像物理坐标系中像点P坐标为：

世界坐标系与图像坐标系变换关系：

1.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

处理效果：

总结：
使用齐次坐标来表示这些点。然后使用一个投影矩阵来创建 Camera对象将这些三维点投影到图像平面并执行绘制操作。

1.3照相机矩阵的分解

如果给定照相机矩阵 P，则需要恢复内参数 K 以及照相机的位置 t 和姿势 R。矩阵分块操作称为因子分解。这里，将使用一种矩阵因子分解的方法，称为 RQ 因子分解。

齐次坐标下，物体的物理坐标是$[x,y,z,1]\prime$的形式，图像上对应点的坐标是 $[u,v,1]\prime$的形式，所以相机矩阵 P作为把物体映射成像的矩阵，应该是一个 3×4 的矩阵。
因此，可以将相机矩阵分解为两个矩阵的乘积：内参矩阵KK和外参矩阵$[R\mid -RC]$ :
$$P=K[R\mid -RC]$$
其中，3∗33∗3的上三角阵$K$描述了相机的内参比如焦距；3∗33∗3的旋转矩阵$R$的列表示相机参考帧的世界坐标轴方向；向量$C$是世界坐标系中的相机中心。那么向量$t=-RC$就给出了相机坐标系中的世界原点位置。我们需要做的就是求解这些参数，当然前提是$P$已知。

其中：

1. 矩阵R是rotation矩阵，因此是正交的；K是上三角矩阵. 对P的前三列进行RQ分解就可得到KR

2. T=−RC, 是摄像机坐标系下世界坐标系原点的位置, tx,ty,tz分别代表世界坐标原点在相机的 左右，上下，前后
   位置关系。在三维重建中，一组标定好了的图片序列，它们各自的相机矩阵中的T应该是相同的。

3. 其中 K 就是内参，R,T 为外参。

将下面的方法添加到 Camera 类中：

    def factor(self):
        """    Factorize the camera matrix into K,R,t as P = K[R|t]. """

        # factor first 3*3 part
        K, R = linalg.rq(self.P[:, :3])

        # make diagonal of K positive
        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 is its own inverse
        self.t = dot(linalg.inv(self.K), self.P[:, 3])

        return self.K, self.R, self.t

RQ 因子分解的结果并不是唯一的。在该因子分解中，分解的结果存在符号二义性。 由于限制旋转矩阵 R 为正定的（否则，旋转坐标轴即可），所以如果需要， 则可以在求解到的结果中加入变换 T 来改变符号。

在示例照相机上运行下面的代码，观察照相机矩阵分解的效果：

import camera
from numpy import *
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()

显示：

1.4 计算照相机中心

相机中心的求解比较简单，利用分解前的相机矩阵，由于$P$的最后一列是由$-MC$得到的，而$M$在原始的相机矩阵的前3∗3部分已经给出了，所以只需要用$-M^{-1}$左乘它即可。
给定照相机投影矩阵 $P$，我们可以计算出空间上照相机的所在位置。照相机的中心 $C$，是一个三维点，满足约束$PC=0$。对于投影矩阵为 $P=K[R|t]$ 的照相机，有：
$$K[R|t]C=KRC+Kt$$
照相机的中心:
$$C=-R^T$$

可见，照相机的中心和内标定矩阵 K 无关。

按照上面公式计算照相机的中心，将其添加到 Camera 类中，返回照相机的中心：

    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. 测量你选定矩形标定物体的边长dX 和 dY；

2. 将照相机和标定物体放置在平面上，使得照相机的背面和标定物体平行，同时物体位于照相机图像视图的中心，你可能需要调整照相机或者物体来获得良好的对
   齐效果；

3. 测量标定物体到照相机的距离 dZ；

4. 拍摄一副图像来检验该设置是否正确，即标定物体的边要和图像的行和列对齐；

5. 使用像素数来测量标定物体图像的宽度和高度 dx和 dy

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

提取两幅图像的 SIFT 特征，然后使用 RANSAC 算法稳健地 估计单应性矩阵：
示例：

from PIL import Image
from numpy import *
from pylab import *
import os


def process_image(imagename, resultname, params="--edge-thresh 10 --peak-thresh 5"):
    """ Process an image and save the results in a file. """

    if imagename[-3:] != 'pgm':
        # create a pgm file
        im = Image.open(imagename).convert('L')
        im.save('tmp.pgm')
        imagename = 'tmp.pgm'

    cmmd = str("sift " + imagename + " --output=" + resultname +
               " " + params)
    os.system(cmmd)
    print 'processed', imagename, 'to', resultname


def read_features_from_file(filename):
    """ Read feature properties and return in matrix form. """

    f = loadtxt(filename)
    return f[:, :4], f[:, 4:]  # feature locations, descriptors


def write_features_to_file(filename, locs, desc):
    """ Save feature location and descriptor to file. """
    savetxt(filename, hstack((locs, desc)))


def plot_features(im, locs, circle=False):
    """ Show image with features. input: im (image as array),
        locs (row, col, scale, orientation of each feature). """

    def draw_circle(c, r):
        t = arange(0, 1.01, .01) * 2 * pi
        x = r * cos(t) + c[0]
        y = r * sin(t) + c[1]
        plot(x, y, 'b', linewidth=2)

    imshow(im)
    if circle:
        for p in locs:
            draw_circle(p[:2], p[2])
    else:
        plot(locs[:, 0], locs[:, 1], 'ob')
    axis('off')


def match(desc1, desc2):
    """ For each descriptor in the first image,
        select its match in the second image.
        input: desc1 (descriptors for the first image),
        desc2 (same for second image). """

    desc1 = array([d / linalg.norm(d) for d in desc1])
    desc2 = array([d / linalg.norm(d) for d in desc2])

    dist_ratio = 0.6
    desc1_size = desc1.shape

    matchscores = zeros((desc1_size[0]), 'int')
    desc2t = desc2.T  # precompute matrix transpose
    for i in range(desc1_size[0]):
        dotprods = dot(desc1[i, :], desc2t)  # vector of dot products
        dotprods = 0.9999 * dotprods
        # inverse cosine and sort, return index for features in second image
        indx = argsort(arccos(dotprods))

        # check if nearest neighbor has angle less than dist_ratio times 2nd
        if arccos(dotprods)[indx[0]] < dist_ratio * arccos(dotprods)[indx[1]]:
            matchscores[i] = int(indx[0])

    return matchscores


def appendimages(im1, im2):
    """ Return a new image that appends the two images side-by-side. """

    # select the image with the fewest rows and fill in enough empty rows
    rows1 = im1.shape[0]
    rows2 = im2.shape[0]

    if rows1 < rows2:
        im1 = concatenate((im1, zeros((rows2 - rows1, im1.shape[1]))), axis=0)
    elif rows1 > rows2:
        im2 = concatenate((im2, zeros((rows1 - rows2, im2.shape[1]))), axis=0)
    # if none of these cases they are equal, no filling needed.

    return concatenate((im1, im2), axis=1)


def plot_matches(im1, im2, locs1, locs2, matchscores, show_below=True):
    """ Show a figure with lines joining the accepted matches
        input: im1,im2 (images as arrays), locs1,locs2 (location of features),
        matchscores (as output from 'match'), show_below (if images should be shown below). """

    im3 = appendimages(im1, im2)
    if show_below:
        im3 = vstack((im3, im3))

    # show image
    imshow(im3)

    # draw lines for matches
    cols1 = im1.shape[1]
    for i, m in enumerate(matchscores):
        if m > 0:
            plot([locs1[i][0], locs2[m][0] + cols1], [locs1[i][1], locs2[m][1]], 'c')
    axis('off')


def match_twosided(desc1, desc2):
    """ Two-sided symmetric version of match(). """

    matches_12 = match(desc1, desc2)
    matches_21 = match(desc2, desc1)

    ndx_12 = matches_12.nonzero()[0]

    # remove matches that are not symmetric
    for n in ndx_12:
        if matches_21[int(matches_12[n])] != n:
            matches_12[n] = 0

    return matches_12

实验示例：
单应性矩阵就是将一幅图像中的标记物上的点映射到另外一幅图像的对应点，在在本次实验中，标记物采取的是书本。

from PCV.geometry import homography, camera
from PCV.localdescriptors import sift

# compute features
sift.process_image('book1frontal.JPG', 'im0.sift')
l0, d0 = sift.read_features_from_file('im0.sift')

sift.process_image('book1perspective.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((747, 1000))
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)

例如在标记物上加一个蓝色的正方形，第一个产生的标定矩阵就是该图像分辨率大小下的标定矩阵P1：

经过前面计算的单应性矩阵变换后，第二个照相机矩阵为P2=P1*K。可得到下图：

作为合理性验证，我们可以使用新矩阵标记投影平面的一个点，会发现投用后的点和使用第一个照相机模型P1和单应性矩阵变换后的点相同：

（四）增强现实

增强现实（Augmented Reality，AR）是将物体和相应信息放置在图像数据上的一 系列操作的总称。最经典的例子是放置一个三维计算机图形学模型，使其看起来属于该场景；如果在视频中，该模型会随着照相机的运动很自然地移动。如上一节所示，给定一幅带有标记平面的图像，我们能够计算出照相机的位置和姿态，使用这些信息来放置计算机图形学模型，能够正确表示它们。其中将用到两个工具包：PyGame 和 PyOpenGL。

4.1 PyGame 和 PyOpenGL

书后面附录3中的网址中找不到Windows系统使用的Python3.7版本需要的PyGame，所以直接 pip install pygame，可以用
PyOpenGL的安装：

PyOpenGL的安装不能直接 pip install PyOpenGL PyOpenGL。因为在这种情况下它默认安装的是32位的PyOpenGL，运行测试代码的时候会出现以下的问题

NullFunctionError: Attempt to call an undefined function glutInitDisplayMode, check for bool(glutInitDisplayMode) before calling

所以需要去 https://www.lfd.uci.edu/~gohlke/pythonlibs/#pyopengl下载64位的PyOpenGL，cp27代表适用python2.7版本，win32代表32位，win_amd64代表64位，然后安装。

测试：

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *


def drawFunc():
    # 清楚之前画面
    glClear(GL_COLOR_BUFFER_BIT)
    glRotatef(0.1, 5, 5, 0)  # (角度,x,y,z)
    glutWireTeapot(0.5)
    # 刷新显示
    glFlush()


# 使用glut初始化OpenGL
glutInit()
# 显示模式:GLUT_SINGLE无缓冲直接显示|GLUT_RGBA采用RGB(A非alpha)
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)
# 窗口位置及大小-生成
glutInitWindowPosition(0, 0)
glutInitWindowSize(400, 400)
glutCreateWindow(b"first")
# 调用函数绘制图像
glutDisplayFunc(drawFunc)
glutIdleFunc(drawFunc)
# 主循环
glutMainLoop()

显示茶壶：

4.2 从照相机矩阵到OpenGL格式

因为OpenGL使用4X4矩阵来表示变换。这和我们使用的3X4照相机矩阵略有区别。需要转换：

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)

实现如何获得一处标定矩阵后的3X4针孔照相机矩阵，并创建一个模拟视图：

def set_modelview_from_camera(Rt): 
    "从照相机姿态中获得模拟视图矩阵"
    
	glMatrixMode(GL_MODELVIEW) 
	glLoadIdentity()
 
    #围绕x轴将茶壶旋转90度，使z轴向上
	Rx = np.array([[1,0,0],[0,0,-1],[0,1,0]])
 
    #获得旋转的最佳逼近
	R = Rt[:,:3] 
	U,S,V = np.linalg.svd(R) 
	R = np.dot(U,V) 
	R[0,:] = -R[0,:]#改变x轴的符号
  
    #获得平移量
	t = Rt[:,3]

    #获得4X4de模拟视图矩阵
	M = np.eye(4) 
	M[:3,:3] = np.dot(R,Rx) 
	M[:3,3] = t

    #转置并压平以获取列序数值
	M = M.T
	m = M.flatten()
 
    #将模拟视图矩阵替换为新的矩阵
	glLoadMatrixf(m)

4.3 在图像中放置虚拟物体

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

def draw_background(imname):
    "使用四边形绘制背景图像"
    #载入背景图像（.bmp格式），转换为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.5,0.0,0.0,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)

综上:

# -*- coding: utf-8 -*-
import math
import pickle
import sys
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 = []
    # 底部
    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])
    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])
    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()
    # 围绕x轴将茶壶旋转90度，使z轴向上
    Rx = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
    # 获得旋转的最佳逼近
    R = Rt[:, :3]
    U, S, V = np.linalg.svd(R)
    R = np.dot(U, V)
    R[0, :] = -R[0, :]  # 改变x轴的符号
    # 获得平移量
    t = Rt[:, 3]
    # 获得4*4的的模拟视图矩阵
    M = np.eye(4)
    M[:3, :3] = np.dot(R, Rx)
    M[:3, 3] = t
    # 转置并压平以获取列序数值
    M = M.T
    m = M.flatten()
    # 将模拟视图矩阵替换成新的矩阵
    glLoadMatrixf(m)


def draw_background(imname):
    # 载入背景图像
    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.5, 0.0, 0.0, 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)


def drawFunc(size):  # 白色茶壶
    glRotatef(0.5, 5, 5, 0)  # (角度,x,y,z)
    glutWireTeapot(size)
    # 刷新显示
    glFlush()


width, height = 1000, 747


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


# 计算特征
sift.process_image('D:\\Python\\chapter4\\book_frontal.jpg', 'im0.sift')
l0, d0 = sift.read_features_from_file('im0.sift')

sift.process_image('D:\\Python\\chapter4\\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((747, 1000))
# 位于边长为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))

Rt = dot(linalg.inv(K), cam2.P)
setup()
draw_background("D:\\Python\\chapter4\\book_perspective.bmp")
set_projection_from_camera(K)
set_modelview_from_camera(Rt)

#draw_teapot(0.05)  # 显示红色茶壶
drawFunc(0.05)  # 显示白色空心茶壶
pygame.display.flip()
while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            sys.exit()

效果：