ジョブにラベルを付けるための座標系の要件ワールド座標系での点群データの使用センサーフュージョン向きの四元数と位置の計算 Ground Truth センサーフュージョン変換

座標系とセンサーフュージョンを理解する

点群データは、常に座標系に配置されます。この座標系は、車両や周囲を検知するデバイスに対してローカルな座標系のこともあれば、ワールド座標系のこともあります。Ground Truth の 3D 点群ラベル付けジョブを使用する場合、すべての注釈は、入力データの座標系を使用して生成されます。一部のラベル付けジョブのタスクタイプおよび機能では、ワールド座標系でデータを指定する必要があります。

このトピックでは、次のことを学習します。

ワールド座標系またはグローバルリファレンスフレームで入力データを提供する必要がある場合。
ワールド座標の概要と、点群データをワールド座標系に変換する方法。
センサーフュージョンを使用するときに、センサーとカメラの外部マトリックスを使用して姿勢データを指定する方法。

ジョブにラベルを付けるための座標系の要件

点群データがローカル座標系で収集された場合、データの収集に使用されたセンサーの外部マトリックスを使用して、ワールド座標系またはグローバル基準フレームに変換できます。点群データの外部メトリクスを取得できず、結果としてワールド座標系で点群を取得できない場合は、3D 点群オブジェクト検出とセマンティックセグメンテーションのタスクタイプ用に、ローカル座標系で点群データを指定できます。

オブジェクトの追跡用には、ワールド座標系で点群データを指定する必要があります。これは、複数のフレームにわたってオブジェクトを追跡する場合、自動運転車両自体がワールド内を移動しているので、すべてのフレームに基準ポイントが必要であるためです。

センサーフュージョン用のカメラデータを含める場合は、3D センサー (LiDAR センサーなど) と同じワールド座標系でカメラの姿勢を指定することをお勧めします。

ワールド座標系での点群データの使用

このセクションでは、グローバル基準フレームとも呼ばれるワールド座標系 (WCS) の概要と、ワールド座標系で点群データを指定する方法について説明します。

ワールド座標系とは

WCS またはグローバル基準フレームは、車両とセンサーの座標系が配置される固定のユニバーサル座標系です。例えば、複数の点群フレームが 2 つのセンサーから収集されたために、異なる座標系に配置されている場合、WCS を使用して、これらの点群フレームのすべての座標を 1 つの座標系に並進させ、すべてのフレームが同じ原点 (0,0,0) を持つようにすることができます。この変換は、並進ベクトルを使用して各フレームの原点を WCS の原点に並進させ、回転マトリックスを使用して 3 つの軸 (通常は x、y、z) を正しい向きに回転させることによって行われます。この剛体変換は、同次変換と呼ばれます。

ワールド座標系は、グローバルパスの計画、ローカリゼーション、マッピング、運転シナリオのシミュレーションにおいて重要です。Ground Truth は、ISO 8855 で定義されているような右手系の直交ワールド座標系を使用します。この座標系では、x 軸は自動車の動きに向かって前方、y 軸は左方で、z 軸は地面から上方を向いています。

グローバル基準フレームはデータによって異なります。一部のデータセットでは、最初のフレームの LiDAR 位置が原点として使用されます。このシナリオでは、すべてのフレームが最初のフレームを基準として使用し、デバイスの進行方向と位置は最初のフレームの原点付近になります。例えば、KITTI データセットには、最初のフレームがワールド座標の基準として含まれています。他のデータセットでは、原点とは異なるデバイス位置が使用されます。

これは GPS/IMU 座標系ではありません。GPS/IMU 座標系は通常、z 軸に沿って 90 度回転しています。点群データが GPS/IMU 座標系である場合 (オープンソースの AV KITTI データセットの OxTS など)、原点をワールド座標系 (通常は車両の基準座標系) に変換する必要があります。この変換を適用するには、データに変換メトリクス (回転マトリックスと並進ベクトル) を乗算します。これにより、データは元の座標系からグローバル基準座標系に変換されます。この変換の詳細については、次のセクションで説明します。

