【自动驾驶】经典LSS算法解析—

LSS-Lift.Splat,Shoot

论文题目：Lift, Splat, Shoot: Encoding Images From Arbitrary Camera Rigs by Implicitly Unprojecting to 3D
代码：https://github.com/nv-tlabs/lift-splat-shoot
概括：先做深度估计和特征融合，然后投影到 BEV 视图中，在 BEV 视图中做特征融合，在融合后的特征图上做检测、规划等任务。

在这里插入图片描述

一、概括说明

1、Lift:首先提取图像特征，估计每个像素的深度分布估计（每个像素对应每个深度的特征向量）

将2D图像( $\times H \times 3$ )增加深度信息，升维得到3D( $\times H \times D$ )，为学习不同深度的维特征，得到 $\times H \times D \times C$ 维的视锥点云。

步骤：

通过向量 $c\in R^{C}$ 和深度概率 $\alpha$ 的外积，得到每个像素对应每个深度的特征向量 $\alpha_{i}c$
通过像素估计到的每个深度特征为： $(u,v,d_{i},\alpha_{i}c)$
结合相机成像原理，已知内外参的情况下，可求出像素对应的3D坐标
$d_{i}(u;v;1)= K[R t](x;y;z;1)=T(x;y;z;1)$
$x;y;z;1) = d_{i}T^{-1}(u;v;1)$
每个像素对应的3D特征向量即完成Lift过程， $\phi(x,y,z) = \alpha_{i}c$

总结：lift过程是通过估计深度来实现2D特征转化为3D特征；此处的深度估计有几种不同的选择，优缺点如下：

特征向量形式	按预测概率分布	均匀分布	One-hot
优势	可识别深度，特征转换更精准	计算快，无学习参数	深度估计的识别度更高，实际为估计每个像素对应的点云
劣势	需要MLP做概率估计	深度估计无差异化	鲁棒性不好

2、Splat：将lift的二点特征压缩为BEV特征

将图像特征投影到BEV空间中，采用cumsum Trick 的方法将特征求和，得到 $\times X \times 维度$ 的BEV特征。

实际为均匀采样点云；每个点的特征为 $\phi(x, y,z) = \alpha_{i}c$
难点分析：1、多相机存在场合趋于，特征压缩时不应有差异；2、点数量多，百万级别的点（±50m范围，采样间隔为0.5m）
解决：

参考PointPillars将BEV空间划分为 $\times W$ 的网格；
对每个估计的3D点（x,y,z）,将其特征归属至最近的pillar
利用归属到某个特定pillar的特征进行求和sum pooling

求和是因为考虑到多个视锥点云落在同一个BEV格栅Grid的情况，会出现以下两种情况：

不同高度的视锥点云会落在同一个栅格中，比如电线杆上的不同像素点
不同相机间存在重叠overlap，不同相机观测到的同一个物体，会落在同一个BEV Grid中

3、Shoot:基于BEV特征，进行路径规划。利用深度分布信息，生成点云，并利用点云信息进行3D重建

将轨迹通过模板映射到BEV空间中，并计算轨迹的损失。

图像 backbone 采用 EfficientNet，通过预训练得到深度估计，需要标记检测出的物体在BEV视角下的投影

监督真值是实例分割结果、可行驶区域，Loss 定义为预测结果与 groud truth 的交叉熵

在LSS源码中，其感知范围，BEV单元格大小，BEV下的网格尺寸如下：

输入图像大小为 $128 \times 352$

感知范围
$x$ 轴方向的感知范围 -50m ~ 50m； $y$ 轴方向的感知范围 -50m ~ 50m； $z$ 轴方向的感知范围 -10m ~ 10m；
BEV单元格大小
$x$ 轴方向的单位长度 0.5m； $y$ 轴方向的单位长度 0.5m； $z$ 轴方向的单位长度 20m；
BEV的网格尺寸
$200 \times 200 \times 1$ ；
深度估计范围
由于LSS需要显式估计像素的离散深度，论文给出的范围是4m ~ 45m，间隔为1m，也就是算法会估计41个离散深度；
模型使用参数
imgs：输入的环视相机图片，imgs = $(b s, N, 3, H, W)$ ， $N$ 代表环视相机个数，nuSence为 $N = 6$ ；
rots：由相机坐标系->车身坐标系的旋转矩阵， $ro t s = (b s, N, 3, 3)$ ；
trans：由相机坐标系->车身坐标系的平移矩阵， $t r an s = (b s, N, 3)$ ；
intrinsic：相机内参， $in t r in s i c = (b s, N, 3, 3)$ ；
post_rots：由图像增强引起的旋转矩阵， $post_rots = (bs, N, 3, 3)$ ；
post_trans：由图像增强引起的平移矩阵， $post_trans = (bs, N, 3)$ ；
binimgs：由于LSS做的是语义分割任务，所以会将真值目标投影到BEV坐标系，将预测结果与真值计算损失；具体而言，在binimgs中对应物体的bbox内的位置为1，其他位置为0；

