[自動運転コード解説]Lift, Splat, Shoot

Posted at 2025-11-02

はじめに

以前下記記事で解説したLift, Splat, Shootの実装が論文だけでは理解しきれなかったため、公式実装を参考に解説を行いたいと思います。
https://qiita.com/Sugar-98/items/65b2df4599507019503b

参照

論文：https://arxiv.org/abs/2008.05711
論文解説：https://qiita.com/Sugar-98/items/65b2df4599507019503b
公式実装：https://github.com/nv-tlabs/lift-splat-shoot

全体構成

モデルの構成は下記のようになっています。

処理フロー：

入力画像から特徴量を抽出、深度ビンごとの確率を算出して特徴量に掛ける
座標変換テンソルの作成
1で得た画像座標系上の3次元特徴マップを、2で作成したテンソルを用いてBEV座標系に投影。Z軸方向の特徴量を全て結合する(ボクセルプーリング)。
特徴量をBEV画像にエンコード

また、モデルの入力は下記要素から構成されています。
入力

変数名	意味	次元
x	入力画像	B,N,C,imH,imW
rots	車両座標系におけるカメラの回転	B,N,3,3
trans	車両座標系におけるカメラの位置	B,N,3
intrins	カメラの内部パラメータ行列	B,N,3,3
post_rots	augmentationした回転量	B,N,3,3
post_trans	augmentationしたオフセット量	B,N,3

B:バッチサイズ
N:カメラ数
C:チャネル
imH,imW:入力画像サイズ

入力画像の特徴量抽出と深度推定

特徴量抽出

特徴量抽出では、バックボーンとして学習済みのefficientnet-b0を用います。efficientnet-b0において最後に解像度が下がった層とその前の層(図reduction_4,reduction_5)の出力を取り出し、reduction_5の方はUpsampleしてreduction_4の解像度と合わせ、チャネルを結合します。その後畳み込み層を2回通して特徴量とします。

コード

"""
Copyright (C) 2020 NVIDIA Corporation.  All rights reserved.
Licensed under the NVIDIA Source Code License. See LICENSE at https://github.com/nv-tlabs/lift-splat-shoot.
Authors: Jonah Philion and Sanja Fidler
"""
    def get_eff_depth(self, x):
        # adapted from https://github.com/lukemelas/EfficientNet-PyTorch/blob/master/efficientnet_pytorch/model.py#L231
        endpoints = dict()

        # Stem
        x = self.trunk._swish(self.trunk._bn0(self.trunk._conv_stem(x)))#メモ：EfficientNet の最初の層 (conv_stem + batchnorm + swish)を適用
        prev_x = x

        # Blocks
        for idx, block in enumerate(self.trunk._blocks):#メモ：EfficientNetのブロックを順番に適用
            drop_connect_rate = self.trunk._global_params.drop_connect_rate
            if drop_connect_rate:#メモ：層のドロップ率が、層が深くなるほど高くなるようにする。Stochastic Depth
                drop_connect_rate *= float(idx) / len(self.trunk._blocks) # scale drop connect_rate
            x = block(x, drop_connect_rate=drop_connect_rate)
            if prev_x.size(2) > x.size(2):#メモ：解像度が下がったら保存する
                endpoints['reduction_{}'.format(len(endpoints)+1)] = prev_x
            prev_x = x

        # Head
        endpoints['reduction_{}'.format(len(endpoints)+1)] = x #メモ：最後の出力も記録する。
        x = self.up1(endpoints['reduction_5'], endpoints['reduction_4'])# メモ：reduction_5の解像度を上げて、reduction_4と結合する。
        return x #B*N,512(upの出力チャネル),reduction_4のH,reduction_4のW

深度推定

深度推定は畳み込み層1つで行います。出力は各ピクセルの特徴量と深度ビンの確率分布であり、図に示す通り特徴量に確率を掛け、特徴量の濃淡で深度確率を表現できるようにします。

    def get_depth_feat(self, x):
        x = self.get_eff_depth(x) #B*N,512(upの出力チャネル),reduction_4のH,reduction_4のW
        # Depth
        x = self.depthnet(x) #B*N,self.D + self.C,reduction_4のH,reduction_4のW

        depth = self.get_depth_dist(x[:, :self.D])#深度ビン毎の確率値にする
        new_x = depth.unsqueeze(1) * x[:, self.D:(self.D + self.C)].unsqueeze(2)#深度の確率を特徴量にかける

        return depth, new_x

