激光雷达和点云算法汇总（长期更新）

1.1 激光雷达硬件平台

1.2 激光雷达原理

脉冲式激光测距由激光发射器发射出的激光经被测量物体的反射后又被接收。测距仪同时记录激光往返的时间。光速和往返时间的乘积的一半就是测距仪和被测量物体之间的距离，设备记录本身在水平和垂直方向的旋转角度，再通过软件，计算出三维数据。

相位式激光测距是通过测量调制的激光信号在待测距离上往返传播所形成的相位移，间接测出激光传播时间(利用波长和频率)，再根据激光传播速度，求出待测距离。

1.3 三维激光系统研发难点

• 测距测角模块 三维激光最核心的技术—测距和测角模块、光学设计，接收板电路，编码器精度等
• 高精度高速度主控板 快速高精度处理测距、测角对主控板(CPU)要求高，这是研发的难点和关键
• 系统协调 FPGA、arm、安卓3系统同时运行，对时序的准确性、同步性要求极高，程序、固件协调性难度很大
• 校准软件 由于测距长、测角测距精度要求高，且面处理数据量大，需要优化算法，开发出校准软件。
• 激光器 目前很难找到精度高、稳定性好的国产激光器，主流的九法是使用进口激光器，这需要大量的适配工作
• 驱动电机 扫猫用的电机多为进口产品，特点是高速度、高精度、稳定耐用。测角编码唱也多为进口，与电机配套的驱动器也需要研发，难度较大

1.4 点云应用方向

由N NN个D DD维的点组成，当这个D = 3 D=3D=3的时候一般代表着( x , y , z ) (x,y,z)(x,y,z)的坐标，当然也可以包括一些法向量、强度等别的特征。这是今天主要讲述的数据类型。

• 无序性
• 近密远疏的特性
• 非结构化数据
  

点云应用方向：
机器人、自动驾驶

• 本地化——SLAM、闭环、注册
• 感知——物体检测、分类
• 重建——SfM，注册

消费类电子产品

• 人脸检测/重建——FaceID
• 手部姿势——Hololens
• 人体姿势——Kinect
  

1.5 点云分类，点云分割，点云特征提取（pointnet++）

点云的分类是将点云分类到不同的点云集。同一个点云集具有相似或相同的属性，例如地面、树木、人等；点云分割是根据空间、几何和纹理等特征点进行划分，同一划分内的点云拥有相似的特征。
基于半径选择局部区域，针对得到的每个区域进行特征提取，关键核心原理：

• 最远点采样法（farthest point sampling）：尽可能覆盖到原始点云数据，例如输入1024个点，要选择128个中心点
  
  farthest point sampling代码实现如下：

def farthest_point_sample(xyz, npoint):
    """
    Input:
        xyz: pointcloud data, [B, N, 3]
        npoint: number of samples
    Return:
        centroids: sampled pointcloud index, [B, npoint]
    """
    device = xyz.device
    B, N, C = xyz.shape
    centroids = torch.zeros(B, npoint, dtype=torch.long).to(device)#8*512
    distance = torch.ones(B, N).to(device) * 1e10 #8*1024
    farthest = torch.randint(0, N, (B,), dtype=torch.long).to(device)#batch里每个样本随机初始化一个最远点的索引
    batch_indices = torch.arange(B, dtype=torch.long).to(device)
    for i in range(npoint):
        centroids[:, i] = farthest #第一个采样点选随机初始化的索引
        centroid = xyz[batch_indices, farthest, :].view(B, 1, 3)#得到当前采样点的坐标 B*3
        dist = torch.sum((xyz - centroid) ** 2, -1)#计算当前采样点与其他点的距离
        mask = dist < distance#选择距离最近的来更新距离（更新维护这个表）
        distance[mask] = dist[mask]#
        farthest = torch.max(distance, -1)[1]#重新计算得到最远点索引（在更新的表中选择距离最大的那个点）
    return centroids

• 分组（gouping）：输入为batch10246（1024个点，每个点对应3个坐标3个法向量信息）， 分组后输出为：batch12816*6(128个中心点，每个簇16个样本)
  gouping代码实现如下：

def query_ball_point(radius, nsample, xyz, new_xyz):
    """
    Input:
        radius: local region radius
        nsample: max sample number in local region
        xyz: all points, [B, N, 3]
        new_xyz: query points, [B, S, 3]
    Return:
        group_idx: grouped points index, [B, S, nsample]
    """
    device = xyz.device
    B, N, C = xyz.shape
    _, S, _ = new_xyz.shape
    group_idx = torch.arange(N, dtype=torch.long).to(device).view(1, 1, N).repeat([B, S, 1])
    sqrdists = square_distance(new_xyz, xyz)#得到B N M （就是N个点中每一个和M中每一个的欧氏距离）
    group_idx[sqrdists > radius ** 2] = N #找到距离大于给定半径的设置成一个N值（1024）索引
    group_idx = group_idx.sort(dim=-1)[0][:, :, :nsample]#做升序排序，后面的都是大的值（1024）
    group_first = group_idx[:, :, 0].view(B, S, 1).repeat([1, 1, nsample])#如果半径内的点没那么多，就直接用第一个点来代替了。。。
    mask = group_idx == N
    group_idx[mask] = group_first[mask]
    return group_idx