3D 点群データを WCS に変換する

Ground Truth では、点群データが既に選択した基準座標系に変換されていることを前提としています。例えば、センサー (LiDAR など) の基準座標系をグローバル基準座標系として選択できます。また、さまざまなセンサーから取得した点群を、センサーのビューから車両の基準座標系ビューに変換することもできます。点群データを WCS またはグローバル基準フレームに変換するには、センサーの外部マトリックス (回転マトリックスと並進ベクトルで構成) を使用します。

以上をまとめると、並進ベクトルと回転マトリックスを使用して外部マトリックスを構成し、これを使用してローカル座標系から WCS にデータを変換することができます。例えば、LiDAR の外部マトリックスは次のように構成できます。ここで、R は回転マトリックス、T は並進ベクトルです。


LiDAR_extrinsic = [R T;0 0 0 1]

例えば、自律運転の KITTI データセットには、各フレームに対する LiDAR の外部変換マトリックス用に回転マトリックスと並進ベクトルが含まれています。pykitti Python モジュールを使用して KITTI データをロードすることができます。データセットでは、dataset.oxts[i].T_w_imu によって i 番目のフレームに対する LiDAR の外部変換が得られ、これをそのフレーム内のポイントに乗算することで、ワールドフレームに変換できます (np.matmul(lidar_transform_matrix, points))。LiDAR フレーム内のポイントに LiDAR の外部マトリックスを乗算すると、ワールド座標に変換されます。ワールドフレーム内のポイントにカメラの外部マトリックスを乗算すると、カメラの基準フレームにおけるポイント座標が得られます。

次のコード例は、KITTI データセットの点群フレームを WCS に変換する方法を示しています。


import pykitti
import numpy as np

basedir = '/Users/nameofuser/kitti-data'
date = '2011_09_26'
drive = '0079'

# The 'frames' argument is optional - default: None, which loads the whole dataset.
# Calibration, timestamps, and IMU data are read automatically. 
# Camera and velodyne data are available via properties that create generators
# when accessed, or through getter methods that provide random access.
data = pykitti.raw(basedir, date, drive, frames=range(0, 50, 5))

# i is frame number
i = 0

# lidar extrinsic for the ith frame
lidar_extrinsic_matrix = data.oxts[i].T_w_imu

# velodyne raw point cloud in lidar scanners own coordinate system
points = data.get_velo(i)

# transform points from lidar to global frame using lidar_extrinsic_matrix
def generate_transformed_pcd_from_point_cloud(points, lidar_extrinsic_matrix):
    tps = []
    for point in points:
        transformed_points = np.matmul(lidar_extrinsic_matrix, np.array([point[0], point[1], point[2], 1], dtype=np.float32).reshape(4,1)).tolist()
        if len(point) > 3 and point[3] is not None:
            tps.append([transformed_points[0][0], transformed_points[1][0], transformed_points[2][0], point[3]])
       
    return tps
    
# customer transforms points from lidar to global frame using lidar_extrinsic_matrix
transformed_pcl = generate_transformed_pcd_from_point_cloud(points, lidar_extrinsic_matrix)

センサーフュージョン

Ground Truth は、最大 8 個のビデオカメラ入力による点群データのセンサーフュージョンをサポートします。この機能により、人間のラベルラーは同期されたビデオフレーム side-by-side で 3D 点群フレームを表示できます。センサーフュージョンを使用すると、ラベル付けのための詳細な視覚的コンテキストが得られるだけでなく、ワーカーは 3D シーンと 2D 画像で注釈を調整し、調整結果をもう一方のビューに投影することができます。次の動画は、LiDAR とカメラのセンサーフュージョンを使用した 3D 点群のラベル付けジョブを示しています。

最適な結果を得るには、センサーフュージョンを使用する場合、点群が WCS 内にある必要があります。Ground Truth は、センサー (LiDAR など)、カメラ、自動運転車両の姿勢情報を使用して、センサーフュージョン用の外部マトリックスと内部マトリックスを計算します。

外部マトリックス

