camera_geometry.py

# -*- coding: utf-8 -*-
"""
Created on Thu Nov 24 21:49:10 2016

@author: ryuhei

このスクリプトは「カメラで世界を撮影する」ということが幾何学的にはどう定式化されるのかを説明するためのものです。
ピンホールカメラモデルにおいて、世界のある座標の1点から出た光線が、撮像素子のどの画素にヒットするのか、
ということを計算しています。点、ピンホール、撮像素子の物理的な位置関係を考えながら外部パラメタや内部パラメタを
色々変えてレンダリングしてみて、撮影される画像と照らし合わせてみてください。
"""

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D


def plot_3d(points, colors, edges, title):
    ps = points.T
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # 線の描画
    for edge in edges:
        x, y, z = ps[edge].T
        ax.plot(x, y, z, c='k')

    # 点の描画
    for p, c in zip(ps, colors):
        x, y, z = p
        ax.scatter(x, y, z, c=c)

    ax.set_xlabel('X (R)')
    ax.set_ylabel('Y (G)')
    ax.set_zlabel('Z (B)')
    plt.gca().set_aspect(1)
    plt.title(title)
    plt.show()


def plot_2d(points, colors, edges, image_size):
    points_2d = points[:2]
    ps = points_2d.T

    # 線の描画
    for edge in edges:
        x, y = ps[edge].T
        plt.plot(x, y, c='k')

    # 点の描画
    for point, c in zip(ps, colors):
        plt.plot(point[0], point[1], c=c, marker='o')

    plt.xlabel('x')
    plt.ylabel('y')
    x_min = -image_size[0] / 2.0
    x_max = image_size[0] / 2.0
    y_min = -image_size[1] / 2.0
    y_max = image_size[1] / 2.0
    plt.xlim(x_min, x_max)
    plt.ylim(y_min, y_max)
    plt.xticks(range(-50, 51, 10))
    plt.yticks(range(-50, 51, 10))
    plt.grid()
    plt.gca().set_aspect(1)
    plt.title("Points in the image frame")
    plt.show()


if __name__ == '__main__':
    # ベクトルは縦ベクトル
    # 座標系は右手系 （右手の親指、人差し指、中指がx, y, z軸）
    # 回転は右ねじ （軸の正方向に親指を向けたとき他の指が巻き付く方向が回転の正方向）

    # 点群の座標 （世界座標系）
    p_world = np.array([[0, 0, 0, 0, 1, 1, 1, 1],
                        [0, 0, 1, 1, 0, 0, 1, 1],
                        [0, 1, 0, 1, 0, 1, 0, 1]], dtype=np.float)
    #         0黒   1青  2緑  3水   4赤  5紫   6黄  7白
    colors = ['k', 'b', 'g', 'c', 'r', 'm', 'y', 'w']
    edges = [[0, 1], [1, 3], [3, 2], [2, 0],
             [4, 5], [5, 7], [7, 6], [6, 4],
             [0, 4], [1, 5], [3, 7], [2, 6]]

    # 点群の３Dプロット
    plot_3d(p_world, colors, edges, "Points in the world frame")

    # カメラの位置と姿勢を表すパラメタ （カメラ外部パラメタ）
    # 実態は原点をピンホールの位置とし、x軸を左、y軸を下、z軸を前の方向として定義した座標系。
    t_world = np.array([[3.0],  # カメラの位置 (x, y, z)^T
                        [0.5],
                        [0.5]])
    R_world = np.array([[0.0, 0.0, -1.0],  # カメラ姿勢を表す行列。3つの列ベクトルが左から
                        [-1.0, 0.0, 0.0],  # x, y, z軸（基底）の方向を表している。各ベク
                        [0.0, 1.0, 0.0]])  # トルは長さが1で互いに直交している（直交行列）。

    # 点群の座標を、世界座標系からカメラ座標系に座標変換する
    p_camera = R_world.T.dot(p_world - t_world)
    print(p_camera)

    plot_3d(p_camera, colors, edges, "Points in the camera frame")

    # カメラの撮像素子の位置と画素の大きさ （内部パラメタ）
    image_size = (100, 100)    # 撮像素子中の画素の個数（画像の解像度） （x, y）
    pixel_size = (0.01, 0.01)  # 撮像素子中の1画素の1辺の物理的な長さ（画像の分解能） (x, y)
    f = -1  # 撮像素子のピンホールからの物理的な距離。ピンホールから遠いとズームイン、近いとズームアウト

    # 各点から出てピンホールを通過する光線が撮像素子とぶつかる点の、カメラ座標系における座標を計算する
    ray_camera = p_camera / p_camera[2]  # zを変数とする光線の係数 (a, b, 1)z
    p_on_sensor_camera = ray_camera * f  # z = f （撮像素子の位置）における光線の座標

    # 撮像素子上の点の座標をカメラ座標系から撮像素子上の画素を単位とする座標系に変換する
    dx, dy = 1 / pixel_size[0], 1 / pixel_size[1]
    A_camera = np.array([[dx, 0],
                         [0, dy]], dtype=np.float)
    p_sensor = A_camera.dot(p_on_sensor_camera[:2])

    plot_2d(p_sensor, colors, edges, image_size)
    print(p_sensor)