照相機模型與增強現實

本章中，主要嘗試對照相機進行建模，并有效地使用這些模型。為了處理三維圖像和平面圖像之間 的映射，我們需要在映射中加入部分照相機產生圖像過程的投影特性。下面講述如何確定照相機的參數，以及在具體應用中，如增強現實，如何使用圖像間的投影變換。

（一）針孔照相機模型

在計算機視覺中，利用所拍攝的圖像來計算出三維空間中被測物體的幾何參數。圖像是空間物體通過成像系統在像平面上的反映，即空間物體在像平面上的投影。

圖像上每一個像素點的灰度反映了空間物體表面某點的反射光的強度，而該點在圖像上的位置則與空間物體表面對應點的幾何位置有關。這些位置的相互關系，由攝像機成像系統的幾何投影模型所決定。

計算機視覺研究中，三維空間中的物體到像平面的投影關系即為 成像模型 ，理想的投影成像模型是光學中的中心投影，也稱為 針孔模型 。

針孔模型

1. 假設物體表面的反射光都經過一個針孔而投影到像平面上，即滿足光的直線傳播條件。
2. 針孔模型主要由光心（投影中心）、成像面和光軸組成。
3. 小孔成像由于透光量太小，因此需要很長的曝光時間，并且很難得到清晰的圖像。實際攝像系統通常都由透鏡或者透鏡組成。
4. 兩種模型具有相同的成像關系，即像點是物點和光心的連線與圖像平面的交點。因此，可以用針孔模型作為照相機成像模型。

針孔照相機
針孔照相機模型（有時稱為射影照相機模型）是計算機視覺中廣泛使用的照相機模型。對于大多數應用來說，針孔照相機模型簡單，并且具有足夠的精確度。

空間點0是投影中心，它到平面π的距離是f。空間點M在平面π上的投影（或像）m是以點0為端點并經過點M的射線與平面π的交點。

平面π ：攝像機的像平面
點0 ：攝像機中心（光心）
f：攝像機的焦距
以點0為端點且垂直于像平面的射線稱為光軸或主軸，主軸與像平面的交點p稱為攝像機的主點。

常用坐標系及其關系：
圖像坐標系：
以圖像左上角為原點建立以像素為單位的直接坐標系u-v。像素的橫坐標u與縱坐標v分別是在其圖像數組中所在的列數與所在行數。（在OpenCV中u對應x，v對應y）

由于(u,v)只代表像素的列數與行數，而像素在圖像中的位置并沒有用物理單位表示出來，所以，我們還要建立以 物理單位 （如毫米）表示的圖像坐標系x-y。將相機光軸與圖像平面的交點（一般位于圖像平面的中心處，也稱為圖像的主點（principal point）定義為該坐標系的原點O1，且x軸與u軸平行，y軸與v軸平行，假設(u0,v0)代表O1在u-v坐標系下的坐標，dx與dy分別表示每個像素在橫軸x和縱軸y上的物理尺寸，則圖像中的每個 像素 在u-v坐標系中的坐標和在x-y坐標系中的坐標之間都存在如下的關系：

其中，我們假設物理坐標系中的單位為毫米，那么dx的的單位為:毫米/像素。那么x/dx的單位就是像素了，即和u的單位一樣都是像素。為了使用方便，可將上式用齊次坐標與矩陣形式表示為：

相機坐標系 ：
相機成像的幾何關系可由圖2表示。其中O點為攝像機光心（投影中心），Xc軸和Yc軸與成像平面坐標系的x軸和y軸平行，Zc軸為相機的光軸，和圖像平面垂直。光軸與圖像平面的交點為圖像的主點O1,由點O與Xc,Yc,Zc軸組成的直角坐標系稱為攝像機的坐標系。OO1為相機的焦距。

世界坐標系 ：也稱真實或現實世界坐標系，或全局坐標系。世界坐標系是為了描述相機的位置而被引入的，如圖2.2中坐標系OwXwYwZw即為世界坐標系。

世界坐標與相機坐標之間的轉換關系 ：
平移向量t和旋轉矩陣R可以用來表示相機坐標系與世界坐標系的關系。所以，假設空間點P在世界坐標系下的齊次坐標是(Xw,Yw,Zw,1)T，（這里T是上標轉置），在相機坐標下的齊次坐標是(Xc,Yc,Zc,1)T,則存在如下的關系：