Ground Truth は、センサー (LiDAR など) の外部マトリックスとカメラの外部/内部マトリックスを使用して、点群データの基準フレームおよびカメラの基準フレームとの間でオブジェクトを投影します。

例えば、3D 点群からカメラの画像平面にラベルを投影するために、Ground Truth は 3D ポイントを LiDAR 独自の座標系からカメラの座標系に変換します。通常は、まず LiDAR の外部マトリックスを使用して、3D ポイントを LiDAR 独自の座標系からワールド座標系 (またはグローバル基準フレーム) に変換します。次に、Ground Truthカメラの逆外部フレーム (グローバル基準フレームのポイントをカメラの基準フレームに変換します) を使用して、前のステップで取得したワールド座標系の 3D ポイントをカメラのイメージプレーンに変換します。LiDAR の外部マトリックスを使用して、3D データをワールド座標系に変換することもできます。3D データが既にワールド座標系に変換されている場合、最初の変換はラベルの並進に影響を与えず、ラベルの並進はカメラの逆外部マトリックスによってのみ決まります。投影されたラベルを視覚化するには、ビューマトリックスが使用されます。これらの変換とビューマトリックスの詳細については、「Ground Truth センサーフュージョン変換」を参照してください。

Ground Truth は、指定した LiDAR とカメラの姿勢データである heading (四元数の qx、qy、qz、qw) と position (x、y、z) を使用して、これらの外部マトリックスを計算します。車両の場合、通常、進行方向と位置はワールド座標系で車両の基準フレームに記述され、自動運転車両の姿勢と呼ばれます。カメラの外部マトリックスごとに、そのカメラの姿勢情報を追加できます。詳細については、「Pose」を参照してください。

内部マトリックス

Ground Truth は、カメラの外部マトリックスと内部マトリックスを使用して、ビューメトリクスを計算し、ラベルを 3D シーンとカメライメージとの間で変換します。Ground Truth は、カメラの焦点距離 (fx、fy) と光中心座標 (cx、cy) を使用して、カメラの内部マトリックスを計算します。詳細については、「内部マトリックスとゆがみ」を参照してください。

画像のゆがみ

画像のゆがみは、さまざまな理由で発生する可能性があります。例えば、バレルや魚眼の効果によってイメージがゆがむ場合があります。Ground Truth は、3D 点群ラベル付けジョブの作成時に指定した画像のゆがみを補正するためのゆがみ係数とともに内部パラメータを使用します。カメラ画像のゆがみが既に補正されている場合は、すべてのゆがみ係数を 0 に設定する必要があります。

Ground Truth が画像のゆがみを補正するために実行する変換の詳細については、「カメラキャリブレーション: 外部マトリックス、内部マトリックス、ゆがみ」を参照してください。

自動運転車両

自律運転アプリケーションのデータを収集するために、点群データの生成に使用される測定値が、車両 (自動運転車両) に搭載されたセンサーから取得されます。Ground Truth で 3D シーンと 2D 画像との間でラベル調整結果を投影するには、ワールド座標系での自動運転車両の姿勢が必要です。自動運転車両の姿勢は、位置座標と向きの四元数で構成されます。

Ground Truth は、自動運転車両の姿勢を使用して回転マトリックスと変換マトリックスを計算します。3 次元の回転は、一連の軸を中心とする 3 つの回転のシーケンスで表すことができます。理論的には、3D ユークリッド空間にまたがる任意の 3 つの軸で十分です。実際には、回転軸は基底ベクトルとして選択されます。3 つの回転は、グローバル基準フレーム (外部) 内で起こると想定されます。Ground Truth は、回転中のオブジェクトにアタッチされ、回転中のオブジェクトと共に移動する、身体中心の基準フレーム (内部) をサポートしていません。Ground Truth でオブジェクトを追跡するには、すべての車両が移動しているグローバル基準から計測する必要があります。Ground Truth の3D 点群のラベル付けジョブを使用する場合、z は回転軸 (外部回転) を示し、ヨーのオイラー角はラジアン (回転角度) で指定します。

Pose

