Python机器学习实战第四章 照相机模型与增强现实

照相机模型与增强现实

为了处理三维图像和平面图像之间的映射，需要在映射中加入照相机产生图像的投影过程，我们将讨论如何确定照相机的参数，以及在增强现实（AR）等方面的具体应用。

针孔照相机模型

针孔照相机模型（射影照相机模型），在光线投影到图像平面之前，从照相机中心C经过（唯一一个点），模型如下图。
图像坐标轴和三维坐标系中的x轴、y轴对齐平行，光轴和z轴（也就是说z轴就是光轴）一致，在投影之前通过旋转和平移变换，在坐标系中加入三维点，会出现完整的投影变换。

针孔照相机中，三维点X投影为图像点x（齐次坐标表示），也就是把一个三维立体图形投影为二维图像，关系可由如下：
$$\lambda x=PX$$
P为3*4的投影矩阵，图像点的系数为逆深度，三位点X=[X,Y,Z,W]齐次坐标系，根据矩阵运算，行向量应为4

照相机矩阵

照相机矩阵可以分解为：$P=K[R|t]$
R为旋转矩阵，t为三维平移向量，内标定矩阵K描述照相机的投影性质，大多数情况下，K可表示为：$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$
f是图像平面和C点的距离，即焦距，另一个参数是光心$\mathrm{c}=\left[{\mathrm{c}}_{\mathrm{x}},{\mathrm{c}}_{\mathrm{y}}\right]$，是z轴和图像平面的交点（在二维平面上），常接近于图像宽度和高度的一半，唯一未知的变量是焦距。

三维点的投影

根据三维点和图像点的关系，及三维投影的定义，可以写出投影的函数。

#导入线性代数部分
#导入线性代数部分
from scipy import linalg
from numpy import *
from matplotlib import pyplot as plt


class Camera(object):
    """ Class for representing pin-hole cameras. """
    
    def __init__(self,P):
        """ Initialize P = K[R|t] camera model. """
        self.P = P
        # 标定矩阵
        self.K = None # calibration matrix
        self.R = None # rotation
        self.t = None # translation
        # 照相机中心
        self.c = None # camera center
        
    
    def project(self,X):
        """    Project points in X (4*n array) and normalize coordinates. """
        
        #定义和坐标归一化 
        # (3,n)
        x = dot(self.P,X)
        for i in range(3):
            x[i] /= x[2]    
        return x
        
points = loadtxt('house.p3d').T
points = vstack((points,ones(points.shape[1])))
# 相机内参
P = hstack((eye(3),array([[0],[0],[-10]])))
cam = Camera(P)
x = cam.project(points)

# 画出投影
plt.figure()
plt.plot(x[0],x[1],'k.')
plt.show()

运行结果如下（数据取自https://www.robots.ox.ac.uk/~vgg/data/mview/）：

刚开始看可能会不理解，为什么需要相机？我们可以这么看：所有的点构成了一组点云，具体怎么看这组点云要看相机的位置，然后P就是相机的位置。如果没有相机，那么我们自然也就无法看到东西，如果诸位接触过游戏制作，应该很容易理解这一点。

制作围绕的三维向量，模拟旋转的投影:

from numpy import *
import scipy
from matplotlib import pyplot as plt