其中，

1. R是3×3的正交單位矩陣（也稱為旋轉矩陣）
   滿足約束條件：
   
   
   
2. T是三維的平移向量，矢量 ，是世界坐標系原點在照相機坐標系中的坐標。
3. M1是4×4矩陣。
   
4. 矩陣R實際上只含有三個獨立變量 $R_x$ ， $R_y$ ， $R_z$ ，再加上 $t_x$ ， $t_y$ ， $t_z$ ，總共六個參數決定了照相機光軸在世界坐標系的坐標，因此這六個參數稱為照相機的 外部參數 。

圖像坐標系與相機坐標系變換關系 ：
照相機坐標系中的一點P在圖像物理坐標系中像點P坐標為：


齊次坐標表示為：

將上式圖像物理坐標系進一步轉化為圖像坐標系：

其中， $u_0$ ， $v_0$ 是圖像中心（光軸與圖像平面的交點）坐標， $d_x$ ， $d_y$ 分別為一個像素在X于Y方向上的物理尺寸， $s_x$ =1/ $d_x$ ， $s_y$ =1/ $d_y$ 分別為X與Y方向上的采樣頻率，即單位長度的像素個數。

因此可得物點P與圖像像素坐標系中像點 $p_f$ 的變換關系為：

其中， $f_x$ =f $s_x$ , $f_y$ =f $s_y$ 分別定義為X和Y方向的等效焦距。 $f_x$ ， $f_y$ ， $u_o$ ， $v_o$ 這四個參數只與照相機內部結構有關，因此稱為照相機的內部參數。

世界坐標系與圖像坐標系變換關系：

轉化為齊次坐標為：

這是針孔模型或者中心圖像的數學表達式，在計算機內部參數確定的條件下 ，利用若干個已知的物點和相應的像點坐標，就可以求解出攝像機的內部和外部參數。

1.1 照相機矩陣

成像模型的代數表示

攝像機坐標系：O-XcXcZc
圖像坐標系：O-XY

根據三角形相似原理，有


上式可表示為下面的矩陣：

其中


如果記P=diag(f,f,1)(I,0)，則上式可表示為m=P $X_c$ ，其中，矩陣P是一個3x4的矩陣，通常稱它為照相機矩陣。

1.2 三維點的投影

1.3 照相機矩陣的分解

齊次坐標下，物體的物理坐標是 [x,y,z,1]′的形式，圖像上對應點的坐標是 [u,v,1]′的形式，所以相機矩陣 P作為把物體映射成像的矩陣，應該是一個 3×4 的矩陣。

因此，照相機矩陣可以表示為如下形式：

| 代表的是增廣矩陣
M 代表的是可逆的 3×3 的矩陣
C 是列向量，代表世界坐標系中相機的位置

要想求得 C, 只需要 P 的前三列組成的M, 求出來 ?M?1 再乘以最后一列，就得到了 C矩陣的確可以把 3D的點投影到 2D 空間，但是，這種形式下表達的信息是很粗糙的，比如

1. 沒有提供相機的擺放姿態
2. 沒有相機內部的幾何特征

為了挖掘這些信息，對矩陣進一步分解為一個內參矩陣和外參矩陣的乘積:

其中

1. 矩陣R是rotation矩陣，因此是正交的；K是上三角矩陣. 對P的前三列進行RQ分解就可得到KR
2. T=?RC, 是攝像機坐標系下世界坐標系原點的位置, tx,ty,tz分別代表世界坐標原點在相機的 左右，上下，前后 位置關系。在三維重建中，一組標定好了的圖片序列，它們各自的相機矩陣中的T應該是相同的。
3. 其中 K 就是內參，R,T 為外參。

如果給定照相機矩陣 P，我們需要恢復內參數 K 以及照相機的 位置t 和姿勢R。矩陣分塊操作稱為因子分解。這里，我們將使用一種矩陣因子分 解的方法，稱為 RQ 因子分解