Ground Truth では、3D ビジュアライゼーションとセンサーフュージョンに姿勢情報を使用します。マニフェストファイルで入力した姿勢情報は、外部マトリックスを計算するために使用されます。既に外部マトリックスがある場合は、それを使用してセンサーとカメラの姿勢データを抽出できます。

例えば、自律運転の KITTI データセットでは、pykitti Python モジュールを使用して KITTI データをロードすることができます。データセットでは、dataset.oxts[i].T_w_imu によって i 番目のフレームに対する LiDAR の外部変換が得られ、これをポイントに乗算することで、ワールドフレームに変換できます (matmul(lidar_transform_matrix, points))。この変換は、入力マニフェストファイルの JSON 形式での LiDAR の位置 (並進ベクトル) と進行方向 (四元数) に変換できます。i 番目のフレームの cam0 のカメラ外部変換は、inv(matmul(dataset.calib.T_cam0_velo, inv(dataset.oxts[i].T_w_imu))) で計算でき、これは cam0 の進行方向と位置に変換できます。


import numpy

rotation = [[ 9.96714314e-01, -8.09890350e-02,  1.16333982e-03],
 [ 8.09967396e-02,  9.96661051e-01, -1.03090934e-02],
 [-3.24531964e-04,  1.03694477e-02,  9.99946183e-01]]
 
origin= [1.71104606e+00,
          5.80000039e-01,
          9.43144935e-01]

         
from scipy.spatial.transform import Rotation as R

# position is the origin
position = origin 
r = R.from_matrix(np.asarray(rotation))

# heading in WCS using scipy 
heading = r.as_quat()
print(f"pose:{position}\nheading: {heading}")

ポジション

入力マニフェストファイルの position は、ワールドフレームを基準としたセンサーの位置を示します。デバイスの位置をワールド座標系で表せない場合は、ローカル座標での LiDAR データを使用できます。同様に、搭載型のビデオカメラの場合は、ワールド座標系での位置と進行方向を指定できます。カメラの位置情報がない場合は、(0, 0, 0) を使用してください。

位置オブジェクトのフィールドは次のとおりです。

x (浮動小数点) - 自動運転車両、センサー、またはカメラの位置の x 座標 (メートル単位)。
y (浮動小数点) - 自動運転車両、センサー、またはカメラの位置の y 座標 (メートル単位)。
z (浮動小数点) - 自動運転車両、センサー、またはカメラの位置の z 座標 (メートル単位)。

position JSON オブジェクトの例を次に示します。


{
    "position": {
        "y": -152.77584902657554,
        "x": 311.21505956090624,
        "z": -10.854137529636024
      }
}

見出し

入力マニフェストファイルの heading は、ワールドフレームを基準としたデバイスの向きを表すオブジェクトです。進行方向の値は四元数で指定する必要があります。四元数とは、測地線の球面特性と一致する向きを表したものです。センサーの進行方向をワールド座標で表せない場合は、恒等四元数 (qx = 0, qy = 0, qz = 0, qw = 1) を使用してください。同様に、カメラの場合は、四元数で進行方向を指定します。カメラの外部キャリブレーションパラメータを取得できない場合は、恒等四元数も使用してください。

heading オブジェクトのフィールドは次のとおりです。

qx (浮動小数点) - 自動運転車両、センサー、またはカメラの向きの x コンポーネント。
qy (浮動小数点) - 自動運転車両、センサー、またはカメラの向きの y コンポーネント。
qz (浮動小数点) - 自動運転車両、センサー、またはカメラの向きの z コンポーネント。
qw (浮動小数点) - 自動運転車両、センサー、またはカメラの向きの w コンポーネント。

heading JSON オブジェクトの例を次に示します。


{
    "heading": {
        "qy": -0.7046155108831117,
        "qx": 0.034278837280808494,
        "qz": 0.7070617895701465,
        "qw": -0.04904659893885366
      }
}

詳細については、「向きの四元数と位置の計算」を参照してください。

向きの四元数と位置の計算