• 维度变换（bnpointsnsamplefeatures，8128166->8616*128）
• 卷积操作（例如：in=6，out=64）（86416*128）
• MAX操作，得到（864128）
• 多次采样，分组，卷积 ，采样中心点（1024->512->128）半径为0.1,0.2,0.4；以及簇采样点个数，每一次操作时，都要进行特征拼接，得到batch中心点个数特征，执行拼接操作（b512128,b512256,b512512）->(b512896)

特征提取代码实现如下：

def forward(self, xyz, points):
        """
        Input:
            xyz: input points position data, [B, C, N]
            points: input points data, [B, D, N]
        Return:
            new_xyz: sampled points position data, [B, C, S]
            new_points_concat: sample points feature data, [B, D', S]
        """
        xyz = xyz.permute(0, 2, 1) #就是坐标点位置特征
        print(xyz.shape)
        if points is not None:
            points = points.permute(0, 2, 1) ##就是额外提取的特征，第一次的时候就是那个法向量特征
        print(points.shape)
        B, N, C = xyz.shape
        S = self.npoint
        new_xyz = index_points(xyz, farthest_point_sample(xyz, S))
        print(new_xyz.shape)
        new_points_list = []
        for i, radius in enumerate(self.radius_list):
            K = self.nsample_list[i]
            group_idx = query_ball_point(radius, K, xyz, new_xyz)#返回的是索引
            grouped_xyz = index_points(xyz, group_idx)#得到各个组中实际点
            grouped_xyz -= new_xyz.view(B, S, 1, C)#去mean new_xyz相当于簇的中心点
            if points is not None:
                grouped_points = index_points(points, group_idx)
                grouped_points = torch.cat([grouped_points, grouped_xyz], dim=-1)
                print(grouped_points.shape)
            else:
                grouped_points = grouped_xyz

            grouped_points = grouped_points.permute(0, 3, 2, 1)  # [B, D, K, S]
            print(grouped_points.shape)
            for j in range(len(self.conv_blocks[i])):
                conv = self.conv_blocks[i][j]
                bn = self.bn_blocks[i][j]
                grouped_points =  F.relu(bn(conv(grouped_points)))
            print(grouped_points.shape)
            new_points = torch.max(grouped_points, 2)[0]  # [B, D', S] 就是pointnet里的maxpool操作
            print(new_points.shape)
            new_points_list.append(new_points)

        new_xyz = new_xyz.permute(0, 2, 1)
        new_points_concat = torch.cat(new_points_list, dim=1)
        print(new_points_concat.shape)
        return new_xyz, new_points_concat

• pointnet得到最终整体特征，再进行分类
  

pointnet得到最终整体特征，再进行分割

1.6 点云补全（PF-Net）

点云补全就是希望基于观察到的残缺不全的点云生成完整的 3D 点云。由于扫描或者距离的原因导致点云局部缺失，对其进行补全，传统算法可能会补不完整，也可能会补的过于完整，今天这里要讲的是PF-Net: Point Fractal Network for 3D Point Cloud Completion，整体网络模型：

关键核心：

• 利用骨骼点来逐级恢复点云
• 在构建标签时依旧选择最远点采样

• 特征提取，融合多尺度特征，信息更丰富
  

PF-Net特征提取代码实现如下：

def forward(self,x):
        print(x.shape)
        x = torch.unsqueeze(x,1)
        print(x.shape)
        x = F.relu(self.bn1(self.conv1(x)))
        print(x.shape)
        x = F.relu(self.bn2(self.conv2(x)))
        print(x.shape)
        x_128 = F.relu(self.bn3(self.conv3(x)))
        print(x_128.shape)
        x_256 = F.relu(self.bn4(self.conv4(x_128)))
        x_512 = F.relu(self.bn5(self.conv5(x_256)))
        x_1024 = F.relu(self.bn6(self.conv6(x_512)))
        print(x_1024.shape)
        x_128 = torch.squeeze(self.maxpool(x_128),2)
        print(x_128.shape)
        x_256 = torch.squeeze(self.maxpool(x_256),2)
        x_512 = torch.squeeze(self.maxpool(x_512),2)
        x_1024 = torch.squeeze(self.maxpool(x_1024),2)
        print(x_1024.shape)
        L = [x_1024,x_512,x_256,x_128]
        x = torch.cat(L,1)
        print(x.shape)
        return x

