Pythonで三次元座標を扱うvol.1 〜ベクトル・行列の基本操作〜

年末ですね。寒さで引きこもりがはかどります。

さて。最近、カメラと座標変換の勉強をしながらPythonで動作確認するということをやっているので、そのときに必要になった三次元座標を扱うテクニックについてまとめてみたいと思います。

検索してみると空間座標を扱うためのモジュールがいくつか開発はされているようですが、OpenCVやmatplotlibと連携させることや知名度（＝資料の多さ）を考慮して、ここではnumpyを使った実装を紹介していきます。

この記事ではベクトル・行列の基本的な演算について紹介します。次の記事では3次元情報を可視化する方法を紹介しているので、よければそちらも読んでみてください。

kamino.hatenablog.com

環境

一応、筆者の環境を。

Python : 3.6.3
numpy : 1.13.3

サンプルコードを実行するときはいつものおまじないでモジュールをインポートしておいてください。

import numpy as np

ベクトル

ベクトルの定義

ベクトルはnumpy.arrayを使って表現します。

$\begin{bmatrix}1 \\ 2 \\ 3\end{bmatrix}$

vec = np.array([1, 2, 3])
print(vec)

複数のベクトルを管理するときは、要素の追加・削除を頻繁に行う場合はリスト、そうでない場合は多次元のnumpy.ndarrayを使うと楽な場面が多い気がします。

raw = [
    [1, 2, 3],
    [2, 3, 4],
    [3, 4, 5],
    [4, 5, 6],
]
# np.arrayのリストを作る場合
vecs = list(map(lambda v: np.array(v), raw))
# np.ndarrayにする場合
vecs = np.array(raw)

ベクトルの加減算

numpy.arrayに演算子が定義されているので、数値の加減算と同じように書くことができます。

$\begin{bmatrix}2 \\ 3 \\ 4\end{bmatrix} - \begin{bmatrix}1 \\ 2 \\ 3\end{bmatrix} = \begin{bmatrix}1 \\ 1 \\ 1\end{bmatrix}$

vec_a = np.array([2, 3, 4])
vec_b = np.array([1, 2, 3])

print(vec_a - vec_b)

ベクトルのスカラ倍

numpy.arrayに演算子が定義されているので、数値の乗除算と同じように書けます。

$2 \cdot \begin{bmatrix}1 \\ 2 \\ 3\end{bmatrix} = \begin{bmatrix}2 \\ 4 \\ 6\end{bmatrix}$

vec = np.array([1, 2, 3])
print(2*vec)

ベクトルの正規化

なぜかnumpyには専用の関数が用意されていないようです。

仕方ないのでベクトルの長さ（L2ノルム）を計算して、それで各成分を割って求めます。

$\vec{v} = \begin{bmatrix}1 \\ 2 \\ 2\end{bmatrix}, \ \ \ |\vec{v}| = 3$

vec = np.array([1, 2, 2])
print(vec/np.linalg.norm(vec))

ベクトルの内積

numpy.dot()を使います。 Python3.5以降は@演算子が使えるようです。

$\begin{bmatrix}1 \\ 2 \\ 3\end{bmatrix}\cdot \begin{bmatrix}4 \\ 5 \\ 6\end{bmatrix} = 32$

vec_a = np.array([1, 2, 3])
vec_b = np.array([4, 5, 6])

print(np.dot(vec_a, vec_b))
# or if you use python3.5<=
print(vec_a@vec_b)

ベクトルの外積

numpy.cross()を使います。

$\begin{bmatrix}1 \\ 2 \\ 3\end{bmatrix} \times \begin{bmatrix}4 \\ 5 \\ 6\end{bmatrix} = \begin{bmatrix}-3 \\ 6 \\ -3\end{bmatrix}$

vec_a = np.array([1, 2, 3])
vec_b = np.array([4, 5, 6])

print(np.cross(vec_a, vec_b))

行列

3×3行列を例に説明していきます。正方行列であれば同じように操作できます。

行列の定義

本来はnumpy.matrixを使うべきですが、型ごとに扱いを区別するのが面倒なので、行列も2次元のnumpy.ndarrayとして定義してしまいます。これでも基本的な演算をする上では全く問題ありません。