Ground Truth では、すべての向き (進行方向) のデータが四元数で指定されている必要があります。四元数とは、回転を近似するために使用できる、測地線の球面特性と一致する向きを表したものです。オイラー角と比較すると、構成が簡単で、ジンバルロックの問題を回避しやすくなります。回転マトリックスと比較すると、よりコンパクトで、数値的に安定しており、効率的です。

回転マトリックスまたは変換マトリックスから四元数を計算できます。

単一の 4x4 剛性変換マトリックスではなく、ワールド座標系の回転マトリックス (軸の回転からなる) と並進ベクトル (または原点) がある場合は、回転マトリックスと並進ベクトルを直接使用して四元数を計算できます。scipy や pyqaternion などのライブラリが役立ちます。次のコードブロックは、これらのライブラリを使用して回転マトリックスから四元数を計算する例を示しています。


import numpy

rotation = [[ 9.96714314e-01, -8.09890350e-02,  1.16333982e-03],
 [ 8.09967396e-02,  9.96661051e-01, -1.03090934e-02],
 [-3.24531964e-04,  1.03694477e-02,  9.99946183e-01]]
 
origin = [1.71104606e+00,
          5.80000039e-01,
          9.43144935e-01]

         
from scipy.spatial.transform import Rotation as R
# position is the origin
position = origin 
r = R.from_matrix(np.asarray(rotation))
# heading in WCS using scipy 
heading = r.as_quat()
print(f"position:{position}\nheading: {heading}")

3D Rotation Converter などの UI ツールも便利です。

4x4 外部変換マトリックスがある場合、変換マトリックスは [R T; 0 0 0 1] という形式になります。R が回転マトリックス、T が並進ベクトルです。つまり、回転マトリックスと並進ベクトルを変換マトリックスから次のように抽出することができます。


import numpy as np

transformation 
= [[ 9.96714314e-01, -8.09890350e-02,  1.16333982e-03, 1.71104606e+00],
   [ 8.09967396e-02,  9.96661051e-01, -1.03090934e-02, 5.80000039e-01],
   [-3.24531964e-04,  1.03694477e-02,  9.99946183e-01, 9.43144935e-01],
   [              0,               0,               0,              1]]

transformation  = np.array(transformation )
rotation = transformation[0:3][0:3]
translation= transformation[0:3][3]

from scipy.spatial.transform import Rotation as R
# position is the origin translation
position = translation
r = R.from_matrix(np.asarray(rotation))
# heading in WCS using scipy 
heading = r.as_quat()
print(f"position:{position}\nheading: {heading}")

独自の設定で、自動運転車両の LiDAR センサーを基準とする GPS/IMU の位置と向き (緯度、経度、高度とロール、ピッチ、ヨー) を使用して、外部変換マトリックスを計算できます。例えば、pose = convertOxtsToPose(oxts) を使用して KITTI raw データから姿勢を計算し、OxTS データを 4x4 剛性変換マトリックスで指定されたローカルユークリッド姿勢に変換できます。次に、ワールド座標系の基準フレーム変換マトリックスを使用して、この姿勢変換マトリックスをグローバル基準フレームに変換できます。


struct Quaternion
{
    double w, x, y, z;
};

Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;

    return q;
}

Ground Truth センサーフュージョン変換

次の各セクションでは、指定した姿勢データを使用して実行される Ground Truth のセンサーフュージョン変換についてさらに詳しく説明します。

LiDAR の外部マトリックス

3D LiDAR シーンと 2D カメラ画像との間で投影を行うために、Ground Truth は、自動運転車両の姿勢と進行方向を使用して剛性変換投影メトリクスを計算します。Ground Truth は、回転と並進の単純なシーケンスを実行することで、ワールド座標の回転と並進を 3D 平面で計算します。

Ground Truth は、次のような進行方向の四元数を使用して回転メトリクスを計算します。

ここで、[x, y, z, w] は heading JSON オブジェクト、[qx, qy, qz, qw] のパラメータに対応しています。Ground Truth は、並進列ベクトルを T = [poseX, poseY, poseZ] として計算します。そのため、外部メトリクスは単に次のようになります。


LiDAR_extrinsic = [R T;0 0 0 1]

