python计算机视觉第四章----照相机模型与增强现实

1、针孔照相机模型

针孔照相机模型（有时称为射影照相机模型）是计算机视觉中广泛使用的照相机模 型。对于大多数应用来说，针孔照相机模型简单，并且具有足够的精确度。这个名字来源于一种类似暗箱机的照相机，该照相机从一个小孔采集射到暗箱内部的光线。在光线投影到图像平面之前，从唯一一个点经过，也就是照相机中心$o_c$。

1.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]$$
大多数情况下，可以将s设为0，纵横比例参数α设为1.f为图像平面和照相机中心间的距离(焦距)，标定矩阵K中剩余的唯一参数为光心（有时称主点）的坐标 $c=[c_x，c_y]$，光心的坐标常接近于图像宽度和高度的一半,因此唯一未知的变量是焦距 f。

1.2 三维点的投影

相机类的创建：

from scipy import linalg
from pylab import  *
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
   def rotation_matrix(a):
       """创建一个用于围绕向量a轴旋转的三维旋转矩阵"""
       R = eye(4)
       R[:3,:3] = linalg.expm([0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0])
       return R

1.3 照相机矩阵的分解

如果给定方程（1.1节中）所示的照相机矩阵P，我们需要恢复内参数K以及照相机的位置t和姿势R。矩阵分块操作称为因子分解。这里，我们将使用一种矩阵因子分解的方法，称为RQ因子分解。将一个 3x3 矩阵 A 进行 RQ 分解是将其分解成为一个上三角阵 R 与一个正交阵（orthogonal matrix） Q 的乘积。要求矩阵 A 的秩为3，即满秩。

def factor(self):
    """将照相机矩阵分解为 K,R,t,其中, R=K[R|t]"""
    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

1.4 照相机中心

照相机的中心C，是一个三维点，满足约束PC=0。故给定照相机投影矩阵P，我们可以计算出空间上照相机的所在位置。对于投影矩阵为$P=K[R|t]$的照相机，有$K[R|t]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

2、照相机标定

在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数（内参、外参、畸变参数）的过程就称之为相机标定。特别的，在零失真相机标定法中，每一个像素矢量在空间都是独立标定的，无需知道相机内部的结构，无需建立几何模型。
相机标定方法有:传统相机标定法、主动视觉相机标定方法、相机自标定法、零失真相机标定法。

1. 传统相机标定法需要使用尺寸已知的标定物，通过建立标定物上坐标已知的点与其图像点之间的对应，利用一定的算法获得相机模型的内外参数。根据标定物的不同可分为三维标定物和平面型标定物。三维标定物可由单幅图像进行标定，标定精度较高，但高精密三维标定物的加工和维护较困难。平面型标定物比三维标定物制作简单，精度易保证，但标定时必须采用两幅或两幅以上的图像。传统相机标定法在标定过程中始终需要标定物，且标定物的制作精度会影响标定结果，那么有些场合不适合放置标定物就会限制传统相机标定法的应用。
2. 目前出现的自标定算法中主要是利用相机运动的约束。相机的运动约束条件太强，因此使得其在实际中并不实用。利用场景约束主要是利用场景中的一些平行或者正交的信息。其中空间平行线在相机图像平面上的交点被称为消失点，它是射影几何中一个非常重要的特征，所以很多学者研究了基于消失点的相机自标定方法。自标定方法灵活性强，可对相机进行在线定标。但由于它是基于绝对二次曲线或曲面的方法，其算法鲁棒性差。
3. 基于主动视觉的相机标定法是指已知相机的某些运动信息对相机进行标定。该方法不需要标定物，但需要控制相机做某些特殊运动，利用这种运动的特殊性可以计算出相机内部参数。基于主动视觉的相机标定法的优点是算法简单，往往能够获得线性解，故鲁棒性较高，缺点是系统的成本高、实验设备昂贵、实验条件要求高，而且不适合于运动参数未知或无法控制的场合。
4. 零失真相机标定法是以LCD显示屏为参考基准，以相移光栅为媒介，建立LCD像素与相机传感器像素之间的映射关系，确定每个相机像素点在LCD上的视点位置。镜头使相机在LCD上的视场为非矩形，在这个有畸变的视场内，可以构造一个内接的虚拟传感器，并保持相同的像素数。这样每个虚拟像素点就一定落在某四个相邻视点构成的任意四边形之内。虚拟像素点的亮度将由这四点的亮度经加权插值确定，而与其它像素点无关。用这四个加权系数的集合对原始图像作重采样（四次乘法、四次加法），就可以得到零畸变的输出图。

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

我们SIFT算法来提取两幅图像的特征，然后使用 RANSAC 算法稳健地估计单应性矩阵。为检验结果的正确性，我们使用一个立方体来进行检验：

from pylab import *
from PIL import Image
from numpy import *
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()

4、增强现实

增强现实(Augmented Reality，简称 AR)，是一种实时地计算摄影机影像的位置及角度并加上相应图像的技术。它把原本在现实世界的一定时间空间范围内很难体验到的实体信息(视觉信息,声音,味道,触觉等),通过科学技术模拟仿真后再叠加到现实世界被人类感官所感知,从而达到超越现实的感官体验。

4.1 PyGame和PyOpenGL

PyGame是非常流行的游戏开发工具包，它可以简单的处理现实窗口、输入设备、事件，以及其他内容。
PyOpenGL是OpenGL图形编程的Python绑定接口。OpenGL可以安装在几乎所有的系统上，并且具有很好的图形性能。OpenGL具有跨平台性，能够在不同的操作系统之间工作。
下面来进行一个简单的小实验：

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
def drawFunc():
    glClear(GL_COLOR_BUFFER_BIT)
    # glRotatef(1, 0, 1, 0)
    glutWireTeapot(0.5)
    glFlush()
glutInit()
glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)
glutInitWindowSize(400, 400)
#参数为b类型而不是string
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 * 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)

4.3 在图像中放置物体

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
import cv2
#绘制立方体
def cube_points(c, wid):
    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 = 758*col/640
    fy = 752*row/480
    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]])
    R = Rt[:,:3] 
    U,S,V = np.linalg.svd(R) 
    R = np.dot(U,V) 
    R[0,:] = -R[0,:]
    t = Rt[:,3]
    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:\\123\图像处理\Image Processing\Image Processing\Chapter 4\\book_frontal.JPG', 'im0.sift')
l0, d0 = sift.read_features_from_file('im0.sift')
sift.process_image('D:\\123\图像处理\Image Processing\Image Processing\Chapter 4\\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))
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]))
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)
#使用第二个照相机矩阵投影
box_cam2 = cam2.project(homography.make_homog(box))
Rt = dot(linalg.inv(K), cam2.P)
setup()
draw_background("D:\\123\图像处理\Image Processing\Image Processing\Chapter 4\\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()