文章目录

第四章：照相机模型与增强实现
- 4.1　针孔照相机模型
- - 4.1.1　照相机矩阵
  - 4.1.2　三维点的投影
  - 4.1.3　照相机矩阵的分解
  - 4.1.4　计算照相机中心
- 4.2　照相机标定
- 4.3　以平面和标记物进行姿态估计
- 4.4　增强现实
- - 4.4.1 PyGame 和 PyOpenGL
  - 4.4.2　从照相机矩阵到 OpenGL 格式
  - 4.4.3　在图像中放置虚拟物体
  - 4.4.4　综合集成

第四章：照相机模型与增强实现

本章中，我们将尝试对照相机进行建模，并有效地使用这些模型。

4.1　针孔照相机模型

在这里插入图片描述

4.1.1　照相机矩阵

照相机矩阵可以分解为：
$P = K [R ∣ t]$
其中，R 是描述照相机方向的旋转矩阵，t 是描述照相机中心位置的三维平移向量，内标定矩阵 K 描述照相机的投影性质。

标定矩阵: $K=\begin{bmatrix}\alpha f&s&c_x\\0&f&c_y\\0&0&1\end{bmatrix}$

图像平面和照相机中心间的距离为焦距 f。当像素数组在传感器上偏斜的时候，需要用到倾斜参数 s。在大多数情况下，s 可以设置成 0。**纵横比例参数 α **是在像素元素非正方形的情况下使用的。通常情况下，我们可以默认设置 α=1。即为
$K=\begin{bmatrix}f&0&c_x\\0&f&c_y\\0&0&1\end{bmatrix}$
标定矩阵中剩余的唯一参数为光心（有时称主点）的坐标 $c=[c_x，c_y]$

4.1.2　三维点的投影

创建照相机类，用来处理我们对照相机和投影建模所需要的全部操作

from scipy import linalg
import numpy as np
import matplotlib.pyplot as plt


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 = np.dot(self.P, X)
        for i in range(3):
            x[i] /= x[2]
        return x
    
    def factor(self):
        """ 将照相机矩阵分解为 K、R、t，其中，P = K[R|t] """
        # 分解前 3×3 的部分
        K,R = linalg.rq(self.P[:,:3])
        # 将 K 的对角线元素设为正值
        T = np.diag(np.sign(np.diag(K)))
        if linalg.det(T) < 0:
            T[1,1] *= -1
        self.K = np.dot(K,T)
        self.R = np.dot(T,R) # T 的逆矩阵为其自身
        self.t = np.dot(linalg.inv(self.K),self.P[:,3])
        return self.K, self.R, self.t

从Model Housing下载文件，进行展示。

points = np.loadtxt('04data/house.p3d').T
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.')
plt.show()

在这里插入图片描述

为了研究照相机的移动会如何改变投影的效果，使用下面的代码。该代码围绕一个随机的三维向量，进行增量旋转的投影