將下面的方法添加到 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，我們可以計算出空間上照相機的所在位置。照相機的中心 C，是一個三維點，滿足約束 PC=0。對于投影矩陣為 P=K[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


            
          

上面的一些方法構成了 Camera 類的基本函數操作。

（二）照相機標定

標定照相機是指計算出該照相機的內參數。在我們的例子中，是指計算矩陣 K。

需要準備一個平面矩形的標定物體（一個書本即可）、用于測 量的卷尺和直尺，以及一個平面。下面是具體操作步驟：

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



            
          

            
              # -*- coding: utf-8 -*-
# -*- coding: utf-8 -*-
# -*- coding: cp936 -*-
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('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((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('D:\\Python\\chapter4\\book_frontal.jpg'))
im1 = array(Image.open('D:\\Python\\chapter4\\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()


            
          

代碼運行效果如下：

（四）增強現實

增強現實（Augmented Reality，AR）是將物體和相應信息放置在圖像數據上的一 系列操作的總稱。最經典的例子是放置一個三維計算機圖形學模型，使其看起來屬于該場景；如果在視頻中，該模型會隨著照相機的運動很自然地移動。如上一節所示，給定一幅帶有標記平面的圖像，我們能夠計算出照相機的位置和姿態，使用這些信息來放置計算機圖形學模型，能夠正確表示它們。其中，我們會用到兩個工具包：PyGame 和 PyOpenGL。

4.1PyGame 和 PyOpenGL

安裝PyGame

pip install pygame

安裝PyOpenGL

pip install PyOpenGL-3.1.3b2-cp27-cp27m-win_amd64.whl
pip install PyOpenGL_accelerate-3.1.3b2-cp27-cp27m-win_amd64.whl

測試：

            
              # -*- coding: utf-8 -*-
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)
    glutSolidTeapot(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 使用4×4 的矩陣來表示變換（包括三維變換和投影）。這和我們使用 的 3×4 照相機矩陣略有差別。但是，照相機與場景的變換分成了兩個矩陣，GL_PROJECTION 矩陣和GL_MODELVIEW 矩陣GL_PROJECTION 矩陣處理圖像成像的性質，等價于我們的內標定矩陣 K。GL_MODELVIEW 矩陣處理物體和照 相機之間的三維變換關系，對應于我們照相機矩陣中的R 和 t 部分。一個不同之處是，假設照相機為坐標系的中心，GL_MODELVIEW 矩陣實際上包含了將物體放置 在照相機前面的變換。

假設我們已經獲得了標定好的照相機，即已知標定矩陣 K，下面的函數可以將照相機參數轉換為 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)


            
          

第 一個 函 數 glMatrixMode() 將工作矩陣設置為 GL_PROJECTION，接下來的命令會修改這個矩 陣 1。 然后，glLoadIdentity() 函數將該矩陣設置為單位矩陣，這是重置矩陣的一般 操作。然后，我們根據圖像的高度、照相機的焦距以及縱橫比，計算出視圖中的垂 直場。OpenGL 的投影同樣具有近距離和遠距離的裁剪平面來限制場景拍攝的深度 范圍。我們設置近深度為一個小的數值，使得照相機能夠包含最近的物體，而遠深 度設置為一個大的數值。我們使用 GLU 的實用函數 gluPerspective() 來設置投影矩 陣，將整個圖像定義為視圖部分（也就是顯示的部分）。和下面的模擬視圖函數相 似，你可以使用 glLoadMatrixf() 函數的一個選項來定義一個完全的投影矩陣。當簡單版本的標定矩陣不夠好時，可以使用完全投影矩陣。

模擬視圖矩陣能夠表示相對的旋轉和平移，該變換將該物體放置在照相機前（效果是照相機在原點上）。模擬視圖矩陣是個典型的 4×4 矩陣，如下所示：

其中，R 是旋轉矩陣，列向量表示 3 個坐標軸的方向，t 是平移向量。當創建模擬視圖矩陣時，旋轉矩陣需要包括所有的旋轉（物體和坐標系的旋轉），可以將單個旋轉分量相乘來獲得旋轉矩陣。

4.3 在圖像中放置虛擬物體

編寫代碼：

            
              # -*- 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()


            
          

代碼運行效果如下：