Python计算机视觉编程 第四章 照相机模型与增强现实

针孔照相机模型

针孔照相机模型（Pinhole Camera Model）是计算机视觉中最基础也是最常用的几何模型之一。它用来模拟理想的照相机如何将三维空间中的场景投射到二维的图像平面上。针孔照相机模型假设光线沿直线穿过一个小孔进入一个暗箱（即相机），并在暗箱内形成一个倒立缩小的图像。下图为从照相机中心前画出图像平面的图解。
照相机的光学坐标轴和z轴一致，该投影几何可以简化成相似三角形。在投影之前通过旋转和平移变换，对该坐标系加入三维点，会出现完整的投影变换。
在针孔照相机中，三维点X投影为图像点x，如下所示：$\lambda x=PX$,这里，3×4的矩阵P为照相机矩阵（或投影矩阵）。

照相机矩阵

照相机矩阵可以分解为：$P=K\left[R\mid t\right]$,其中，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}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$,这里，我们使用了另外的记号$f_x$和$f_y$,其中$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=\left[c_x,c_y\right]$

三维点的投影

代码如下

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

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

# 生成示例点
points = np.array([
    [0.0, 1.0, 0.0, 0.0],
    [0.0, 0.0, 1.0, 0.0],
    [0.0, 0.0, 0.0, 1.0],
    [1.0, 1.0, 1.0, 1.0]
]).T

# 设置照相机参数
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()

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

# 旋转矩阵和投影
plt.figure()
for t in range(20):
    cam.P = np.dot(cam.P, rot)
    x = cam.project(points)
    plt.plot(x[0], x[1], 'k.')
plt.show()

结果如下：

照相机矩阵的分解

矩阵分块操作称为因子分解。这里，我们将使用一种矩阵因子分解的方法，称为RQ因子分解。
将下面的方法添加到Camera类中：

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

在示例照相机上运行下面的代码

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

结果如下：

计算照相机中心

照相机的中心$C$是一个三维点，满足约束$PC=0$。对于投影矩阵为$P=K\left[R\mid t\right]$的照相机，有：$K\left[R\mid t\right]C=KRC+Kt=0$,照相机的中心可以由下述式子来计算：$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

照相机标定

标定照相机是指计算出该照相机的内参数。在我们的例子中，是指计算矩阵$K$。标定照相机的标准方法是，拍摄多幅平面棋盘模式的图像，然后进行处理计算。

一个简单的标定方法

这里我们将要介绍一个简单的照相机标定方法。
具体步骤：
1.准备棋盘格：使用一个标准的棋盘格图案作为标定板。
2.采集图像：拍摄一系列包含棋盘格的不同角度和位置的图像。
3.检测棋盘角点：使用OpenCV检测每张图像中的棋盘角点。
4.计算世界坐标系中的角点位置：根据棋盘格的实际尺寸，计算世界坐标系中的角点位置。
5.标定照相机：使用检测到的角点位置来计算照相机的内参和外参。
6.评估标定结果：计算重投影误差来评估标定的效果。
使用下面的相似三角形关系可以获得焦距：$$f_x=\frac{dx}{dX}dZ,f_y=\frac{dy}{dY}dZ$$

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

如果图像中包含平面状的标记物体，并且已经对照相机进行了标定，那么我们可以计算出照相机的姿态（旋转和平移）。
我们使用下面的代码来提取两幅图像的SIFT特征，然后使用RANSAC算法稳健地估计单应性矩阵：

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)

现在我们得到了单应性矩阵。该单应性矩阵将一幅图像中标记物（在这个例子中，标记物是指书本）上的点映射到另一幅图像中的对应点。
为了检验单应性矩阵结果的正确性，我们需要将一些简单的三维物体放置在标记物上，这里我们使用一个立方体。你可以使用下面的函数来产生立方体上的点：

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 array(p).T

有了单应性矩阵和照相机的标定矩阵，我们现在可以得出两个视图间的相对变换：

# 计算照相机标定矩阵
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))
 # 测试：将点投影在z=0上，应该能够得到相同的点
point = array([1,1,0,1]).T
 print homography.normalize(dot(dot(H,cam1.P),point))
 print cam2.project(point)

增强现实

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

PyGame和PyOpenGL

PyGame 是非常流行的游戏开发工具包，它可以非常简单地处理显示窗口、输入设备、事件，以及其他内容。事实上，它是一个Python绑定的SDL游戏引擎。

PyOpenGL 是 OpenGL 图形编程的Python绑定接口。OpenGL可以安装在几乎所有的系统上，并且具有很好的图形性能。OpenGL具有跨平台性，能够在不同的操作系统之间工作。

从照相机矩阵到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*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)

下面的函数实现如何获得移除标定矩阵后的3×4针孔照相机矩阵（将$P$和$K^{-1}$相乘），并创建一个模拟视图：

def set_modelview_from_camera(Rt):
  glMatrixMode(GL_MODELVIEW)
  glLoadIdentity()
  Rx = array([[1,0,0],[0,0,-1],[0,1,0]])
  R = Rt[:,:3]
  U,S,V = linalg.svd(R)
  R = dot(U,V)
  R[0,:] = -R[0,:] 
  t = Rt[:,3]
  M = eye(4)
  M[:3,:3] = dot(R,Rx)
  M[:3,3] = t
  M = M.T
  m = M.flatten()
  glLoadMatrixf(m)

由于OpenGL中的坐标系和上面用到的有点不同，所以我们将x轴翻转。然后，我们将模拟视图矩阵M设置为旋转矩阵的乘积。glLoadMatrixf() 函数通过输入参数为按列排列的16个数值数组，来设置模拟视图。将M矩阵转置，然后压平并输入glLoadMatrixf()函数。

在图像中放置虚拟物体

我们需要做的第一件事是将图像（打算放置虚拟物体的图像）作为背景添加进来。在OpenGL中，该操作可以通过创建一个四边形的方式来完成，该四边形为整个视图。完成该操作最简单的方式是绘制出四边形，同时将投影和模拟试图矩阵重置，使得每一维的坐标范围在-1到1之间。
下面的函数可以载入一幅图像，然后将其转换成一个OpenGL纹理，并将该纹理放置在四边形上：

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)

该函数首先使用PyGame中的一些函数来载入一幅图像，将其序列化为能够在PyOpenGL 中使用的原始字符串表示。然后，重置模拟视图，清除颜色和深度缓存。接下来，绑定这个纹理，使其能够在四边形和指定插值中使用它。