二、算法步骤

LSS算法的五个步骤如下：

1、生成视锥，并根据相机内外参将视锥中的点投影到ego坐标系；
2、对环视图像进行特征提取，并构建图像特征点云；
3、利用变换后的ego坐标系的点与图像特征点云利用voxel pooling构建BEV特征；
4、对生成的BEV特征利用BEV Encoder做进一步的特征融合；
5、利用特征融合后的BEV特征完成语义分割

模型整体初始化函数，主要分为以下三个模块：

CamEncode：图像特征提取
BevEncode：BEV特征检测
frustum：视锥，用于图像点云坐标和BEV栅格间的坐标转换

class LiftSplatShoot(nn.Module):
    def __init__(self, grid_conf, data_aug_conf, outC):
        super(LiftSplatShoot, self).__init__()
        self.grid_conf = grid_conf
        self.data_aug_conf = data_aug_conf

        dx, bx, nx = gen_dx_bx(self.grid_conf['xbound'],  
                                              self.grid_conf['ybound'],
                                              self.grid_conf['zbound']) 
        # ['xbound']:最小值、最大值以及步长
        # dx:X轴、Y轴和Z轴方向上的每个体素的大小 bx:整个网格中最开始的那个体素的中心位置
        # nx:每个轴上体素的数量
        self.dx = nn.Parameter(dx, requires_grad=False)
        self.bx = nn.Parameter(bx, requires_grad=False)
        self.nx = nn.Parameter(nx, requires_grad=False)

        self.downsample = 16
        self.camC = 64
        self.frustum = self.create_frustum()   # 1
        self.D, _, _, _ = self.frustum.shape
        self.camencode = CamEncode(self.D, self.camC, self.downsample)  # 2
        self.bevencode = BevEncode(inC=self.camC, outC=outC)   # 3

        # toggle using QuickCumsum vs. autograd
        self.use_quickcumsum = True

        def get_voxels(self, x, rots, trans, intrins, post_rots, post_trans):
            # x: (B, N); rots, trans：相机外参，以旋转矩阵和平移矩阵形式表示
            # intrins：相机内参; post_rots，post_trans：图像增强时使用的旋转矩阵和平移矩阵，用于在模型训练时撤销图像增强引入的位姿变化
            geom = self.get_geometry(rots, trans, intrins, post_rots, post_trans)
            x = self.get_cam_feats(x)

            x = self.voxel_pooling(geom, x)

            return x

        def forward(self, x, rots, trans, intrins, post_rots, post_trans):
            x = self.get_voxels(x, rots, trans, intrins, post_rots, post_trans)
            x = self.bevencode(x)
            return x

1、生成视锥，并根据相机内外参将视锥中的点投影到ego坐标系；

生成视锥的代码如下：


def create_frustum():
    # 原始图片大小  ogfH:128  ogfW:352
    ogfH, ogfW = self.data_aug_conf['final_dim']

    # 下采样16倍(self.downsample)后图像大小  fH: 8  fW: 22
    fH, fW = ogfH // self.downsample, ogfW // self.downsample 

    # self.grid_conf['dbound'] = [4, 45, 1]
    # 在深度方向上划分网格,即每个点的深度 ds: DxfHxfW (41x8x22)
    ds = torch.arange(*self.grid_conf['dbound'], dtype=torch.float).view(-1, 1, 1).expand(-1, fH, fW)

    """
    1. torch.linspace(0, ogfW - 1, fW, dtype=torch.float)
    tensor([0.0000, 16.7143, 33.4286, 50.1429, 66.8571, 83.5714, 100.2857,
            117.0000, 133.7143, 150.4286, 167.1429, 183.8571, 200.5714, 217.2857,
            234.0000, 250.7143, 267.4286, 284.1429, 300.8571, 317.5714, 334.2857,
            351.0000])
            
    2. torch.linspace(0, ogfH - 1, fH, dtype=torch.float)
    tensor([0.0000, 18.1429, 36.2857, 54.4286, 72.5714, 90.7143, 108.8571,
            127.0000])
    """

    # 在0到351上划分22个格子 xs: DxfHxfW(41x8x22)
    xs = torch.linspace(0, ogfW - 1, fW, dtype=torch.float).view(1, 1, fW).expand(D, fH, fW)  
    
    # 在0到127上划分8个格子 ys: DxfHxfW(41x8x22)
    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[k,i,j,0]就是(i,j)位置，深度为k的像素的宽度方向上的栅格坐标 frustum: DxfHxfWx3
    frustum = torch.stack((xs, ys, ds), -1)  
    return nn.Parameter(frustum, requires_grad=False)