座標変換テンソルの作成

画像座標系の特徴量と深度確率分布を、BEV座標系に変換するテンソルを作成します。このテンソルはサイズD,H,W,3で、各要素がBEV座標系上の位置x,y,zを格納しています。つまり、

 G(d,h,w)=[x, y, z]^T

D\text{:深度ビンの数}

d\text{:深度ビンインデックス}

h\text{:縦方向のピクセルインデックス}

w\text{:横方向のピクセルインデックス}

変換テンソルは下記の手順で作成します。

1. Frustumの作成

ピクセル/深度ビンインデックスと、画像上の位置/深度をマッピングするFrustumを作成します。定義は下記です。

F_{d,h,w} = 
\begin{bmatrix}
u_w \\
v_h \\
d_s
\end{bmatrix}

u_w\text{:$w$におけるカメラ座標系横位置}

v_h\text{:$h$におけるカメラ座標系縦位置}

d_s\text{:$d$における深度}

Frustumとは角錐の事で、画像座標系が下図のように角錐型に広がることからこのように呼ばれています。

実装は下記の通りです。

    def create_frustum(self):
        # make grid in image plane
        ogfH, ogfW = self.data_aug_conf['final_dim']
        fH, fW = ogfH // self.downsample, ogfW // self.downsample
        ds = torch.arange(*self.grid_conf['dbound'], dtype=torch.float).view(-1, 1, 1).expand(-1, fH, fW)#メモ：ds=[4,5,6,7,...,45] 深度ビン
        D, _, _ = ds.shape
        xs = torch.linspace(0, ogfW - 1, fW, dtype=torch.float).view(1, 1, fW).expand(D, fH, fW)#メモ：画像座標系横位置
        ys = torch.linspace(0, ogfH - 1, fH, dtype=torch.float).view(1, fH, 1).expand(D, fH, fW)#メモ：画像座標系縦位置

        # D x H x W x 3
        frustum = torch.stack((xs, ys, ds), -1)
        return nn.Parameter(frustum, requires_grad=False)#frustum型の立体の各点に(u,v,ds)のベクトルを持つベクトル場

2. Augumentationの逆変換

入力画像にAugumentationを行うと位置がずれてしまうため、各ピクセルの位置座標にAugumentationの逆変換(回転とオフセット)を行います。

P_{n,d,h,w} = r_n^{-1}(F_{d,h,w} - t_n)

Augumentationの回転行列。2次平面上回転行列を3次に拡張して使用。

r_n = 
\begin{bmatrix}
r_n'\in\mathbb{R}^{2\times2} &  0\\
0 & 1
\end{bmatrix}

オフセット量

t_n = 
\begin{bmatrix}
u_{0,n} \\
v_{0,n} \\
0
\end{bmatrix}

n\text{:カメラインデックス}

u_{0,n}\text{:カメラnの画像に対する横オフセット量}

v_{0,n}\text{:カメラnの画像に対する縦オフセット量}

3. 画像座標系からカメラ座標系への変換

画像座標系はあくまで画像上の位置座標と深度を表すもののため、物理的な位置を示す直交座標系に変換する必要があります。本実装では、まずカメラを原点とするカメラ座標系に変換し、その後カメラの位置や角度の情報を用いてBEV座標系に変換します。カメラ座標系の位置$[x,y,z]^T$と画像座標系上の位置$[u,v,d]^T$の関係は

d
\begin{bmatrix}
u \\
v \\
1
\end{bmatrix}
=I_n
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}

I_n = 
\begin{bmatrix}
f_{x,n} & 0 & c_{x,n}\\
0 & f_{y,n} & c_{y,n}\\
0 & 0 & 1
\end{bmatrix}

と表されます。ここで、$I_n$は内部パラメータと呼ばれ、カメラ座標系を画像座標系に変換する行列です。$f_{x,n},f_{y,n}$は焦点距離、$c_{x,n},c_{y,n}$は主点を表します。
参考：https://mem-archive.com/2018/02/21/post-157/

従って、画像座標系 → カメラ座標系への変換は下記のように表せます。

P'_{𝑛,𝑑,ℎ,𝑤} = I_n^{-1}
\begin{bmatrix}
d_s & 0 & 0 \\
0 & d_s & 0 \\
0 & 0 & 1\\
\end{bmatrix}
P_{𝑛,𝑑,ℎ,𝑤}