• 输出各阶段预测点，还考虑骨骼之间的关系
  

PF-Net分层预测代码实现如下：

def forward(self,x):
        print(np.array(x).shape)
        x = self.latentfeature(x)
        print(x.shape)
        x_1 = F.relu(self.fc1(x)) #1024
        print(x_1.shape)
        x_2 = F.relu(self.fc2(x_1)) #512
        print(x_2.shape)
        x_3 = F.relu(self.fc3(x_2))  #256
        print(x_3.shape)
        
        
        pc1_feat = self.fc3_1(x_3)
        print(pc1_feat.shape)
        pc1_xyz = pc1_feat.reshape(-1,64,3) #64x3 center1
        print(pc1_xyz.shape)
        
        pc2_feat = F.relu(self.fc2_1(x_2))
        print(pc2_feat.shape)
        pc2_feat = pc2_feat.reshape(-1,128,64)
        print(pc2_feat.shape)
        pc2_xyz =self.conv2_1(pc2_feat) #6x64 center2
        print(pc2_xyz.shape)
        
        pc3_feat = F.relu(self.fc1_1(x_1))
        print(pc3_feat.shape)
        pc3_feat = pc3_feat.reshape(-1,512,128)
        print(pc3_feat.shape)
        pc3_feat = F.relu(self.conv1_1(pc3_feat))
        print(pc3_feat.shape)
        pc3_feat = F.relu(self.conv1_2(pc3_feat))
        print(pc3_feat.shape)
        pc3_xyz = self.conv1_3(pc3_feat) #12x128 fine
        print(pc3_xyz.shape)
        
        pc1_xyz_expand = torch.unsqueeze(pc1_xyz,2)
        print(pc1_xyz_expand.shape)
        pc2_xyz = pc2_xyz.transpose(1,2)
        print(pc2_xyz.shape)
        pc2_xyz = pc2_xyz.reshape(-1,64,2,3)
        print(pc2_xyz.shape)
        pc2_xyz = pc1_xyz_expand+pc2_xyz
        print(pc2_xyz.shape)
        pc2_xyz = pc2_xyz.reshape(-1,128,3) 
        print(pc2_xyz.shape)
        
        pc2_xyz_expand = torch.unsqueeze(pc2_xyz,2)
        print(pc2_xyz_expand.shape)
        pc3_xyz = pc3_xyz.transpose(1,2)
        print(pc3_xyz.shape)
        pc3_xyz = pc3_xyz.reshape(-1,128,int(self.crop_point_num/128),3)
        print(pc3_xyz.shape)
        pc3_xyz = pc2_xyz_expand+pc3_xyz
        print(pc3_xyz.shape)
        pc3_xyz = pc3_xyz.reshape(-1,self.crop_point_num,3) 
        print(pc3_xyz.shape)
        
        return pc1_xyz,pc2_xyz,pc3_xyz #center1 ,center2 ,fine

• Chamfer Distance来衡量预测效果与GT之间的差异
  
• 整体架构还是GAN形式
  
  BCELoss模块代码实现如下:

import math

r11 = 0 * math.log(0.8707) + (1-0) * math.log((1 - 0.8707))
r12 = 1 * math.log(0.7517) + (1-1) * math.log((1 - 0.7517))
r13 = 1 * math.log(0.8162) + (1-1) * math.log((1 - 0.8162))

r21 = 1 * math.log(0.3411) + (1-1) * math.log((1 - 0.3411))
r22 = 1 * math.log(0.4872) + (1-1) * math.log((1 - 0.4872))
r23 = 1 * math.log(0.6815) + (1-1) * math.log((1 - 0.6815))

r31 = 0 * math.log(0.4847) + (1-0) * math.log((1 - 0.4847))
r32 = 0 * math.log(0.6589) + (1-0) * math.log((1 - 0.6589))
r33 = 0 * math.log(0.5273) + (1-0) * math.log((1 - 0.5273))

r1 = -(r11 + r12 + r13) / 3
#0.8447112733378236
r2 = -(r21 + r22 + r23) / 3
#0.7260397266631787
r3 = -(r31 + r32 + r33) / 3
#0.8292933181294807
bceloss = (r1 + r2 + r3) / 3 
print(bceloss)

判别模块代码实现如下：

 def forward(self, x):
        x = F.relu(self.bn1(self.conv1(x)))
        x_64 = F.relu(self.bn2(self.conv2(x)))
        x_128 = F.relu(self.bn3(self.conv3(x_64)))
        x_256 = F.relu(self.bn4(self.conv4(x_128)))
        x_64 = torch.squeeze(self.maxpool(x_64))
        x_128 = torch.squeeze(self.maxpool(x_128))
        x_256 = torch.squeeze(self.maxpool(x_256))
        Layers = [x_256,x_128,x_64]
        x = torch.cat(Layers,1)
        x = F.relu(self.bn_1(self.fc1(x)))
        x = F.relu(self.bn_2(self.fc2(x)))
        x = F.relu(self.bn_3(self.fc3(x)))
        x = self.fc4(x)
        return x

