PCA校准(alignment)两个对应的点集合，并且获得旋转矩阵R和转移矩阵T

本文的主要工作是验证【1】中通过PCA方法，对两个点集合alignment得到对应旋转矩阵R和转移矩阵t的方法。

代码的主要流程如下所示：
1. 通过椭圆的公式计算出椭圆边的点的坐标，并作为点集合$A$。这里需要注意的是，需要对椭圆的公式进行一下处理。原始的椭圆公式为：

$$\frac{x^2}{a^2} + \frac{y^2}{b^2} = 1$$
可以令， $x=a\cos(\theta)$, $y=b\sin(\theta)$
则
$$\frac{x^2}{a^2} + \frac{y^2}{b^2} = 1\\ \frac{(a\cos(\theta))^2}{a^2} + \frac{(b\sin(\theta))^2}{b^2} = 1 \\ \cos(\theta)^2+\sin(\theta)^2=1$$
2. 手动生成旋转矩阵R和转移矩阵t,通过旋转矩阵获得点集合B
3. 通过如下图所示的流程图，生成新的旋转矩阵R和转移矩阵t

import numpy as np
import matplotlib.pyplot as plt

def draw_ellipse(a=5,b=3,plt_marker='x',plt_linestyle='-',size=10):
    fig = plt.figure()
    ax = fig.add_subplot(111)
    theta = np.arange(0,2*np.pi,np.pi/size)
    ax.set_xlim([-10,10])
    ax.set_ylim([-10,10])
    y = b*np.sin(theta)
    x = a*np.cos(theta)
    plt.plot(x,y,'b^')
    points = np.mat(np.random.rand(3,2*size))
    points[0,:] = x
    points[1,:] = y
    points[2,:] = 1
    return points

def points_center(points):
    return np.mean(points,axis=1)

def show_eigen_vector(points,eigen_val,eigen_vec):
    x1 = [points[0,0], points[0,0]+eigen_vec[1, 0]]
    y1 = [points[1,0], points[1,0]+eigen_vec[1, 1]]
    x2 = [points[0,0], points[0,0]+eigen_vec[0, 0]]
    y2 = [points[1,0], points[1,0]+eigen_vec[0, 1]]
    plt.plot(x1,y1)
    plt.plot(x2,y2)

def max_eigen_vec_val(eigen_val,eigen_vec):
    max_index = 0
    max_val = eigen_val[0]
    for index in range(len(eigen_val)):
        if max_val < eigen_val[index]:
            max_index = index
    return eigen_vec[max_index,:]

if __name__=='__main__':
    theta = np.pi/5
    R = np.mat([[np.cos(theta), np.sin(theta), 1],
                [-np.sin(theta),np.cos(theta), 2],
                [0,0,1]])
    print("The transform mat: ")
    print(R)
    points_a =draw_ellipse(size=50)
    points_b = np.dot(R,points_a)
    plt.plot(points_b[0,:],points_b[1,:],'ro')
    cov_a = np.cov(points_a)
    center_point_b = points_center(points_b)
    points_b = points_b - center_point_b
    cov_b = np.cov(points_b)

    center_point_a = points_center(points_a)
    eigen_val_a,eigen_vec_a = np.linalg.eig(cov_a)
    show_eigen_vector(center_point_a,eigen_val_a,eigen_vec_a)
    eigen_val_b,eigen_vec_b = np.linalg.eig(cov_b)
    show_eigen_vector(center_point_b,eigen_val_b,eigen_vec_b)

    max_eigen_vec_a = max_eigen_vec_val(eigen_val_a,eigen_vec_a)
    max_eigen_vec_b = max_eigen_vec_val(eigen_val_b,eigen_vec_b)

    theta_a = np.math.atan2(max_eigen_vec_a[1],max_eigen_vec_a[0])
    theta_b = np.math.atan2(max_eigen_vec_b[1],max_eigen_vec_b[0])
    delta_theta = theta_b-theta_a

    R_2 = np.mat([[np.cos(delta_theta), np.sin(delta_theta), 0],
                [-np.sin(delta_theta), np.cos(delta_theta), 0],
                [0, 0, 1]])
    t_2 = np.mat([[1, 0, center_point_b[0]-center_point_a[0]],
                [0, 1, center_point_b[1]-center_point_a[1]],
                [0, 0, 1]])

    new_points_b = np.dot(R_2,points_a)
    new_points_b = np.dot(t_2,new_points_b)

    plt.plot(new_points_b[0, :], new_points_b[1, :], 'bx')
    print("The result: ")
    print(np.dot(t_2,R_2))
    plt.show()

运行结果为：

蓝色的代表集合A，红色的代表集合B

通过所计算出来的R和t，得到新的集合B’和原来的集合B重合在了一起。

缺点

假设点集合是一个圆，则集合的所有的特征值的大小是一样的，本文方法无法在两个点集合之间进行变换。

【1】Bellekens, Ben & Spruyt, Vincent & Berkvens, Raf & Weyn, Maarten. (2014). A Survey of Rigid 3D Pointcloud Registration Algorithms.