$\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix}$

mat = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
print(mat)

また、単位行列の場合はnumpy.identity()が使えます。

$\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$

mat = np.identity(3)
print(mat)

行列の加減乗除

+-*/演算子を使うと同じ位置にある要素同士で値が計算された行列が計算できます。

$\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix} + \begin{bmatrix} 1 & 4 & 7 \\ 2 & 5 & 8 \\ 3 & 6 & 9 \end{bmatrix} = \begin{bmatrix} 2 & 6 & 10 \\ 6 & 10 & 14 \\ 10 & 14 & 18 \end{bmatrix}$

mat_a = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
mat_b = np.array([[1, 4, 7], [2, 5, 8], [3, 6, 9]])

print(mat_a + mat_b)

また、オペランドの一方がスカラの場合、全ての要素がその値になった同じサイズの行列にしたときと同じ結果になります。

$\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix} + \begin{bmatrix} 2 & 2 & 2 \\ 2 & 2 & 2 \\ 2 & 2 & 2 \end{bmatrix} = \begin{bmatrix} 3 & 4 & 5 \\ 6 & 7 & 8 \\ 9 & 10 & 11 \end{bmatrix}$

mat = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
print(mat + 2)

行列の転置

numpy.transpose()を使います。

$\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix}^T = \begin{bmatrix} 1 & 4 & 7 \\ 2 & 5 & 8 \\ 3 & 6 & 9 \end{bmatrix}$

mat = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
print(np.transpose(mat))

行列の積

ベクトルの積と同じく、numpy.dot()もしくは@演算子を使います。 *演算子は同じ位置にある要素同士の掛け算になってしまうので注意が必要です。

$\begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix} \begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} = \begin{bmatrix} 3 & 1 & 2 \\ 6 & 4 & 5 \\ 9 & 7 & 8 \end{bmatrix}$

mat_a = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
mat_b = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])

print(np.dot(mat_a, mat_b))
# or if you use python3.5<=
print(mat_a@mat_b)

行列とベクトルの積

これもnumpy.dot()もしくは@演算子です。

$\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 1 \\ 2 \\ 3 \end{bmatrix} = \begin{bmatrix} 2 \\ 3 \\ 1 \end{bmatrix}$

mat = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
vec = np.array([1, 2, 3])

print(np.dot(mat, vec))
# or if you use python3.5<=
print(mat@vec)

行列式

numpy.linalg.det()を使います。

$\begin{vmatrix}1 & 2 & 0 \\ 0 & 1 & 2 \\ 1 & 0 & 2\end{vmatrix} = 6$

mat = np.array([[1, 2, 0], [0, 1, 2], [1, 0, 2]])
print(np.linalg.det(mat))

逆行列

numpy.linalg.inv()を使います。

$\begin{bmatrix}1 & 2 & 0 \\ 0 & 1 & 2 \\ 1 & 0 & 2\end{bmatrix}^{-1} = \frac{1}{6}\begin{bmatrix}2 & -4 & 4 \\ 2 & 2 & -2 \\ -1 & 2 & 1\end{bmatrix}$

mat = np.array([[1, 2, 0], [0, 1, 2], [1, 0, 2]])
print(np.linalg.inv(mat))

ただし、連立一次方程式の解を求めることが目的のときはnumpy.linalg.solve()を使う方が高速になるようです（参考：https://d.hatena.ne.jp/sleepy_yoshi/20120513/p1）。

$\begin{eqnarray}\begin{bmatrix}1 & 2 & 0 \\ 0 & 1 & 2 \\ 1 & 0 & 2\end{bmatrix}b = \begin{bmatrix}3 \\ 3 \\ 3\end{bmatrix} \\ b= \begin{bmatrix}1 \\ 1 \\\1\end{bmatrix}\end{eqnarray}$

mat = np.array([[1, 2, 0], [0, 1, 2], [1, 0, 2]])
vec = np.array([3, 3, 3])
print(np.linalg.solve(mat, vec))

以上、Pythonでベクトルと行列を扱う方法を紹介しました。

かみのメモ

コンピュータビジョン・プログラムな話題中心の勉強メモ