def rotation_matrix(a):
    """ 创建一个用于围绕向量 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


# 创建变换
r = 0.05*np.random.rand(3)
rot = rotation_matrix(r)
# 旋转矩阵和投影
plt.figure()
for t in range(10):
    cam.P = np.dot(cam.P, rot)
    x = cam.project(points)
    plt.plot(x[0], x[1], 'k.')

在这里插入图片描述

4.1.3　照相机矩阵的分解

观察照相机矩阵分解的效果

K = np.array([[1000,0,500],[0,1000,300],[0,0,1]])
tmp = rotation_matrix([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())

[[1000    0  500]
 [   0 1000  300]
 [   0    0    1]] [[ 0.54030231 -0.84147098  0.         50.        ]
 [ 0.84147098  0.54030231  0.         40.        ]
 [ 0.          0.          1.         30.        ]]
(array([[ 1.00000000e+03,  2.27373675e-13,  5.00000000e+02],
       [ 0.00000000e+00, -1.00000000e+03,  3.00000000e+02],
       [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]), array([[ 0.54030231, -0.84147098,  0.        ],
       [-0.84147098, -0.54030231,  0.        ],
       [ 0.        ,  0.        ,  1.        ]]), array([ 50., -40.,  30.]))

4.1.4　计算照相机中心

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

$\mathbf{C}=-R^{T}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

4.2　照相机标定

标定照相机是指计算出该照相机的内参数。

大多数参数可以使用基本的假设来设定（正方形垂直的像素，光心位于图像中心），比较难处理的是获得正确的焦距f。方法如下：

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

$f_{x}=\frac{\mathrm{d}x}{\mathrm{d}X}\mathrm{d}Z,\quad f_{y}=\frac{\mathrm{d}y}{\mathrm{d}Y}\mathrm{d}Z$

假设算得： $f_x=2555,\quad f_y=2586$

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

4.3　以平面和标记物进行姿态估计

import homography
import camera
import sift
# 计算特征
sift.process_image('book_frontal.JPG','im0.sift')
l0,d0 = sift.read_features_from_file('im0.sift')
sift.process_image('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 = homography.H_from_ransac(fp,tp,model)

D:\software\vlfeat\vlfeat-0.9.20\bin\win32\sift.exe tmp.pgm --output=im0.sift --edge-thresh 10 --peak-thresh 5
processed tmp.pgm to im0.sift
D:\software\vlfeat\vlfeat-0.9.20\bin\win32\sift.exe tmp.pgm --output=im1.sift --edge-thresh 10 --peak-thresh 5
processed tmp.pgm to im1.sift

def cube_points(c, wid):
    """ 创建用于绘制立方体的一个点列表（前 5 个点是底部的正方形，一些边重合了）"""
    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 np.array(p).T

# 计算照相机标定矩阵
K = my_calibration((747,1000))
# 位于边长为 0.2，z=0 平面上的三维点
box = cube_points([0,0,0.1],0.1)
# 投影第一幅图像上底部的正方形
cam1 = camera.Camera( np.hstack((K,np.dot(K,np.array([[0],[0],[-1]])) )) )
# 底部正方形上的点
box_cam1 = cam1.project(homography.make_homog(box[:,:5]))
# 使用 H 将点变换到第二幅图像中
H,_=H
the_dot=np.dot(np.array(H),np.array(box_cam1))
box_trans = homography.normalize(the_dot)
# 从 cam1 和 H 中计算第二个照相机矩阵
cam2 = camera.Camera(np.dot(H,cam1.P))
A = np.dot(linalg.inv(K),cam2.P[:,:3])
A = np.array([A[:,0],A[:,1],np.cross(A[:,0],A[:,1])]).T
cam2.P[:,:3] = np.dot(K,A)
# 使用第二个照相机矩阵投影
box_cam2 = cam2.project(homography.make_homog(box))
# 测试：将点投影在 z=0 上，应该能够得到相同的点
point = np.array([1,1,0,1]).T
print( homography.normalize(np.dot(np.dot(H,cam1.P),point)))
print( cam2.project(point))

[-1.41541161e+03 -2.04923190e+02 -1.10979002e+00]
[1.27538686e+03 1.84650417e+02 1.00000000e+00]

4.4　增强现实

增强现实（Augmented Reality，AR）是将物体和相应信息放置在图像数据上的一系列操作的总称。最经典的例子是放置一个三维计算机图形学模型，使其看起来属于该场景；如果在视频中，该模型会随着照相机的运动很自然地移动。

4.4.1 PyGame 和 PyOpenGL

PyGame是非常流行的游戏开发工具包，它可以非常简单地处理显示窗口、输入设备、事件，以及其他内容。
PyOpenGL 是 OpenGL 图形编程的 Python 绑定接口。

安装后测试：

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

pygame 2.5.1 (SDL 2.28.2, Python 3.8.8)
Hello from the pygame community. https://www.pygame.org/contribute.html

4.4.2　从照相机矩阵到 OpenGL 格式

OpenGL 使用 4×4 的矩阵来表示变换（包括三维变换和投影）。这和我们使用的 3×4 照相机矩阵略有差别。假设我们已经获得了标定好的照相机，即已知标定矩阵 K，下面的函数可以将照相机参数转换为 OpenGL 中的投影矩阵

def set_projection_from_camera(K):
    """ 从照相机标定矩阵中获得视图 """
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    fx = K[0, 0]
    fy = K[1, 1]
    fovy = 2*np.arctan(0.5*height/fy)*180/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 = 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)

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
 
def Draw():
    glClear(GL_COLOR_BUFFER_BIT)
    glRotatef(0.5, 0, 1, 0)
    glutWireTeapot(0.5)
    glFlush()
 
glutInit()
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)
glutInitWindowSize(400, 400)
glutCreateWindow("test")
glutDisplayFunc(Draw)
glutIdleFunc(Draw)
glutMainLoop()

在这里插入图片描述

创建一个test的Window，绘制茶壶，如上所示。

4.4.3　在图像中放置虚拟物体

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_MAG_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)

import pickle
with open('ar_camera.pkl', 'wb') as f:
    pickle.dump(K, f)
    pickle.dump(np.dot(linalg.inv(K), cam2.P), f)

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)
    glutWireTeapot(size)
    glFlush()

4.4.4　综合集成

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from pygame.locals import *
import pickle
import OpenGL
# from OpenGL.GLUT.objects import glutSolidTeapot
width, height = 1000, 747
pi=3.1415

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


# 载入照相机数据
with open('ar_camera.pkl', 'rb') as f:
    K = pickle.load(f)
    Rt = pickle.load(f)

setup()
draw_background('book_perspective.bmp')
set_projection_from_camera(K)
set_modelview_from_camera(Rt)
draw_teapot(0.02)
while True:
    event = pygame.event.poll()
    if event.type in (QUIT,KEYDOWN):
        break
pygame.display.flip()