锥点由图像坐标系向自车坐标系进行坐标转化这一过程主要涉及到相机的内外参数，对应代码中的函数为get_geometry()。

def get_geometry(self, rots, trans, intrins, post_rots, post_trans):
    B, N, _ = trans.shape  # B: batch size N：环视相机个数

    # undo post-transformation
    # B x N x D x H x W x 3
    # 1.抵消数据增强及预处理对像素的变化  RX+T=Y X=R^{-1}(Y-T)
    points = self.frustum - post_trans.view(B, N, 1, 1, 1, 3)
    points = torch.inverse(post_rots).view(B, N, 1, 1, 1, 3, 3).matmul(points.unsqueeze(-1))    

    # 图像坐标系 -> 归一化相机坐标系 -> 相机坐标系 -> 车身坐标系 cam_to_ego
    # 但是自认为由于转换过程是线性的，所以反归一化是在图像坐标系完成的，然后再利用求完逆的内参投影回相机坐标系
    # 转换到真实坐标系再乘以内存去畸变，需要注意的是，上一步得到的 xy 是单位深度下的相机坐标，不同深度对应的 xy 是一样的，因此需要乘以深度d才能得到真实世界的坐标
    points = torch.cat((points[:, :, :, :, :, :2] * points[:, :, :, :, :, 2:3],
                        points[:, :, :, :, :, 2:3]
                        ), 5)  # 反归一化

    # 通过外参转换到 BEV 坐标系下 (R(intrins)^-1)x + t = y
    combine = rots.matmul(torch.inverse(intrins))
    points = combine.view(B, N, 1, 1, 1, 3, 3).matmul(points).squeeze(-1)
    points += trans.view(B, N, 1, 1, 1, 3)

    # (bs, N, depth, H, W, 3)：其物理含义
    # 每个batch中的每个环视相机图像特征点，其在不同深度下位置对应在ego坐标系下的坐标
    return points

2、get_cam_feats:对环视图像进行特征提取camera_features，并构建图像特征点云

a）利用Efficientnet-B0主干网络对环视图像进行特征提取

输入的环视图像 $(b s, N, 3, H, W)$ ，在进行特征提取之前，会将前两个维度进行合并，一起提取特征，对应维度变换为 $\rightarrow (bs * N, 3, H, W)$ ；其输出的多尺度特征尺寸大小如下：

level0 = (bs * N, 16, H / 2, W / 2)
level1 = (bs * N, 24, H / 4, W / 4)
level2 = (bs * N, 40, H / 8, W / 8)
level3 = (bs * N, 112, H / 16, W / 16)
level4 =  (bs * N, 320, H / 32, W / 32)

b）对其中的后两层特征进行融合，丰富特征的语义信息，融合后的特征尺寸大小为 $(b s * N, 512, H /16, W /16)$

Step1: 对最后一层特征升采样到倒数第二层大小；
level4 -> Up -> level4' = (bs * N, 320, H / 16, W / 16)