4. カメラ座標系からBEV座標系への変換

最後にカメラの回転行列$R_n\in\mathbb{R}^{3\times3}$、BEV座標上の位置$Tn=[X_{0,n},Y_{0,n},Z_{0,n}]^T$を用いて、BEV座標系に変換します。単純に回転とオフセットを行うのみです。

G_{𝑛,𝑑,ℎ,𝑤} = T_n + R_nP'_{𝑛,𝑑,ℎ,𝑤}

ちなみに、公式実装ではnuScenesデータセットを用いているので、BEV座標系は後軸の中心を原点とする右手系、カメラ座標系は正面をz、カメラ光学中心を原点とする右手系になります。

2,3,4の実装は下記の通りになります。

コード

    def get_geometry(self, rots, trans, intrins, post_rots, post_trans):
        """Determine the (x,y,z) locations (in the ego frame)
        of the points in the point cloud.
        Returns B x N x D x H/downsample x W/downsample x 3
        """
        B, N, _ = trans.shape #メモ：バッチ，カメラ数, (x,y,z)

        # undo post-transformation
        # B x N x D x H x W x 3
        points = self.frustum - post_trans.view(B, N, 1, 1, 1, 3)#メモ：frustumの全u,v,dsから、post_trans分オフセットする D,H,W,3 - B,N,1,1,1,3 = B,N,D,H,W,3
        points = torch.inverse(post_rots).view(B, N, 1, 1, 1, 3, 3).matmul(points.unsqueeze(-1))
        #メモ：frustumの全u,v,dsをpost_rots分回転させる B,N,1,1,1,3,3*B,N,D,H,W,3,1 = B,N,D,H,W,3,1

        # cam_to_ego
        points = torch.cat((points[:, :, :, :, :, :2] * points[:, :, :, :, :, 2:3],
                            points[:, :, :, :, :, 2:3]
                            ), 5)
        #メモ：車両座標系のX,Yを導出する準備。(B,N,D,H,W,3(u,v,d))→(B,N,D,H,W,3(u*d,v*d,d))。x=d*u/f, y=d*v/fのため。
        combine = rots.matmul(torch.inverse(intrins))#メモ：カメラ→egoなので逆行列にする。B,N,3,3*B,N,3,3 = B,N,3,3
        points = combine.view(B, N, 1, 1, 1, 3, 3).matmul(points).squeeze(-1)#メモ：B,N,1,1,1,3,3*B,N,D,H,W,3,1=B,N,D,H,W,3,1 → B,N,D,H,W,3
        points += trans.view(B, N, 1, 1, 1, 3)#メモ：B,N,D,H,W,3 + B,N,1,1,1,3 = B,N,D,H,W,3

        return points #メモ：B,N,D,imH/downsample,imW/downsample,3 N:カメラ数，D:深度ビン数 , 3:XYZ

ボクセルプーリング

座標変換テンソルを用いて特徴量をBEV座標系に変換し、同じ座標については累積和にしてボクセルに格納していきます。

1. 変換テンソルの座標を位置からBEVボクセルインデックスに更新

各点の座標$(x,y,z)$を、BEV格子のインデックス$(i_x,i_y,i_z)$に写像します。

i_x = \left\lfloor \frac{x - \bigl(b_x - \Delta x/2\bigr)}{\Delta x} \right\rfloor,
i_y = \left\lfloor \frac{y - \bigl(b_y - \Delta y/2\bigr)}{\Delta y} \right\rfloor,
i_z = \left\lfloor \frac{z - \bigl(b_z - \Delta z/2\bigr)}{\Delta z} \right\rfloor

b_x,b_y,b_z\text{:BEVグリッドの下限}

\Delta x,\Delta y,\Delta z,\text{:BEV格子サイズ}

