目录

一、针孔照相机模型

1.照相机矩阵

2.三维点的投影

3.照相机矩阵的分解

4.计算照相机中心

二、照相机标定

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

四、增强现实

1.PyGame和PyOpenGL

2.从照相机矩阵到OpenGL格式

3.在图像中放置虚拟物体

4.综合集成

一、针孔照相机模型

针孔照相机模型（也叫做射影照相机模型）是计算机视觉中广泛使用的照相机模型。在针孔照相机模型中，在光线投影到图像平面之前，从唯一一个点经过，也就是照相机中心C。其大致模型为：

照相机的光学坐标轴和z轴一致，该投影几何可以简化成相似三角形。在投影之前通过旋转和平移变换，对该坐标系加入三维点，会出现完整的投影变换。

1.照相机矩阵

照相机矩阵可以分解为下式：

R是描述照相机方向的旋转矩阵，t是描述照相机中心位置的三维平移向量，内标定矩阵K描述照相机的投影性质。标定矩阵仅和照相机自身的情况相关，可以写成：

图像平面和照相机中心间的距离为焦距 f。令

，纵横比例参数 α 是在像素元素非正方形的情况下使用的。在大多数情况下，s 可以设置成 0。因此可将标定矩阵写为：

除焦距之外，标定矩阵中剩余的唯一参数为光心，也就是光线坐标轴和图像平面的交点。

2.三维点的投影

首先需要创建一个照相机的类：

<code>class Camera(object):

def __init__(self, P):

self.P = P

self.R = None

self.t = None

self.K = None

self.c = None

def project(self, X):

X = np.dot(self.P, X)

for i in range(3):

X[i] /= X[2]

return X

然后可以将下载的house数据集的数据投影到图像平面上并执行绘制操作

def rotation_matrix(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

points = np.loadtxt(r'D:\test\111\3D\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)

r = 0.05*np.random.rand(3)

rot = rotation_matrix(r)

plt.figure()

plt.subplot(1,2,1)

plt.plot(x[0],x[1],'k.')

plt.subplot(1,2,2)

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

结果为： 

上述结果的右图是将左图围绕一个随机的三维向量，进行增量旋转投影的结果。 

3.照相机矩阵的分解

矩阵分块操作称为因子分解。这里使用的矩阵因子分解的方法称为RQ因子分解。其实现的方法为：

<code> def factor(self):

K,R = linalg.rq(self.P[:,:3])

T = np.diag(np.sign(np.diag(K)))

if np.linalg.det(R) < 0:

T[1,1] *= -1

self.K = np.dot(K,T)

self.R = np.dot(T,R)

self.t = np.dot(linalg.inv(self.K),self.P[:,3])

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

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

4.计算照相机中心

给定照相机投影矩阵P，可以计算出空间上照相机的所在位置。照相机的中心C，是一个三维点，且满足约束 PC=0。可以使用下述式子来计算：

照相机的中心和内标定矩阵K无关。其实现方法的代码为：

<code> def center(self):

if self.c is not None:

return self.c

else:

self.factor()

self.c = -np.dot(self.R.T,self.t)

return self.c

二、照相机标定

标定照相机是指计算出该照相机的内参数。标定照相机的标准方法是，拍摄多幅平面棋盘模式的图像，然后进行处理计算。

大多数参数可以使用基本的假设来设定，比较难处理的是获得正确的焦距。其具体的步骤为：

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

可以使用相似三角形关系获得焦距：

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

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

<code>process_image(r'd:\test\111\11.jpg', 'im0.sift')

l0, d0 = read_sift('im0.sift')

process_image(r'd:\test\111\22.jpg', 'im1.sift')

l1, d1 = read_sift('im1.sift')

# match features and estimate homography

matches = 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 = H_from_ransac(fp, tp, model)

得到的单应性矩阵将一幅图像中标记物上的点映射到另一幅图像中的对应点。为了检验单应性矩阵结果的正确性，我们需要将一些简单的三维物体放置在标记物 上，这里我们使用一个立方体。产生立方体的点的函数为：

def cube_points(c,wid):

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((900,675))

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

box_trans = homography.normalize(np.dot(H,box_cam1))

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

point = np.array([1,1,0,1]).T

print (homography.normalize(np.dot(np.dot(H,cam1.P),point)))

print (cam2.project(point))

im0 = np.array(Image.open(r'd:\test\111\11.jpg'))

im1 = np.array(Image.open(r'd:\test\111\22.jpg'))

plt.figure()

plt.subplot(131)

plt.imshow(im0)

plt.axis('off')

plt.plot(box_cam1[0,:],box_cam1[1,:],linewidth=3)

plt.subplot(132)

plt.imshow(im1)

plt.axis('off')

plt.plot(box_trans[0,:],box_trans[1,:],linewidth=3)

plt.subplot(133)

plt.imshow(im1)

plt.plot(box_cam2[0,:],box_cam2[1,:],linewidth=3)

plt.axis('off')

plt.show()

结果为：

四、增强现实

增强现实（AR）是将物体和相应信息放置在图像数据上的一系列操作的总称。

1.PyGame和PyOpenGL

PyGame 是非常流行的游戏开发工具包，它可以非常简单地处理显示窗口、输入设备、事件，以及其他内容。PyOpenGL是OpenGL图形编程的Python绑定接口。OpenGL可以安装在几乎所有的系统上，并且具有很好的图形性能。OpenGL具有跨平台性，能够在不同的操作系统之间工作。在代码中使用如下的命令导入对应的包：

<code>from OpenGL.GL import *

from OpenGL.GLU import *

import pygame, pygame.image

from pygame.locals import *

2.从照相机矩阵到OpenGL格式

OpenGL 使用 4×4 的矩阵来表示变换，照相机与场景的变换分成了两个矩阵，GL_PROJECTION 矩阵和GL_MODELVIEW 矩阵。GL_PROJECTION 矩阵处理图像成像的性质。GL_MODELVIEW 矩阵处理物体和照 相机之间的三维变换关系。以将照相机参数转换为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*np.height/fy)*180/np.pi

aspect = (np.width*fy)/(np.height*fx)

near = 0.1

far = 100.0

gluPerspective(fovy,aspect,near,far)

glViewport(0,0,np.width,np.height)

模拟视图矩阵能够表示相对的旋转和平移，该变换将该物体放置在照相机前，其为一个4*4的矩阵，如下所示：

获得移除标定矩阵后的3×4针孔照相机矩阵，并创建一个模拟视图的函数为：

<code>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 = 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)

该操作使用 SVD 分解方法，旋转矩阵 的最佳逼近可以通过

来获得。

3.在图像中放置虚拟物体

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

<code>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,np.width,np.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)

将Utah 茶壶作为物体放置入场景中，其由如下命令生成：

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)

4.综合集成

即把上述小点中的代码综合起来，在加上如下代码：

def setup():

pygame.init()

pygame.display.set_mode((width,height),OPENGL | DOUBLEBUF)

pygame.display.set_caption('OpenGL demo')

with open('ar_camera.pkl','rb') as f:

K = pickle.load(f)

Rt = pickle.load(f)

setup()

draw_background(r"D:\test\pil.png")

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

发现出现如下的错误：

 参照博客最终该问题得到了解决，再次运行该程序，又出现如下错误：

按照网上的方法尝试之后还是没有得到解决。

在 PyGame 中，使用带有对所有变化进行定期询问的无限循环来处理事件。这些事件可以为键盘、鼠标，或者其他。pygame.display.flip() 命令将会在屏幕上绘制出物体。