1.7 点云配准（RPM-Net）

点云配准实际上可以理解为:通过计算得到完美的坐标变换,将处于不同视角下的点云数据经过旋转平移等刚性变换统一整合到指定坐标系之下的过程。配准应用是一个基础技术，下游任务很多

传统算法代表：ICP，RPM等，涉及很多经验参数选择，今天这里要讲的是RPM-Net: Robust Point Matching using Learned Features，一条龙服务得到变换矩阵，效果对比：

关键核心：

1. 经过PointNet++提取输入的特征（X和Y分别进行特征提取）
2. 使用得到的特征去预测传统算法（RPM）所需参数并求变换矩阵
   
   Parameter Prediction ©模块代码实现如下：

def forward(self, x):
        """ Returns alpha, beta, and gating_weights (if needed)

        Args:
            x: List containing two point clouds, x[0] = src (B, J, 3), x[1] = ref (B, K, 3)

        Returns:
            beta, alpha, weightings
        """

        src_padded = F.pad(x[0], (0, 1), mode='constant', value=0)
        ref_padded = F.pad(x[1], (0, 1), mode='constant', value=1)
        concatenated = torch.cat([src_padded, ref_padded], dim=1)
        print(concatenated.shape)
        print(concatenated.permute(0, 2, 1).shape)

        prepool_feat = self.prepool(concatenated.permute(0, 2, 1))
        print(prepool_feat.shape)
        pooled = torch.flatten(self.pooling(prepool_feat), start_dim=-2)
        print(pooled.shape)
        raw_weights = self.postpool(pooled)
        print(raw_weights.shape)
        beta = F.softplus(raw_weights[:, 0])
        alpha = F.softplus(raw_weights[:, 1])

        return beta, alpha

Feature Extraction(B)模块代码实现如下：

def sample_and_group_multi(npoint: int, radius: float, nsample: int, xyz: torch.Tensor, normals: torch.Tensor,
                           returnfps: bool = False):
    """Sample and group for xyz, dxyz and ppf features

    Args:
        npoint(int): Number of clusters (equivalently, keypoints) to sample.
                     Set to negative to compute for all points
        radius(int): Radius of cluster for computing local features
        nsample: Maximum number of points to consider per cluster
        xyz: XYZ coordinates of the points
        normals: Corresponding normals for the points (required for ppf computation)
        returnfps: Whether to return indices of FPS points and their neighborhood

    Returns:
        Dictionary containing the following fields ['xyz', 'dxyz', 'ppf'].
        If returnfps is True, also returns: grouped_xyz, fps_idx
    """

    B, N, C = xyz.shape

    if npoint > 0:
        S = npoint
        fps_idx = farthest_point_sample(xyz, npoint)  # [B, npoint, C]
        new_xyz = index_points(xyz, fps_idx)
        nr = index_points(normals, fps_idx)[:, :, None, :]
    else:
        S = xyz.shape[1]
        fps_idx = torch.arange(0, xyz.shape[1])[None, ...].repeat(xyz.shape[0], 1).to(xyz.device)
        new_xyz = xyz
        nr = normals[:, :, None, :]

    idx = query_ball_point(radius, nsample, xyz, new_xyz, fps_idx)  # (B, npoint, nsample)
    grouped_xyz = index_points(xyz, idx)  # (B, npoint, nsample, C)
    print(grouped_xyz.shape)
    d = grouped_xyz - new_xyz.view(B, S, 1, C)  # d = p_r - p_i  (B, npoint, nsample, 3)
    ni = index_points(normals, idx)
    print(ni.shape)
    print(nr.shape)
    print(d.shape)
    nr_d = angle(nr, d)
    print(nr_d.shape)
    ni_d = angle(ni, d)
    print(ni_d.shape)
    nr_ni = angle(nr, ni)
    print(nr_ni.shape)
    d_norm = torch.norm(d, dim=-1)

    xyz_feat = d  # (B, npoint, n_sample, 3)
    ppf_feat = torch.stack([nr_d, ni_d, nr_ni, d_norm], dim=-1)  # (B, npoint, n_sample, 4)
    print(ppf_feat.shape)
    if returnfps:
        return {
    
    'xyz': new_xyz, 'dxyz': xyz_feat, 'ppf': ppf_feat}, grouped_xyz, fps_idx
    else:
        return {
    
    'xyz': new_xyz, 'dxyz': xyz_feat, 'ppf': ppf_feat}

1.8 点云算法项目应用

如果需要本文完整代码，以上算法论文或者点云数据资源的小伙伴可以私信我哦！