Python+OpenCV实现照相机的姿态估计和增强现实
一、以平面和标记物进行姿态估计
如果图像中包含平面状的标记物体，并且已经对照相机进行了标记，那么我们可以计算出照相机的姿态（旋转和平移。使用平面物体作为标记物，来计算用于新视图投影矩阵。
1、运行结果
将图像的特征好人对其后的标记匹配，计算出单应性矩阵，然后用于计算照相机的姿态。带有一个灰色正方形区域的模板图像，如下图所示：
从未知视角拍摄的一幅图像，该图像包含同一个正方形，该正方形已经经过估计的单应性矩阵进行了变换，如下图所示：
使用计算出的照相机矩阵变换立方体，如下图所示：
2、代码
# -*- coding: cp936 -*-
from pylab import *
from PIL import Image
# If you have PCV installed, these imports should work
from PCV.geometry import homography, camera
from PCV.localdescriptors import sift
"""
This is the augmented reality and pose estimation cube example from Section 4.3.
"""
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
def my_calibration(sz):
"""
Calibration function for the camera (iPhone4) used in this example.
"""
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
# 计算特征
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, 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))
# 可视化投影后的点
im0 = array(Image.open('book_frontal.JPG'))
im1 = array(Image.open('book_perspective.JPG'))
#底部正方形的二维投影
figure()
imshow(im0)
plot(box_cam1[0, :], box_cam1[1, :], linewidth=3)
title('2D projection of bottom square')
axis('off')
#使用H对二维投影进行变换
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）是将物体和相应信息放置在图像数据上的一系列操作的总称。给定一幅带有标记平面的图像，我们能够计算出照相机的位置和姿态，使用这些信息来放置计算机图形学模型，能够正确的表示他们。
1、安装PyGame和PyOpenGL工具包
PyGame是非常流行的游戏开发工具，他可以非常简单地处理显示窗口，输入设备，事件，以及其他内容。实际上，它是一个Python绑定的SDL游戏引擎。
PyOpenGL是OpenGL图形编程的Python绑定接口。OpenGL可以安装在几乎所有的系统上，并且具有很好的图形性能，且具有跨平台性，能够在不同的操作系统之间工作。
直接在命令行输入：pip install pygame 和 pip install pyopengl。安装需要时间，可以用国内的镜像源来加速
在原先的命令后面加上-i http://pypi.douban.com/simple/ --trusted-host pypi.douban.com
若提示需要更新pip，就输入所提示的命令 python -m pip install --upgrade pip即可成功升级
2、代码
# -*- coding: cp936 -*-
from pylab import *
from PIL import Image
# If you have PCV installed, these imports should work
from PCV.geometry import homography, camera
from PCV.localdescriptors import sift
"""
This is the augmented reality and pose estimation cube example from Section 4.3.
"""
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
def my_calibration(sz):
"""
Calibration function for the camera (iPhone4) used in this example.
"""
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
# 计算特征
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, 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))
# 可视化投影后的点
im0 = array(Image.open('book_frontal.JPG'))
im1 = array(Image.open('book_perspective.JPG'))
#底部正方形的二维投影
figure()
imshow(im0)
plot(box_cam1[0, :], box_cam1[1, :], linewidth=3)
title('2D projection of bottom square')
axis('off')
#使用H对二维投影进行变换
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()
3、运行时出现的问题
本人在运行代码时，pygame窗口出现了闪退，无法显示我所绘制的图像。尝试在cmd命令行进行运行，结果出现了如下错误：
经过查阅资料，得知这个错误是freeglut和glut共存的缘故，它们俩定义了相同的方法，这个是动态链接库的重叠问题。然后在我的C:\Python27\Lib\site-packages\OpenGL\DLLS文件夹里面，保留glut32.vc9.dll，删除其它的所有，就可以成功运行了。
4、运行结果