Step2：对主干网络输出的后两层特征进行concat；
cat(level4', level3) -> output = (bs * N, 432, H / 16, W / 16)

Step3：对concat后的特征，利用ConvLayer卷积层做进一步特征拟合；
       
ConvLayer(output) -> output' = (bs * N, 512, H / 16, W / 16)

其中ConvLayer层构造如下：
"""Sequential(
  (0): Conv2d(432, 512, kernel_size=(3, 3), stride=(1, 1), padding=(1, 1), bias=False)
  (1): BatchNorm2d(512, eps=1e-05, momentum=0.1, affine=True, track_running_stats=True)
  (2): ReLU(inplace=True)
  (3): Conv2d(512, 512, kernel_size=(3, 3), stride=(1, 1), padding=(1, 1), bias=False)
  (4): BatchNorm2d(512, eps=1e-05, momentum=0.1, affine=True, track_running_stats=True)
  (5): ReLU(inplace=True)
)"""

c）估计深度方向的概率分布(用41维数据表示)，并输出特征图每个位置的语义特征 (用64维的特征表示)，整个过程用 $\times 1$ 卷积层实现

整体pipeline
output' -> Conv1x1 -> x = (bs * N, 41+64=105, H / 16, W / 16)

a)步骤输出的特征：
output = Tensor[(bs * N, 512, H / 16, W / 16)]

b)步骤使用的1x1卷积层：
Conv1x1 = Conv2d(512, 105, kernel_size=(1, 1), stride=(1, 1))

c)步骤输出的特征以及对应的物理含义：
x = Tensor[(bs * N, 105, H / 16, W / 16)] 
第二维的105个通道分成两部分；第一部分：前41个维度代表不同深度上41个离散深度；
                         第二部分：后64个维度代表特征图上的不同位置对应的语义特征；

d）对c)步骤估计出来的离散深度利用softmax()函数计算深度方向的概率密度
e）利用得到的深度方向的概率密度和语义特征通过外积运算构建图像特征点云

# d)步骤得到的深度方向的概率密度
depth = x[:, :self.D] = (bs * N, 41, H / 16, W / 16) -> unsqueeze -> (bs * N, 1, 41, H / 16, W / 16)

# c)步骤得到的特征，选择后64维是预测出来的语义特征
x[:, self.D:(self.D + self.C)] = (bs * N, 64, H / 16, W / 16) -> unsqueeze(2) -> (bs * N, 64, 1, H / 16, W / 16)

# 概率密度和语义特征做外积，构建图像特征点云
# (bs * N, 1, 41, H / 16, W / 16) * (bs * N, 64, 1, H / 16, W / 16) -> (bs * N, 64, 41, H / 16, W / 16)
new_x = depth.unsqueeze(1) * x[:, self.D:(self.D + self.C)].unsqueeze(2)  
#  new_x = x[:, self.D:(self.D + self.C)].unsqueeze(2) * depth.unsqueeze(1)

3、利用ego坐标系下的坐标点与图像特征点云，利用Voxel Pooling构建BEV特征

def voxel_pooling(self, geom_feats, x):
    # geom_feats:(B x N x D x H x W x 3)：在ego坐标系下的坐标点；
    # x:(B x N x D x fH x fW x C)：图像点云特征

    B, N, D, H, W, C = x.shaped
    Nprime = B * N * D * H * W 

    # 将特征点云展平，一共有 B*N*D*H*W 个点 -> (B*N*D*H*W, C)
    x = x.reshape(Nprime, C) 

    # flatten indices
    geom_feats = ((geom_feats - (self.bx - self.dx/2.)) / self.dx).long() # ego下的空间坐标转换到体素坐标（计算栅格坐标并取整）
    geom_feats = geom_feats.view(Nprime, 3)  # 将体素坐标同样展平，geom_feats: (B*N*D*H*W, 3)
    batch_ix = torch.cat([torch.full([Nprime//B, 1], ix,
                          device=x.device, dtype=torch.long) for ix in range(B)]) # 每个点对应于哪个batch
    geom_feats = torch.cat((geom_feats, batch_ix), 1)  # geom_feats: (B*N*D*H*W, 4)

    # filter out points that are outside box
    # 过滤掉在边界线之外的特征点 x:0~199  y: 0~199  z: 0
    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])
    x = x[kept]
    geom_feats = geom_feats[kept]

    # 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]  # 给每一个点一个rank值，rank相等的点在同一个batch，并且在在同一个格子里面
    sorts = ranks.argsort()
    x, geom_feats, ranks = x[sorts], geom_feats[sorts], ranks[sorts]  # 按照rank排序，这样rank相近的点就在一起了
   
    # cumsum trick x: 64 x 1 x 200 x 200
    if not self.use_quickcumsum:
        x, geom_feats = cumsum_trick(x, geom_feats, ranks)
    else:
        x, geom_feats = QuickCumsum.apply(x, geom_feats, 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)  # final: bs x 64 x 1 x 200 x 200
    final[geom_feats[:, 3], :, geom_feats[:, 2], geom_feats[:, 0], geom_feats[:, 1]] = x  # 将x按照栅格坐标放到final中

    # collapse Z
    final = torch.cat(final.unbind(dim=2), 1)  # 消除掉z维

    return final  # final: bs x 64 x 200 x 200

