3D ターゲット検出の基礎知識
3D 空間でオブジェクトを記述する方法
-
位置は
xyz座標です -
サイズ
lwh は、長さ、幅、高さ、つまり 3D ボックスのサイズ (x_size, y_size, z_size) で、通常、オブジェクトの 3D ボックスの x、y、z 軸に沿った長さとして定義されます。機首方位角ヨー角は 0 です。 -
姿勢の3つの表現方法
：オイラー角、回転行列、四元数-
オイラー角は
ピッチ角、ヨー角（ヨー角）、ロール角に分けられます。車両などの地上の物体は一般に反転したり傾いたりしない
ため、通常は進行方向の角度のみを考慮する必要があります。
機首角度の定義: 重力軸として軸を選択し、重力軸に垂直な平面上の基準方向を選択すると、基準方向の機首角度ヨーが 0 になり、時計回りに機首角度が増加します。下図に示すように、z 軸が重力軸、基準方向が y 軸、車両前方方向と基準方向 y 軸とのなす角がヨーとなります。以下の図の座標軸は車両座標系に基づいていることに注意してください。つまり、x 軸は前方、y 軸は左方向、z 軸は上方向です。
-
回転行列 ある
座標系の座標と別の座標系の表現の間の変換関係 -
クォータニオン
(w、x、y、z)、参照ポーズのさまざまな表現
-
空間座標変換
基準座標系における物体の姿勢を表現するには、角度変換関係( rotation)を利用できます。角度変化に加えて、基準系の原点との相対的な位置関係も存在します。一般に、平行移動行列( ) は、四元数と回転translation行列、変換行列、および複合変換行列を表現するために使用されます。convert_nuScenes.pyファイル コード
の定義と関係は、一貫した読み取りを容易にするために詳細に紹介されています。上記の内容は、以下に直接引用されています。三、main 函数
変換行列
既知空間P_{0}(x_{0}, y_{0}, z_{0})内の点 P 0 ( x 0 , y 0 , z 0 )P0( ×0、y0、z0)、次にP 0 P_{0}P0tx、ty、tz をX 軸、Y 軸、Z 軸に沿って移動します t_{x}、t_{y}、t_{z}t×、tはい、tz距離、点を取得P 1 ( x 1 , y 1 , z 1 ) P_{1}(x_{1}, y_{1}, z_{1})P1( ×1、y1、z1)の場合、このプロセスは次のように表すことができますP 1 = 翻訳 M 行列 ⋅ P 0 P1 = TranslationMatrix \cdot P0P1_ _=翻訳マトリックス_ _ _ _ _ _ _ _ _ _ _ _ _ _⋅P 0、つまりP 0 P_{0}P0変換行列を左に乗算してP 1 P_{1}を取得しますP1
[ x 1 y 1 z 1 1 ] = [ x 0 + txy 0 + tyz 0 + tz 1 ] = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] [ x 0 y 0 z ] \ left[\begin{array}{c}\mathrm{x}_{1}\\\mathrm{y}_{1}\\\mathrm{z}_{1}\\1 \end{array }\ right]=\left[\begin{array}{c}\mathrm{x}_{0}+\mathrm{t}_{\mathrm{x}}\\\mathrm{y}_{0} +\ mathrm{t}_{\mathrm{y}}\\\mathrm{z}_{0}+\mathrm{t}_{\mathrm{z}}\\1 \end{array}\right] =\ left[\begin{array}{cccc}1&0&0&\mathrm{t}_{\mathrm{x}}\\0&1&0&\mathrm{t}_{\mathrm{y }} \\ 0 & 0 & 1 & \ mathrm {t}_{\mathrm {z}} \\ 0 & 0 & 0 & 1 \ end{array}\right]\left[\begin{array}{c } \mathrm{x}_{0}\\ \mathrm{y}_{0}\\\mathrm{z}_{0}\\1\end{配列}\right]
バツ1y1z11
=
バツ0+t×y0+tはいz0+tz1
=
100001000010t×tはいtz1
バツ0y0z01
特定の変数の場合
TranslationMatrix = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] TranslationMatrix = \left[\begin{array}{cccc} 1 & 0 & 0 & \ mathrm{; t}_{\mathrm{x}}\\0&1&0&\mathrm{t}_{\mathrm{y}}\\0&0&1&\mathrm{t}_{\mathrm{z }} \\ 0&0&0&1\end {配列}\右]翻訳マトリックス_ _ _ _ _ _ _ _ _ _ _ _ _ _=
100001000010t×tはいtz1
回転行列
4 つの要素と回転行列は、どちらも剛体の姿勢情報を記述するために使用されますが、表現形式が異なり、数式で変換できます。ここでは、4 つの要素を回転行列に変換する必要があります。回転行列は後で使用する必要があります。 対応する計算
: 既知の四元数
Q uaternion = ( w , x , y , z ) Quaternion = (w, x, y, z)四半期_ _ _ _ _ _ _=( w 、× 、よ、z )
は、次のように回転させることができます
。 y 2 − z 2 2 ( yz − wx ) 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 ] RotationMatrix = \left[\begin{array}{ccc} w^ {2}+x^{2}-y^{2}-z^{2} & 2(x yw z) & 2(x z+wy) \\ 2(x y+wz) & w^{2 }-x^{2}+y^{2}-z^{2} & 2(y zw x) \\ 2(x zw y) & 2(y z+wx) & w^{2}-x ^{2}-y^{2}+z^{2} \end{配列}\right]回転マトリックス_ _ _ _ _ _ _ _ _ _ _ _=
w2+バツ2−y2−z22 ( x y+wz ) _2 ( × z−wy ) _2 ( x y−wz ) _w2−バツ2+y2−z22 ( yz+w x )2 ( × z+wy ) _2 ( yz−w x )w2−バツ2−y2+z2
既知空間P_{0}(x_{0}, y_{0}, z_{0})内の点 P 0 ( x 0 , y 0 , z 0 )P0( ×0、y0、z0)、次にP 0 P_{0}P0X 軸、Y 軸、Z 軸を中心に異なる角度で回転するとき (回転関係が上記の 4 つの組で表されると仮定します)、点 P 1 ( x 1, y 1, z 1) P_ {1}(x_{1} を取得、y_{1}、z_{1})P1( ×1、y1、z1)の場合、このプロセスは次のように表される回転行列に変換できます。P 1 = [ Rotation Matrix 0 0 1 ] ⋅ P 0 P1 = \left[\begin{array}{cc} RotationMatrix & 0 \\ 0 & 1 \end {配列}\right] \cdot P0P1_ _=[回転マトリックス_ _ _ _ _ _ _ _ _ _ _ _001】⋅P 0、つまりP 0 P_{0}P0左側の回転行列を乗算してP 1 P_{1}を取得しますP1、今すぐ
P 1 = [ x 1 y 1 z 1 1 ] = [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) 2 ( xz + wy ) 0 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) 0 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 0 0 0 0 1 ] [ x 0 y 0 z P1 = \ left[\begin{array}{c}\mathrm{x}_{1}\\\mathrm{y}_{1}\\\mathrm{z}_{1}\\1\end{ array}\ right]=\left[\begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^{2}-\mathrm{ z}^ {2} & 2(\mathrm{xy}-\mathrm{wz}) & 2(\mathrm{xz}+\mathrm{wy}) & 0\\2(\mathrm{xy}+\mathrm{wz} ) & \mathrm{w}^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2}&2(\mathrm{ yz}-\ mathrm{wx})&0\\2(\mathrm{xz}-\mathrm{wy})&2(\mathrm{yz}+\mathrm{wx})&\mathrm{w}^{ 2}-\mathrm{ x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2}&0\\0&0&0&1\end{array}\right ]\left[\begin{array}{c}\ mathrm{x}_{0}\\\mathrm{y}_{0}\\\mathrm{z}_{0}\\1\end{配列}\right]P1_ _=
バツ1y1z11
=
w2+バツ2−y2−z22 ( xy+ｗｗｗ）2 ( xz−なぜか）02 ( xy−ｗｗｗ）w2−バツ2+y2−z22 ( yz+wx )02 ( xz+なぜか）2 ( yz−wx )w2−バツ2−y2+z200001
バツ0y0z01
複合変換行列
既知空間P_{0}(x_{0}, y_{0}, z_{0})内の点 P 0 ( x 0 , y 0 , z 0 )P0( ×0、y0、z0)、次にP 0 P_{0}P0X 軸、Y 軸、Z 軸を中心に異なる角度で回転するとき (回転関係が上記の 4 つの組で表されると仮定します)、点 P 1 ( x 1, y 1, z 1) P_ {1}(x_{1} を取得、y_{1}、z_{1})P1( ×1、y1、z1)、次に点P 1 P_{1}P1tx、ty、tz t_{x}、t_{y}、t_{z} をそれぞれ X 軸、Y 軸、Z 軸に沿って移動します。t×、tはい、tz距離、点を取得P 2 ( x 2 , y 2 , z 2 ) P_{2}(x_{2}, y_{2}, z_{2})P2( ×2、y2、z2)の場合、変換プロセスは次のようになります。
P 2 = [ x 2 y 2 z 2 1 ] = [ 1 0 0 tx 0 1 0 ty 0 0 1 tz 0 0 0 1 ] [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) ( xz + wy ) 0 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) 0 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y + z 2 0 0 0 0 1 ] [ x 0 y 0 z 0 1 ] P2 = \left[\begin{array}{c}\mathrm{x}_{2}\\\mathrm{y}_{2 } \ \\mathrm{z}_{2}\\1\end{array}\right]=\left[\begin{array}{cccc}1 & 0 & 0 & \mathrm{t}_{\mathrm {x }}\\0&1&0&\mathrm{t}_{\mathrm{y}}\\0&0&1&\mathrm{t}_{\mathrm{z}}\\0& 0 & 0 & 1 \end{array}\right] \left[\begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^ {2}-\mathrm{z}^{2} &2(\mathrm{xy}-\mathrm{wz})&2(\mathrm{xz}+\math{wy})&0\\2(\mathrm{xy}+\mathrm{wz}) & \mathrm{w }^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2 } &2(\mathrm{yz}-\mathrm{wx}) & 0 \\ 2(\mathrm{xz}-\mathrm{wy}) & 2(\mathrm{yz}+\mathrm{wx}) & \mathrm {w}^{2}-\mathrm{x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2}&0\\0&0&0&1\end {array}\right]\ left[\begin{array}{c}\mathrm{x}_{0}\\\mathrm{y}_{0}\\\mathrm{z}_{0}\\ 1 \end{array}\右]P2_ _=
バツ2y2z21
=
100001000010t×tはいtz1
w2+バツ2−y2−z22 ( xy+ｗｗｗ）2 ( xz−なぜか）02 ( xy−ｗｗｗ）w2−バツ2+y2−z22 ( yz+wx )02 ( xz+なぜか）2 ( yz−wx )w2−バツ2−y2+z200001
バツ0y0z01
それはP 0 P_{0}P0左側の回転行列を乗算し、次に左側の平行移動行列を乗算して、P 2 P_{2}を取得します。P2、このプロセスは次のように複合変換行列として表現できます。
変換行列 = 変換行列 ⋅ 回転行列 = [ w 2 + x 2 − y 2 − z 2 2 ( xy − wz ) 2 ( xz + wy ) tx 2 ( xy + wz ) w 2 − x 2 + y 2 − z 2 2 ( yz − wx ) ty 2 ( xz − wy ) 2 ( yz + wx ) w 2 − x 2 − y 2 + z 2 tz 0 0 0 1 ] TransformMatrix = TranslationMatrix \cdot RotationMatrix = \left[ \begin{array}{cccc}\mathrm{w}^{2}+\mathrm{x}^{2}-\mathrm{y}^{2}-\mathrm{z}^{2} & 2( \mathrm{xy}-\mathrm{wz}) & 2(\mathrm{xz}+\mathrm{wy}) &\mathrm{t}_{\mathrm{x}} \\ 2(\mathrm {xy} +\mathrm{wz}) & \mathrm{w}^{2}-\mathrm{x}^{2}+\mathrm{y}^{2}-\mathrm{z}^{2} & 2( \mathrm{yz}-\mathrm{wx}) &\mathrm{t}_{\mathrm{y}}\2(\mathrm{xz}-\mathrm{wy}) & 2(\mathrm{yz}+ \mathrm{wx}) & \mathrm{w}^{2}-\mathrm{x}^{2}-\mathrm{y}^{2}+\mathrm{z}^{2} &\mathrm{ t}_{\mathrm{z}}\\0&0& 0 & 1 \end{配列}\right]マトリックスのトランスミッション_ _ _ _ _ _ _ _ _ _=翻訳マトリックス_ _ _ _ _ _ _ _ _ _ _ _ _ _⋅回転マトリックス_ _ _ _ _ _ _ _ _ _ _ _=
w2+バツ2−y2−z22 ( xy+ｗｗｗ）2 ( xz−なぜか）02 ( xy−ｗｗｗ）w2−バツ2+y2−z22 ( yz+wx )02 ( xz+なぜか）2 ( yz−wx )w2−バツ2−y2+z20t×tはいtz1
つまり、最初に回転し、次に平行移動します。
座標系
自動運転の認識システムでは、自車両の座標系に加えて、3次元目標検出にはカメラやライダーなどのセンサーも関与し、各センサーは独自の座標系を持ちます。以下に示すように
-
LIDAR 座標系のX 軸
は前方、Y 軸は左、Z 軸は上方向です。重力軸は Z 軸、基準方向は X 軸の正の方向、進行角 rz はターゲットの前方方向と LIDAR 座標系の X 軸の間の角度です。 -
カメラ座標系のZ 軸
は前方、y 軸は下方、x 軸は右方にあります。重力軸は y 軸、基準方向は x 軸の正の方向、方位角 ry はカメラ座標におけるターゲットの前方方向と x 軸の間の角度です。
ヨー角、観測角、目標方位角の関係
以下の図に示すように、青が自車、緑が対象物体、カメラ座標系の Z 軸は前方になります。
-
方位角θは、車両と車両の進行方向からずれた対象物体とを結ぶ線との間の角度として定義される。
-
方位角回転_y は、ターゲットの方向とカメラの X 軸の正の方向 (時計回りが正) の間の角度であり、現実世界におけるターゲットの方向を表します。図に示すように∠ BOC \angle BOC∠BOCに示すとおり。rotaiton_y の値の範囲は[ − π , π ] [-\pi, \pi][ − π ,π ]は、ターゲット位置が変化しても変化しません。
-
観察角アルファは、カメラの視点に対するターゲットの向きを表します。これは、カメラの原点の中心と、カメラの原点とオブジェクトの中心を結ぶ線を半径として定義されます。ターゲットは重力を中心に回転します。ターゲットの進行方向が自車と同じになるまで軸を回転させ、図に示すように必要な角度∠ BOD \angle BOD∠ BO Dを示す。観測角アルファの値の範囲は[ − π , π ] [-\pi, \pi][ − π ,π ]、ターゲット位置の変更に応じて変化します
Rotation_y と Alpha は相互に変換できます。なぜなら∠ AOC = 9 0 ∘ − theta \angle \mathrm{AOC}=90^{\circ}-\text { theta }∠AOC _=9 0○− theta ,
angle∠ AOB = ∠ AOC − ∠ BOC = 9 0 ∘ − theta − rotaion_y \angle \mathrm{AOB}=\angle \mathrm{AOC}-\angle \mathrm{BOC}=90^{\circ} - \text {θ}-\text{回転\_y}∠AOB _=∠AOC _−∠BOC _=9 0○− シータ − rotaion_y
であり、∠ AOB + ∠ BOD = 9 0 ∘ \angle \mathrm{AOB} + \angle \mathrm{BOD} = 90^{\circ}∠AOB _+∠BOD _=9 0∘ , 内部
アルファ = ∠ BOD = 9 0 ∘ − ∠ AOB = theta +回転_y \text { alpha }=\angle \mathrm{BOD}=90^{\circ}-\angle \mathrm{AOB}=\text {シータ }+\text { 回転\_y } アルファ =∠BOD _=9 0○−∠AOB _= シータ + 回転_y は
、回転_y とアルファの両方が反時計回りの負であるとみなします。 したがって、
− アルファ = シータ − 回転_y -\text { alpha }=\text { theta }-\text {回転\_y }− アルファ = シータ − 回転_y
即
アルファ = 回転_y − シータ \text { アルファ }=\text { 回転\_y } - \text { シータ } アルファ = 回転_y − シータ
データセット
ニューシーンズ
点群データ独自フォーマット
NuScenes のデータにはライダー点群とレーダー点群があり、ライダーは 3 次元座標を持つ密な点群、レーダーは疎な点群であり、一般的な FMCW ミリ波レーダーには高度情報がありません (4D レーダーは高度を検出できます)。LIDAR 点群データに関する研究は比較的多く行われているため、ここではあまり紹介しません。
-
レーダー点群データの形式
レーダーの点群データの形式は下図のとおりであり、各線が点となり、その点の座標、速度、強度などの情報が含まれます。点群データは通常、.pcd ファイルを通じて保存され、ヘッダーおよびその他の情報はプレーン テキストで、特定の点群データはバイナリ データです。
-
視覚化
同じpcdファイルを異なる角度から観察すると、通常のミリ波レーダーでは高さ情報を計測できないため、右図のZ軸の値は同じになります。
-
高度データを確認する
値はすべて0です
座標変換
Nuscenes データ セットは、グローバル座標系、ボディ座標系、カメラ座標系、ピクセル座標系、レーダー座標系、および LIDAR 座標系に分割されます。グローバル座標系以外はすべて相対座標系であり、アノテーションの真の値はグローバル座標系に基づきます。
基本原則
アルゴリズムによって適合されるデータのほとんどは、センサー座標系に基づいています。注釈付きの真の値は、グローバル座標系からそれぞれのセンサー座標系に変換する必要があります。すべての変換は、まず、次の方法で身体座標系に変換する必要があります。センサーの外部パラメーターを取得し、センサー座標系に変換します。
以下のように、グローバル座標系のラベルボックスをセンサー座標系に変換します。
# Move box to ego vehicle coord system.
box.translate(-np.array(pose_record['translation']))
box.rotate(Quaternion(pose_record['rotation']).inverse)
# Move box to sensor coord system.
box.translate(-np.array(cs_record['translation']))
box.rotate(Quaternion(cs_record['rotation']).inverse)
2 つの座標系間で変換する方法
変換は 2 つの座標系の座標変換行列 (平行移動行列 + 回転行列) によって行われますが、追加の内部パラメータを通じてカメラをピクセル座標系に変換する必要があります。
def transform_matrix(translation: np.ndarray = np.array([0, 0, 0]),
rotation: Quaternion = Quaternion([1, 0, 0, 0]),
inverse: bool = False) -> np.ndarray:
"""
Convert pose to transformation matrix.
:param translation: <np.float32: 3>. Translation in x, y, z.
:param rotation: Rotation in quaternions (w ri rj rk).
:param inverse: Whether to compute inverse transform matrix.
:return: <np.float32: 4, 4>. Transformation matrix.
"""
tm = np.eye(4)
if inverse:
rot_inv = rotation.rotation_matrix.T
trans = np.transpose(-np.array(translation))
tm[:3, :3] = rot_inv
tm[:3, 3] = rot_inv.dot(trans)
else:
# rotation.rotation_matrix 表示将四元素 rotation 转换为旋转矩阵
# 并将旋转矩阵赋值给对角矩阵 tm 的前三行前三列
tm[:3, :3] = rotation.rotation_matrix
# 将矩阵 tm 的前三行第四列赋值为 translation
tm[:3, 3] = np.transpose(np.array(translation))
# 最后返回一个 4×4 的复合变换矩阵
return tm
上記で返された変換行列は、位置 ( ) と回転情報 ( ) を含む複合変換行列です。四元数、回転行列、平行移動行列、複合変換行列の定義と関係については、convert_nuScenes のコードの詳細な説明を参照してください。 .py.ファイル
translationrotation三、main 函数
マルチセンサーフュージョン時の座標変換
異なるセンサーは異なる収集頻度を持ち、同期的にトリガーされません。タイム アライメントを実現するには、1 つのセンサーのデータをグローバル座標系に投影し、次にグローバル座標系を介して別のセンサーに投影する必要があります。動き補償はグローバル座標系 (絶対座標系) を利用して実行されるため、異なるセンサー間の時間調整が完了します。
- レーダー外部パラメータ: レーダー座標系から自我(身体)座標系への複合変換行列。
- カメラ外部パラメータ: カメラ座標系からエゴ座標系への複合変換行列
- ego_pose: エゴ座標系からグローバル座標系への複合変換行列
最後に、行列の乗算によって点群が現在のフレームの座標系に投影され、タイム アライメントが完了します。
# Fuse four transformation matrices into one and perform transform.
trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])
velocity_trans_matrix = reduce(np.dot, [ref_from_car_rot, car_from_global_rot, global_from_car_rot, car_from_current_rot])
current_pc.transform(trans_matrix)
データの前処理