class Camera(object):
    """ Class for representing pin-hole cameras. """
    
    def __init__(self,P):
        """ Initialize P = K[R|t] camera model. """
        self.P = P
        # 标定矩阵
        self.K = None # calibration matrix
        self.R = None # rotation
        self.t = None # translation
        # 照相机中心
        self.c = None # camera center
        
    
    def project(self,X):
        """    Project points in X (4*n array) and normalize coordinates. """
        
        #定义和坐标归一化 
        # (3,n)
        x = dot(self.P,X)
        for i in range(3):
            x[i] /= x[2]    
        return x
    def rotation_matrix(self,a):
        """    Creates a 3D rotation matrix for rotation
        around the axis of the vector a. """
        
        # 旋转矩阵
        R = eye(4)
        R[:3,:3] = scipy.linalg.expm([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
        return R
        
points = loadtxt('house.p3d').T
points = vstack((points,ones(points.shape[1])))
# 相机内参
P = hstack((eye(3),array([[0],[0],[-10]])))
cam = Camera(P)
x = cam.project(points)

# 画出投影
plt.figure()
plt.plot(x[0],x[1],'k.')
plt.show()

# 研究相机的移动如何影响投影
# 随机生成一个相机矩阵
r = random.rand(3)
# 旋转矩阵
rot = cam.rotation_matrix(r)

# 显示旋转的投影
plt.figure()
for t in range(10):
    cam.P = dot(cam.P,rot)
    x = cam.project(points)
    plt.plot(x[0],x[1],'k.')


plt.show()

运行结果：

可能有一些旋转的不是很多或者角度不对，就会导致效果不明显，这个应该是比较容易得到明显的效果的，我们可以看到点云周围有明显的旋转的痕迹。甚至于中心都出现了“风眼”。

照相机矩阵的分解

相机标定的过程中，我们可能会需要从结果来反推相机参数，也即给定照相机矩阵P，我们需要恢复内参数K以及照相机位置t和姿势R。那么我们可以使用RQ因子分解矩阵。

直接上代码:

from numpy import *
import scipy
from matplotlib import pyplot as plt


class Camera(object):
    """ Class for representing pin-hole cameras. """
    
    def __init__(self,P):
        """ Initialize P = K[R|t] camera model. """
        self.P = P
        # 标定矩阵
        self.K = None # calibration matrix
        self.R = None # rotation
        self.t = None # translation
        # 照相机中心
        self.c = None # camera center
        
    
    def project(self,X):
        """    Project points in X (4*n array) and normalize coordinates. """
        
        #定义和坐标归一化 
        # (3,n)
        x = dot(self.P,X)
        for i in range(3):
            x[i] /= x[2]    
        return x
    def rotation_matrix(self,a):
        """    Creates a 3D rotation matrix for rotation
        around the axis of the vector a. """
        
        # 旋转矩阵
        R = eye(4)
        R[:3,:3] = scipy.linalg.expm([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
        return R
    
    def factor(self):
        """    Factorize the camera matrix into K,R,t as P = K[R|t]. """
        
        # 分解前三列
        K,R = scipy.linalg.rq(self.P[:,:3])
        # 分解后三列
        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 is its own inverse
        self.t = dot(linalg.inv(self.K),self.P[:,3])
        
        return self.K, self.R, self.t
        
        
K = array([[1000,0,500],[0,1000,300],[0,0,1]])
tmp = hstack((eye(3),array([[0],[0],[-10]])))
P = dot(K,tmp)
cam = Camera(P)
print(cam.factor())

运行结果：

可以看到有三个矩阵，每个结果都符合我们的预期。

计算照相机中心

公式：
$$\mathrm{C}=-{\mathrm{R}}^{\mathrm{t}}\mathrm{t}$$
与内标定矩阵K无关。

下面代码计算照相机的中心：

    def center(self):
        """    Compute and return the camera center. """
        
        if self.c is not None:
            return self.c
        else:
            # Compute c by factoring
            self.factor()
            self.c = -np.dot(self.R.T,self.t)
            return self.c

这段代码要放在类里面，此处不再展示运行结果。

相机标定

由于我的电脑上没有办法进行交互式标定，且相机标定现在已经有非常成熟且完善的方法了（例如matlab），因此此处只是简单介绍。
标定照相机是指计算该照相机的内参数，即计算矩阵K，K的计算唯一的未知参数是焦距。
步骤如下：
• 测量你选定矩形标定物体的边长 dX 和 dY；

• 将照相机和标定物体放置在平面上，使得照相机的背面和标定物体平行，同时物
体位于照相机图像视图的中心，你可能需要调整照相机或者物体来获得良好的对
齐效果；

• 测量标定物体到照相机的距离 dZ；

• 拍摄一副图像来检验该设置是否正确，即标定物体的边要和图像的行和列对齐；

• 使用像素数来测量标定物体图像的宽度和高度 dx 和 dy。

根据相似三角形原则:
$${\mathrm{f}}_{\mathrm{x}}=\frac{\mathrm{dx}}{\mathrm{dX}}\mathrm{dZ},{\mathrm{f}}_{\mathrm{y}}=\frac{\mathrm{dy}}{\mathrm{dX}}\mathrm{dZ}$$
如果标定板（见下图）是没问题的，那么就可以根据标定板的参数来计算相机的参数。

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

如果图像中包含平面状的标记物，并且对照相机进行了标定，那么可以计算出照相机的姿态（旋转和平移）。
先提取SIFT特征，再使用RANSAC算法稳健地估计单应性算法。
由于这一部分比较复杂，因此很大程度上参考了书上的代码：

from numpy import *
import scipy
from matplotlib import pyplot as plt
import cv2


class Camera(object):
    """ Class for representing pin-hole cameras. """
    
    def __init__(self,P):
        """ Initialize P = K[R|t] camera model. """
        self.P = P
        # 标定矩阵
        self.K = None # calibration matrix
        self.R = None # rotation
        self.t = None # translation
        # 照相机中心
        self.c = None # camera center
        
    
    def project(self,X):
        """    Project points in X (4*n array) and normalize coordinates. """
        
        #定义和坐标归一化 
        # (3,n)
        x = dot(self.P,X)
        for i in range(3):
            x[i] /= x[2]    
        return x
    def rotation_matrix(self,a):
        """    Creates a 3D rotation matrix for rotation
        around the axis of the vector a. """
        
        # 旋转矩阵
        R = eye(4)
        R[:3,:3] = scipy.linalg.expm([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
        return R
    
    def factor(self):
        """    Factorize the camera matrix into K,R,t as P = K[R|t]. """
        
        # 分解前三列
        K,R = scipy.linalg.rq(self.P[:,:3])
        # 分解后三列
        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 is its own inverse
        self.t = dot(linalg.inv(self.K),self.P[:,3])
        
        return self.K, self.R, self.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

# 生成齐次坐标
def make_homog(points):
    """Convert points to homogeneous coordinates."""
    return vstack((points,ones((1,points.shape[1])))) 

def normalize(points):
    """ Normalize a collection of points in 
        homogeneous coordinates so that last row = 1. """

    for row in points:
        row /= points[-1]
    return points

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




im0 = cv2.imread('book_frontal.JPG')
im1 = cv2.imread('book_perspective.JPG')

# 计算特征点
sift = cv2.SIFT_create()
l0, d0 = sift.detectAndCompute(im0, None)
l1, d1 = sift.detectAndCompute(im1, None)

# 特征匹配
bf = cv2.BFMatcher(cv2.NORM_L1, crossCheck=True)
matches = bf.match(d0, d1)
matches = sorted(matches, key=lambda x: x.distance)

# 计算单应性矩阵
src_pts = array([l0[m.queryIdx].pt for m in matches])
dst_pts = array([l1[m.trainIdx].pt for m in matches])
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)

# 标定矩阵
K = my_calibration((747, 1000))

# 3D points at plane z=0 with sides of length 0.2
box = cube_points([0, 0, 0.1], 0.1)

# Project bottom square in first image
cam1 = Camera(hstack((K, dot(K, array([[0], [0], [-1]])))))
# 第一个图像的底部平面

box_cam1 = cam1.project(make_homog(box[:, :5]))

# 用H矩阵变换
box_trans = normalize(dot(H, box_cam1))

# 第二个图像的底部平面
cam2 = 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(make_homog(box))

# 绘制
im0 = cv2.cvtColor(im0, cv2.COLOR_BGR2RGB)
im1 = cv2.cvtColor(im1, cv2.COLOR_BGR2RGB)

plt.figure()
plt.imshow(im0)
plt.plot(box_cam1[0, :], box_cam1[1, :], linewidth=3)
plt.title('2D projection of bottom square')
plt.axis('off')

plt.figure()
plt.imshow(im1)
plt.plot(box_trans[0, :], box_trans[1, :], linewidth=3)
plt.title('2D projection transfered with H')
plt.axis('off')

plt.figure()
plt.imshow(im1)
plt.plot(box_cam2[0, :], box_cam2[1, :], linewidth=3)
plt.title('3D points projected in second image')
plt.axis('off')

plt.show()

运行结果：



可以看到运行结果很不错，基本上是完美的符合了书上的样子，但是有一点需要注意一下，书上说，标定相机的分辨率是2592×1936，但是这两张图片明显不符合。这应该是由于作者标定的相机和拍摄上述照片的相机是同一款所导致的，如果是同一款相机的话，那么自然不用修改参数，直接计算即可。
还有就是，我们此处使用的点都是齐次坐标，要修改的话也应当注意这一点。

增强现实

增强现实（Augmented Reality，AR）是将物体和相应信息放置在图像数据上的一系列操作的总称。最经典的例子是放置一个三维计算机图形学模型，使其看起来属于该场景；如果在视频中，该模型会随着照相机的运动很自然地移动。
下面基本上就是api调用了，不再介绍。
此处仅提供代码以及效果：

# -*- coding: utf-8 -*-
import math
import pickle
import sys
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


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 = []
    # 底部
    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):
    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 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()
    # 围绕x轴将茶壶旋转90度，使z轴向上
    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, :]  # 改变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)


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

Rt = dot(linalg.inv(K), cam2.P)
setup()
draw_background("D:\\Python\\chapter4\\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()

小结

这一章还是讲了挺多东西的。首先就是相机的模型，以及内参外参。然后我们如何去计算这些参数。再然后给定一个相机，我们该如何去修正参数。最后是我们拿到了这些相机的参数之后，我们可以做什么。
总体而言，模型、标定和姿态估计比较重要，增强现实只是简单介绍了一下。如果以后想要做这个方向的话倒是可以细看，不然感觉这一章似乎还是留给做相机（产品）的人去看比较好。