采用cumsum_trick完成Voxel Pooling运算，代码如下：

class QuickCumsum(torch.autograd.Function):
    @staticmethod
    def forward(ctx, x, geom_feats, ranks):
        x = x.cumsum(0) # 求前缀和
        kept = torch.ones(x.shape[0], device=x.device, dtype=torch.bool)  
        kept[:-1] = (ranks[1:] != ranks[:-1])  # 筛选出ranks中前后rank值不相等的位置

        x, geom_feats = x[kept], geom_feats[kept] # rank值相等的点只留下最后一个，即一个batch中的一个格子里只留最后一个点
        x = torch.cat((x[:1], x[1:] - x[:-1]))  # x后一个减前一个，还原到cumsum之前的x，此时的一个点是之前与其rank相等的点的feature的和，相当于把同一个格子的点特征进行了sum, 1000 + (679-167) = 1512

        # save kept for backward
        ctx.save_for_backward(kept)

        # no gradient for geom_feats
        ctx.mark_non_differentiable(geom_feats)

        return x, geom_feats

对生成的BEV特征利用BEV Encoder做进一步的特征融合 + 语义分割结果预测

a）对BEV特征先利用ResNet-18进行多尺度特征提取，输出的多尺度特征尺寸如下:

level0：(bs, 64, 100, 100)
level1: (bs, 128, 50, 50)
level2: (bs, 256, 25, 25)

b）对输出的多尺度特征进行特征融合 + 对融合后的特征实现BEV网格上的语义分割

Step1: level2 -> Up (4x) -> level2' = (bs, 256, 100, 100)

Step2: concat(level2', level0) -> output = (bs, 320, 100, 100)

Step3: ConvLayer(output) -> output' = (bs, 256, 100, 100)

'''ConvLayer的配置如下
Sequential(
  (0): Conv2d(320, 256, kernel_size=(3, 3), stride=(1, 1), padding=(1, 1), bias=False)
  (1): BatchNorm2d(256, eps=1e-05, momentum=0.1, affine=True, track_running_stats=True)
  (2): ReLU(inplace=True)
  (3): Conv2d(256, 256, kernel_size=(3, 3), stride=(1, 1), padding=(1, 1), bias=False)
  (4): BatchNorm2d(256, eps=1e-05, momentum=0.1, affine=True, track_running_stats=True)
  (5): ReLU(inplace=True)
)'''

Step4: Up2(output') -> final = (bs, 1, 200, 200) # 第二个维度的1就代表BEV每个网格下的二分类结果
'''Up2的配置如下
Sequential(
  (0): Upsample(scale_factor=2.0, mode=bilinear)
  (1): Conv2d(256, 128, kernel_size=(3, 3), stride=(1, 1), padding=(1, 1), bias=False)
  (2): BatchNorm2d(128, eps=1e-05, momentum=0.1, affine=True, track_running_stats=True)
  (3): ReLU(inplace=True)
  (4): Conv2d(128, 1, kernel_size=(1, 1), stride=(1, 1))
)'''

最后就是将输出的语义分割结果与binimgs的真值标注做基于像素的交叉熵损失，从而指导模型的学习过程。

三、总结

1、精度低，对内外参敏感，鲁棒性差

地平面假设在实际工况中，只有较少的场景下满足，且距离越远效果越差；
对有高度的目标，投影后被拉长，畸变严重

2、成本低

特征转换过程非常直观，计算量相对比较小。

四、参考链接

[1] https://zhuanlan.zhihu.com/p/589146284
[2]https://mp.weixin.qq.com/s?__biz=MjM5NDQwNzMxOA==&mid=2650930587&idx=1&sn=4209bfa3f3a6a9816965ddc839f3cbb5&scene=21&poc_token=HBoBamij0VvmKL9LA_G_VA6K1QDijMvSKDbsCrj1

【自动驾驶】经典LSS算法解析——深度估计