カメラキャリブレーション: 外部マトリックス、内部マトリックス、ゆがみ

幾何学的カメラキャリブレーションは、カメラの切除とも呼ばれるもので、画像またはビデオカメラのレンズとイメージセンサーのパラメータを推定します。これらのパラメータを使用して、レンズのゆがみを補正したり、オブジェクトのサイズをワールド単位で測定したり、シーン内のカメラの位置を決定したりできます。カメラのパラメータには、内部マトリックスとゆがみ係数があります。

カメラの外部マトリックス

カメラの姿勢が指定されている場合、Ground Truth は、3D 平面からカメラ平面への剛性変換に基づいて、カメラの外部マトリックスを計算します。この計算は「LiDAR の外部マトリックス」で使用される計算と同じですが、Ground Truth がカメラの姿勢(position と heading) を使用し、逆外部マトリックスを計算する点が異なります。


 camera_inverse_extrinsic = inv([Rc Tc;0 0 0 1]) #where Rc and Tc are camera pose components

内部マトリックスとゆがみ

ピンホールカメラや魚眼カメラなどの一部のカメラでは、写真に大きなゆがみが生じることがあります。このゆがみは、ゆがみ係数とカメラの焦点距離を使用して補正できます。詳細については、OpenCV のドキュメントの「Camera calibration With OpenCV」を参照してください。

Ground Truth で補正できるゆがみには、放射方向ゆがみと接線方向ゆがみの 2 種類があります。

放射方向ゆがみは、光線が光学的中心よりもレンズのエッジ近くでより大きく曲がる場合に発生します。レンズが小さいほど、ゆがみは大きくなります。放射方向ゆがみの存在は、バレルまたは魚眼効果の形で現れます。Ground Truth では、式 1 を使用して放射状ゆがみを補正します。

式 1:

接線方向ゆがみは、画像撮影に使用されたレンズが画像平面と完全に平行でないために発生します。これは式 2 で補正できます。

式 2:

入力マニフェストファイルでゆがみ係数を指定すると、Ground Truth でイメージのゆがみが補正されます。ゆがみ係数はすべて浮動小数点です。

k1、k2、k3、k4 - 放射方向ゆがみ係数。魚眼カメラモデルとピンホールカメラモデルの両方でサポートされています。
p1、p2 - 接線方向ゆがみ係数。ピンホールカメラモデルでサポートされています。

画像のゆがみが既に補正されている場合、入力マニフェストではすべてのゆがみ係数を 0 にする必要があります。

補正されたイメージを正しく再構成するために、Ground Truth は焦点距離に基づいてイメージの単位変換を行います。一般的な焦点距離を、両方の軸に指定されたアスペクト比 (1 など) と共に使用する場合、上の式の焦点距離は 1 つになります。この 4 つのパラメータを含むマトリックスは、インカメラ内部キャリブレーションマトリックスと呼ばれます。

ゆがみ係数は、使用するカメラの解像度に関係なく同じですが、キャリブレーションされた解像度から現在の解像度でスケールする必要があります。

以下は浮動小数点値です。

fx - x 方向の焦点距離。
fy - y 方向の焦点距離。
cx - 主点の x 座標。
cy - 主点の y 座標。

Ground Truth は、次のコードブロックに示すように、カメラの外部マトリックスとカメラの内部マトリックスを使用してビューメトリクスを計算し、3D シーンと 2D イメージの間でラベルを変換します。


def generate_view_matrix(intrinsic_matrix, extrinsic_matrix):
    intrinsic_matrix = np.c_[intrinsic_matrix, np.zeros(3)]
    view_matrix = np.matmul(intrinsic_matrix, extrinsic_matrix)
    view_matrix = np.insert(view_matrix, 2, np.array((0, 0, 0, 1)), 0)
    return view_matrix

ブラウザで JavaScript が無効になっているか、使用できません。

AWS ドキュメントを使用するには、JavaScript を有効にする必要があります。手順については、使用するブラウザのヘルプページを参照してください。

ドキュメントの表記規則

点群シーケンス入力マニフェストの作成

動画フレーム入力データ