3D点群は座標変換してから使う

3D点群を扱う際の座標変換の概要について、世界座標系と車体座標系の例を挙げて説明します。自動運転で車体座標系に変換する理由や、平行移動行列・回転行列を用いた具体的な変換式、PythonのOpen3Dによる実装例も紹介します。

本記事では、３D点群を扱う時に便利な座標変換の概要について説明します。

座標系のイメージ

3D点群の処理では、座標変換を行うことで、その後の計算を簡潔にできる場合があります。
ここでは例として、３D点群の処理でよく使われる、2つの主要な座標系を説明します。

世界座標系(左下図参照)
空間に固定された座標系です。
例えば、LiDAR搭載車を走行させて点群情報を取得する時、点群取得を開始した位置を原点とします。
車体座標系(右下図参照)
車体の位置を基準とした座標系です。
車体の中心などを原点とし、進行方向をx軸またはy軸と定めます。

座標変換を行う理由

自動運転では、車と障害物との相対的な位置関係を用いることが多いため、世界座標系の3D点群は、まず車体座標系に変換してから処理するのが一般的です。
予め世界座標系の3D点群を車体座標系に変換しておくことで、車の位置や向きによらず常に同じ処理で対応できるようになり、設計がシンプルになります。

具体例

自動運転には、事前にLiDARを搭載した車体を走行させ、周囲の地形や建物の情報を点群データで表した世界座標系の3D点群のマップを利用するものがあります。
ここでは、その方式において、車体が前方の障害物を回避する状況を例に説明します。
世界座標系をそのまま使う場合、常に移動する車体と、マップ上の障害物という、2つの要素の位置関係を繰り返し計算して車体の進行方向を決めるため、処理が煩雑になります。
車体座標系に変換した場合、車体の中心を原点とした時の障害物の位置を求めるため、進行方向の計算がシンプルになります。

座標変換の式

座標変換は、平行移動行列と回転行列を組み合わせて行います。
世界座標系から車体座標系に変換するための平行移動行列と回転行列は、世界座標系における車体の位置と向きが分かれば、公式に当てはめることで簡単に決定できます。
以下に例として、世界座標系において、車体が座標（xc, yc, zc）に位置し、x軸周りにθx、ｙ軸周りにθy、ｚ軸周りにθzだけ傾いている時の式を示します。

平行移動行列：

x軸周りの回転行列：

y軸周りの回転行列：

z軸周りの回転行列：

回転行列と平行移動行列を用いて、世界座標系のある点Ｐを、車体座標系の点P’に変換するには以下のように処理します。

そして、得られた車体座標系の点Ｐ’を用いて何かしらの処理を行い、目的の点Ｑを求めます。
点Qは、車体座標系のＱに元の変換行列の逆行列を掛けることで、世界座標系の点Q‘に戻すことができます。

これにより、処理の中身はすべて車体座標系で設計、実装をすることができつつ、世界座標系のLiDARの点群を扱うことができます。

実装例

以下に、PythonのOpen3Dを用いた回転処理の実装例を掲載します。

import open3d as o3d

import numpy as np
import math


# 座標変換する対象の点を1つ用意
point = np.asarray([[1.0, 0.0, 0.0]])
point_before = o3d.geometry.PointCloud()
point_before.points = o3d.utility.Vector3dVector(point)

# 回転角度の設定
theta_x = 0
theta_y = 0
theta_z = np.pi / 2

# 回転式
rotation_matrix_x = np.array([
    [1, 0, 0],
    [ 0, math.cos(theta_x), -math.sin(theta_x)],
    [ 0, math.sin(theta_x),  math.cos(theta_x)]])

rotation_matrix_y = np.array([
    [ math.cos(theta_y), 0,  math.sin(theta_y)],
    [0, 1, 0],
    [-math.sin(theta_y), 0, math.cos(theta_y)]])

rotation_matrix_z = np.array([
    [ math.cos(theta_z), -math.sin(theta_z), 0],
    [ math.sin(theta_z),  math.cos(theta_z), 0],
    [0, 0, 1]])

 

r_matrix = np.matmul(rotation_matrix_z, np.matmul(rotation_matrix_y, rotation_matrix_x))

# 回転の実行
point_after = point_before.rotate(r_matrix, center=(0, 0, 0))

# 回転結果の出力
print(np.array(point_after.points)) # 出力結果： [[6.123234e-17 1.000000e+00 0.000000e+00]]