右手系なので、BEV格子の右下が原点になり、インデックスをカウントしていくイメージですね。$\Delta x/2を引いているのは座標をボクセルの端ではなく中心に合わせるためです。$また、設定領域外の座標については除外します。

        # flatten indices
        geom_feats = ((geom_feats - (self.bx - self.dx/2.)) / self.dx).long()
        #メモ：(ego座標上の点 - (右下のボクセル中心 - ボクセルサイズ/2))/ボクセルサイズ=ボクセル範囲左下を(0,0,0)とする点/ボクセルサイズ=ボクセルインデックス(i,j,k)
        geom_feats = geom_feats.view(Nprime, 3)#メモ：ピクセル*深度ビンの全サンプル点をフラットにする。
        batch_ix = torch.cat([torch.full([Nprime//B, 1], ix,
                             device=x.device, dtype=torch.long) for ix in range(B)])
        geom_feats = torch.cat((geom_feats, batch_ix), 1)#メモ：各点にバッチ番号を付与。(x_idx, y_idx, z_idx, batch_idx)

        # filter out points that are outside box
        kept = (geom_feats[:, 0] >= 0) & (geom_feats[:, 0] < self.nx[0])\
            & (geom_feats[:, 1] >= 0) & (geom_feats[:, 1] < self.nx[1])\
            & (geom_feats[:, 2] >= 0) & (geom_feats[:, 2] < self.nx[2])#メモ：bound外のインデックスを除外
        x = x[kept]
        geom_feats = geom_feats[kept]

2. ピクセル順のデータをBEVボクセル順にソート

下記のように、BEV座標系の軸毎に異なる係数をかけて、各バッチのすべてのボクセルにユニークなインデックスを付与します。この時、同一バッチ内で同じボクセルに落ちた複数サンプル(e.g.複数カメラで同じ場所を写している)がある場合は同じインデックスが付与され、ソートによって隣同士になります。

index	$i_x$	$i_y$	$i_z$	batch
0	0	0	0	0
1	0	0	0	1
2	0	0	0	2
...	...	...	...	...
B	0	0	1	0
B+1	0	0	1	1
...	...	...	...	...
B*$n_z$	0	1	0	0
...	...	...	...	...
B$n_y$$n_z$	1	0	0	0
...	...	...	...	...
B$n_y$$n_z$$i_x$ +B$n_z$$i_y$+B$i_z$+b	$i_x$	$i_y$	$i_z$	$b$

        # get tensors from the same voxel next to each other
        ranks = geom_feats[:, 0] * (self.nx[1] * self.nx[2] * B)\
            + geom_feats[:, 1] * (self.nx[2] * B)\
            + geom_feats[:, 2] * B\
            + geom_feats[:, 3]#メモ：同じバッチ内でボクセルごとに番号を割り振る。同じボクセルだと同じ番号になる。
        sorts = ranks.argsort()
        x, geom_feats, ranks = x[sorts], geom_feats[sorts], ranks[sorts]#メモ：ボクセル順にデータを並べ替え

3. 同じボクセルの特徴量を累積和にする

前ステップで同じインデックスが付与されたデータは、同じボクセルのデータなので、累積和を計算して一つの特徴量にします。この時、cumsum trickという手法を用いて累積和を高速に計算することができます。計算の流れは下記の通りです。

cumsum()を使ってすべての特徴量を積み上げる
連続したボクセルインデックスの最後の要素だけ残す
前のインデックスの特徴量を引き、余分に累積したものを元に戻す

def cumsum_trick(x, geom_feats, ranks):
    x = x.cumsum(0)#メモ：xの値を全て積み上げる
    kept = torch.ones(x.shape[0], device=x.device, dtype=torch.bool)
    kept[:-1] = (ranks[1:] != ranks[:-1])#メモ：ボクセルインデックスの最後の要素だけtrueになる

    x, geom_feats = x[kept], geom_feats[kept]#メモ：ボクセルインデックスの最後の要素だけ取得
    x = torch.cat((x[:1], x[1:] - x[:-1]))#メモ：異なるボクセル同士で累積和を取った部分を元に戻す

    return x, geom_feats

4. ボクセルに特徴量を格納

ボクセルに特徴量を格納します。xに特徴量、geom_featsにインデックス$i_x,i_y,i_z$が入っており、これらはranksに基づいて同じようにソートされているので、単純に格納するのみです。

        # griddify (B x C x Z x X x Y)
        final = torch.zeros((B, C, self.nx[2], self.nx[0], self.nx[1]), device=x.device) #メモ：空のボクセルグリッド B*C*Z方向ボクセル数*X方向...*Y方向...
        final[geom_feats[:, 3], :, geom_feats[:, 2], geom_feats[:, 0], geom_feats[:, 1]] = x
        #メモ：geom_featsで定めたインデックスに特徴量を格納する。
        #メモ：final[batch_idx, :, z_idx, x_idx, y_idx] = x

5. Z軸方向の特徴量を結合

Z軸方向の特徴量を全て連結します。

        # collapse Z
        final = torch.cat(final.unbind(dim=2), 1)#メモ：Z軸方向を特徴量に潰す(B,C,Z,X,Y) → (B,C×Z,X,Y)

BEV画像出力

入力の特徴量抽出と似ていますが、こちらは未学習のresnet18を用い、同じように中間層出力をUpsampleして結合します。出力チャネルは予測するBEVクラスの数に合わせます。

添付コードのライセンス

本記事に掲載したコードは下記ライセンスに基づいています。

LICENSE

NVIDIA Source Code License for Lift, Splat, Shoot

1. Definitions

“Licensor” means any person or entity that distributes its Work.
“Software” means the original work of authorship made available under this License.
“Work” means the Software and any additions to or derivative works of the Software that are made available under this License.
The terms “reproduce,” “reproduction,” “derivative works,” and “distribution” have the meaning as provided under U.S. copyright law; provided, however, that for the purposes of this License, derivative works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work.
Works, including the Software, are “made available” under this License by including in or with the Work either (a) a copyright notice referencing the applicability of this License to the Work, or (b) a copy of this License.

2. License Grant

2.1 Copyright Grant. Subject to the terms and conditions of this License, each Licensor grants to you a perpetual, worldwide, non-exclusive, royalty-free, copyright license to reproduce, prepare derivative works of, publicly display, publicly perform, sublicense and distribute its Work and any resulting derivative works in any form.

3. Limitations

3.1 Redistribution. You may reproduce or distribute the Work only if (a) you do so under this License, (b) you include a complete copy of this License with your distribution, and (c) you retain without modification any copyright, patent, trademark, or attribution notices that are present in the Work.

3.2 Derivative Works. You may specify that additional or different terms apply to the use, reproduction, and distribution of your derivative works of the Work (“Your Terms”) only if (a) Your Terms provide that the use limitation in Section 3.3 applies to your derivative works, and (b) you identify the specific derivative works that are subject to Your Terms. Notwithstanding Your Terms, this License (including the redistribution requirements in Section 3.1) will continue to apply to the Work itself.

3.3 Use Limitation. The Work and any derivative works thereof only may be used or intended for use non-commercially. Notwithstanding the foregoing, NVIDIA and its affiliates may use the Work and any derivative works commercially. As used herein, “non-commercially” means for research or evaluation purposes only.

3.4 Patent Claims. If you bring or threaten to bring a patent claim against any Licensor (including any claim, cross-claim or counterclaim in a lawsuit) to enforce any patents that you allege are infringed by any Work, then your rights under this License from such Licensor (including the grant in Section 2.1) will terminate immediately.

3.5 Trademarks. This License does not grant any rights to use any Licensor’s or its affiliates’ names, logos, or trademarks, except as necessary to reproduce the notices described in this License.

3.6 Termination. If you violate any term of this License, then your rights under this License (including the grant in Section 2.1) will terminate immediately.

4. Disclaimer of Warranty.

THE WORK IS PROVIDED “AS IS” WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WARRANTIES OR CONDITIONS OF M ERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE OR NON-INFRINGEMENT. YOU BEAR THE RISK OF UNDERTAKING ANY ACTIVITIES UNDER THIS LICENSE. 

5. Limitation of Liability.

EXCEPT AS PROHIBITED BY APPLICABLE LAW, IN NO EVENT AND UNDER NO LEGAL THEORY, WHETHER IN TORT (INCLUDING NEGLIGENCE), CONTRACT, OR OTHERWISE SHALL ANY LICENSOR BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF OR RELATED TO THIS LICENSE, THE USE OR INABILITY TO USE THE WORK (INCLUDING BUT NOT LIMITED TO LOSS OF GOODWILL, BUSINESS INTERRUPTION, LOST PROFITS OR DATA, COMPUTER FAILURE OR MALFUNCTION, OR ANY OTHER COMM ERCIAL DAMAGES OR LOSSES), EVEN IF THE LICENSOR HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.

おわりに

細かく解説した結果長い記事になってしまいましたが、LSSの実装を追っていくと、BEV予測モデルの仕組みについて理解を深めることができると思います。
現在CARLAで生成したデータセットでLSSを学習させる事に取り組んでおり、近日中に記事化できると思うので、是非読んでみて頂けると幸いです。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up

[自動運転 コード解説]Lift, Splat, Shoot