自动驾驶:基于PCL的激光雷达感知

磐创AI

    介绍
    自动驾驶是现代技术中一个相对较新且非常迷人的领域。在2004年的DARPA Grand Challenge期间公开展示,并在2007年转向更具挑战性的城市环境,自那以后,工业界和学术界一直在追求自动驾驶。
    这些应用程序在个人自动驾驶汽车、自动出租车、运输、送货等方面都有所不同,但这项技术还没有成熟。
    自动驾驶陷入低谷的原因之一是,感知组件是一个非常复杂的问题。虽然大多数团队都采用基于激光雷达的感知方式,但仍有人试图通过相机来感知(Tesla 和 Wayve)。
    依赖激光雷达的解决方案也可以分为两类:处理点云的传统计算机视觉算法和基于深度学习的方法。
    神经网络有望以较高的平均精度解决感知问题,然而,如果我们想在最坏的情况下证明合理的准确性,这是不够的。
    在本文中,我们将看一看在PCL(一个开源的点云库)的帮助下制作的自动驾驶堆栈。
    首先,我们将坚持系统级的测试驱动开发(TDD),以确保在第一次现场部署之前对我们的整个代码进行彻底测试。
    为此,我们需要一个数据集来运行代码。卡尔斯鲁厄理工学院(Karlsruhe Institute of Technology)和芝加哥丰田理工学院(Toyota Technology Institute)2012年的经典数据集Kitti将非常适合这一目的。这是首批收集的大规模高质量数据集之一,可作为自动驾驶领域计算机视觉算法的基准。
    
    Kitti跟踪由21个同步PNG图像序列、Velodyne激光雷达扫描和来自RT3003 GPS-IMU模块的NMEA记录组成。
    数据集的一个重要特征是传感器之间的彻底相互校准,包括矩阵“Tr_imu_velo”,它是从GPS-imu坐标到Velodyne激光雷达坐标的转换。
    感知管道的架构如下所示。
    
    让我们分别讨论每一个组件,深入挖掘他们的C++实现。
    点云抽取
    为什么我们可能需要从深度传感器(可能是一个或几个激光雷达)中抽取点云?
    自动驾驶软件最重要的要求是满足实时操作约束。
    第一个要求是处理管道要跟上激光雷达扫描采样的速率。在现实生活中,扫描速度可能从10到25次/秒不等,这导致最大延迟为100毫秒到40毫秒不等。如果某些操作导致延迟超过100 ms(对于每秒10次扫描的速度),要么会发生帧丢失,要么管道的总延迟将开始任意增长。这里的解决方案之一是丢掉一些点,而不是丢失整个帧。这将逐渐降低准确性指标(召回率和精度),并保持管道实时运行。
    第二个要求是系统的总体延迟或反应时间。同样,总延迟应该被限制在至少100或200毫秒。对于自动驾驶来说,500ms甚至1秒的反应时间是不可接受的。因此,在算法设计开始时,首先采用抽取的方法处理少量的点是有意义的。
    抽取的标准选项包括:
    1. 有规律的
    2. (伪)随机
    3. 格栅下采样
    常规下采样速度很快,但可能会导致点云上的锯齿模式。随机或伪随机下采样也很快,但可能会导致不可预测的小对象完全消失。像PCL的pcl::VoxelGrid<>类一样的格栅下采样是智能和自适应的,但需要额外的计算和内存。
    原始点云:
    
    大量点云:
    
    多扫描聚合
    多扫描聚合是指当车相对于地面移动时,将多个历史激光雷达扫描记录到共同坐标系的过程。通用的坐标系统可以是局部导航框架或当前的激光雷达传感器坐标。我们将以后者为例。
    这个阶段在理论上是可选的,但在实践中是非常重要的。问题是,后续的聚类阶段依赖于LiDAR点的密度,如果密度不够,可能会产生过聚类的影响。过聚类意味着任何对象(汽车、公共汽车、建筑墙等)都可以被分割成几个部分。
    就其本身而言,这可能不是一个检测障碍的问题,然而,对于感知-跟踪-聚类的下游模块来说,这是一个实质性的挑战。跟踪器可能会不准确地关联对象的各个部分,这最终导致车辆突然刹车。我们绝对不希望聚类中的小错误在下游组件中造成雪崩式的错误。
    多次连续扫描(5到10次)的聚合成比例地增加了落在每个物体上的激光雷达点的密度,并促进了精确的聚类。汽车运动的一个很好的特点是,汽车能够从不同的视角观察同一物体,激光雷达扫描模式覆盖物体的不同部分。
    
    让我们看看执行聚合的代码。
    第一阶段是保留一个限制长度的队列,其中包含历史点云以及后续扫描仪的姿势转换。请注意,我们如何使用从RT3003 GPS-IMU模块获得的平移速度[Vx,Vy]和旋转速度Wz来构造姿势变换。
    // We accumulate the incoming scans along with their localization metadata
    // into a deque to perform subsequent aggregation.
    {
      Transform3f next_veh_pose_vs_curr = Transform3f::Identity();
    if (gpsimu_ptr)
    {
        float frame_interval_sec = 0.1f;
        // First, we need to calculate yaw change given the yaw rate
        // (angular speed over Z axis) and the time inteval between frames.
        float angle_z = gpsimu_ptr->wz * frame_interval_sec;
        auto rot = Eigen::AngleAxisf(angle_z, Eigen::Vector3f::UnitZ());
               next_veh_pose_vs_curr.rotate(rot);
        // Second, we need a translation transform to the next frame
        // given the speed of the ego-vehicle and the frame interval.
        next_veh_pose_vs_curr.translate(Eigen::Vector3f(
                  gpsimu_ptr->vf * frame_interval_sec,
                  gpsimu_ptr->vl * frame_interval_sec,
           0.0f
               ));
      }
    // Since later we want to aggregate all scans into the coordinate
    // frame of the last scans, we need the inverse transform.
    auto curr_veh_pose_vs_next = next_veh_pose_vs_curr.inverse();
    // Put the resulting pair of the cloud and the transform into a queue.
    auto cloud_and_metadata = CloudAndMetadata{decimated_cloud_ptr, curr_veh_pose_vs_next};
    m_queue.push_back(cloud_and_metadata);
    while (m_queue.size() > m_params->m_num_clouds)
    {
          m_queue.pop_front();
      }
    }
    在第二阶段,我们从最新的扫描时间向后遍历队列,进行聚合,并将聚合转换应用到每个历史帧。
    使用这种方法,计算成本为O(N*D),其中N是点的数量,D是历史的深度(扫描的数量)。
    // We accumulate the transforms starting from the latest back in time and
    // transform each historical point cloud into the coordinates of the current frame.
    auto aggregated_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZI> >();
    Eigen::Matrix4f aggragated_transform = Eigen::Matrix4f::Identity();
    for (int i = m_queue.size()-1; i >= 0; i--)
    {
        constauto& cloud_and_metadata = m_queue[i];
        constauto& cloud_ptr = cloud_and_metadata.cloud_ptr;
        constauto& trans = cloud_and_metadata.transform_to_next;
        pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud_ptr;
        if (i != m_queue.size()-1)
    {
          aggragated_transform *= trans.matrix();
          transformed_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZI> >();
          pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, aggragated_transform);
      }
    else
      {
        // For the current scan no need to transform
               transformed_cloud_ptr = cloud_ptr;
      }
    // Concatenate the transformed point cloud into the aggregate cloud
      *aggregated_cloud_ptr += *transformed_cloud_ptr;
    }
    聚合后,如果移动的物体看起来有点模糊,点云会显得有些模糊。可以在聚类阶段进一步解决。在这个阶段,我们需要的是一个更密集的点云,它可以从多个帧中积累信息。
    
    地面移除
    感知堆栈的目的是提供有关动态对象和静止障碍物的信息。汽车应该在道路上行驶,通常路面不被视为障碍物。
    因此,我们可以移除所有从路面反射的激光雷达点。要做到这一点,我们首先将地面检测为平面或曲面,并移除表面周围或下方约10厘米的所有点。有几种方法可以检测点云上的地面:
    1. 用Ransac探测平面
    2. 用Hough变换检测平面
    3. 基于Floodfill的非平面表面检测
    让我们在EGIN和PCL库的帮助下,研究RANSAC的C++实现。
    首先,让我们定义候选平面。我们将使用基点加法向量的形式。
    // A plane is represented with a point on the plane (base_point)
    // and a normal vector to the plane.
    struct Plane
    {
        Eigen::Vector3f base_point;
        Eigen::Vector3f normal;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
    然后,我们定义了一个辅助函数,它允许我们在点云转换为平面坐标后,在Z坐标上找到满足条件的所有点的索引。代码中的注释给出了实现的细节。
    // This helper function finds indices of points that are considered inliers,
    // given a plane description and a condition on distance from the plane.
    std::vector<size_t> find_inlier_indices(
      const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud_ptr,
      const Plane& plane,
      std::function<bool(float)> condition_z_fn)
    {
      typedef Eigen::Transform<float, 3, Eigen::Affine, Eigen::DontAlign> Transform3f;
      auto base_point = plane.base_point;
      auto normal = plane.normal;
      // Before rotation of the coordinate frame we need to relocate the point cloud to
      // the position of base_point of the plane.
      Transform3f world_to_ransac_base = Transform3f::Identity();
      world_to_ransac_base.translate(-base_point);
      auto ransac_base_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
      pcl::transformPointCloud(*input_cloud_ptr, *ransac_base_cloud_ptr, world_to_ransac_base);
      // We are going to use a quaternion to determine the rotation transform
      // which is required to rotate a coordinate system that plane's normal
      // becomes aligned with Z coordinate axis.
      auto rotate_to_plane_quat = Eigen::Quaternionf::FromTwoVectors(
                normal,
                 Eigen::Vector3f::UnitZ(
      ).normalized();
      // Now we can create a rotation transform and align the cloud that
      // the candidate plane matches XY plane
      Transform3f ransac_base_to_ransac = Transform3f::Identity();
      ransac_base_to_ransac.rotate(rotate_to_plane_quat);
      auto aligned_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
      pcl::transformPointCloud(*ransac_base_cloud_ptr, *aligned_cloud_ptr, ransac_base_to_ransac);
      // Once the point cloud is transformed into the plane coordinates,
      // We can apply a simple criterion on Z coordinate to find inliers.
      std::vector<size_t> indices;
      for (size_t i_point = 0; i_point < aligned_cloud_ptr->size(); i_point++)
    {
          constauto& p = (*aligned_cloud_ptr)[i_point];
         if (condition_z_fn(p.z))
       {
              indices.push_back(i_point);
         }
       }
       return indices;
    }
    最后,主要的Ransac实现如下所示。第一步是基于Z坐标对点进行粗略过滤。此外,我们需要再次抽取点,因为我们不需要聚集云中的所有点来验证候选平面。这些操作可以一次完成。
    接下来,我们开始迭代。在C++标准库的 std::mt19937伪随机生成器的帮助下,每次迭代采样3个随机点。对于每个三元组,我们计算平面并确保其法线指向上方。然后我们使用相同的辅助函数find_inlier_index来计算内点的数量。
    迭代结束后,我们剩下的是最佳候选平面,我们最终使用它来复制点云中所有索引不存在于列表中的点的副本。请注意std::unordered_set<>的用法。它允许执行恒定时间O(1)搜索,而不是对std::vector<>进行的线性O(N)搜索。
    // This function performs plane detection with RANSAC sampling of planes
    // that lie on triplets of points randomly sampled from the cloud.
    // Among all trials the plane that is picked is the one that has the highest
    // number of inliers. Inlier points are then removed as belonging to the ground.
    auto remove_ground_ransac(
        pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr)
    {
      // Threshold for rough point dropping by Z coordinate (meters)
      constfloat rough_filter_thr = 0.5f;
      // How much to decimate the input cloud for RANSAC sampling and inlier counting
      constsize_t decimation_rate = 10;
      // Tolerance threshold on the distance of an inlier to the plane (meters)
      constfloat ransac_tolerance = 0.1f;
      // After the final plane is found this is the threshold below which all
      // points are discarded as belonging to the ground.
      constfloat remove_ground_threshold = 0.2f;
      // To reduce the number of outliers (non-ground points) we can roughly crop
      // the point cloud by Z coordinate in the range (-rough_filter_thr, rough_filter_thr).
      // Simultaneously we perform decimation of the remaining points since the full
      // point cloud is excessive for RANSAC.
      std::mt19937::result_type decimation_seed = 41;
      std::mt19937 rng_decimation(decimation_seed);
      auto decimation_gen = std::bind(
           std::uniform_int_distribution<size_t>(0, decimation_rate), rng_decimation);
     
      auto filtered_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
      for (constauto& p : *input_cloud_ptr)
    {
           if ((p.z > -rough_filter_thr) && (p.z < rough_filter_thr))
                {
               // Use random number generator to avoid introducing patterns
              // (which are possible with structured subsampling
              // like picking each Nth point).
               if (decimation_gen() == 0)
                   {
                            filtered_ptr->push_back(p);
                     }
                }
            }
      // We need a random number generator for sampling triplets of points.
      std::mt19937::result_type sampling_seed = 42;
      std::mt19937 sampling_rng(sampling_seed);
      auto random_index_gen = std::bind(
           std::uniform_int_distribution<size_t>(0, filtered_ptr->size()), sampling_rng);
      // Number of RANSAC trials
      constsize_t num_iterations = 25;
      // The best plane is determined by a pair of (number of inliers, plane specification)
      typedefstd::pair<size_t, Plane> BestPair;
      auto best = std::unique_ptr<BestPair>();
      for (size_t i_iter = 0; i_iter < num_iterations; i_iter++)
          {
           // Sample 3 random points.
          // pa is special in the sense that is becomes an anchor - a base_point of the plane
                  Eigen::Vector3f pa = (*filtered_ptr)[random_index_gen()].getVector3fMap();
                  Eigen::Vector3f pb = (*filtered_ptr)[random_index_gen()].getVector3fMap();
                  Eigen::Vector3f pc = (*filtered_ptr)[random_index_gen()].getVector3fMap();
           // Here we figure out the normal to the plane which can be easily calculated
                 // as a normalized cross product.
                  auto vb = pb - pa;
                  auto vc = pc - pa;
                  Eigen::Vector3f normal = vb.cross(vc).normalized();
          // Flip the normal if points down
                 if (normal.dot(Eigen::Vector3f::UnitZ()) < 0)
                {
                       normal = -normal;
                  }
           
          Plane plane{pa, normal};
        
        // Call find_inlier_indices to retrieve inlier indices.
              // We will need only the number of inliers.
              auto inlier_indices = find_inlier_indices(filtered_ptr, plane,
                    [ransac_tolerance](float z) -> bool {
                             return (z >= -ransac_tolerance) && (z <= ransac_tolerance);
                      });
              // If new best plane is found, update the best
              bool found_new_best = false;
              if (best)
             {
                      if (inlier_indices.size() > best->first)
                    {
                              found_new_best = true;
                      }
               }
              else
             {
                    // For the first trial update anyway
                    found_new_best = true;
               }
       if (found_new_best)
            {
                      best = std::unique_ptr<BestPair>(new BestPair{inlier_indices.size(), plane});
              }
      }
      // For the best plane filter out all the points that are
     // below the plane + remove_ground_threshold.
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_ground_ptr;
    if (best)
          {
                    cloud_no_ground_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
                    auto inlier_indices = find_inlier_indices(input_cloud_ptr, best->second,
                         [remove_ground_threshold](float z) -> bool {
                                return z <= remove_ground_threshold;
                           });
                    std::unordered_set<size_t> inlier_set(inlier_indices.begin(), inlier_indices.end());
                    for (size_t i_point = 0; i_point < input_cloud_ptr->size(); i_point++)
                  {
                           bool extract_non_ground = true;
                           if ((inlier_set.find(i_point) == inlier_set.end()) == extract_non_ground)
                         {
                                 constauto& p = (*input_cloud_ptr)[i_point];
                                 cloud_no_ground_ptr->push_back(p);
                           }
                    }
               }
              else
             {
                   cloud_no_ground_ptr = input_cloud_ptr;
               }
        return cloud_no_ground_ptr;
    }
    让我们看看地面移除的结果。
    在移除地面之前:
    
    地面移除后:
    
    移除地面后,我们准备对剩余的点进行聚类,并通过凸包提取来压缩对象元数据。这两个阶段应该有自己的文章。我将在即将到来的第二部分中介绍它们的实现。同时下面是聚类的最终结果——凸包提取。
    可视化的最终对象:
    
    凸包绝对是任何跟踪器都渴望接受作为其输入的元数据类型。它们在RAM使用方面更加紧凑,并且比定向边界框更准确地表示对象的边界。
    KITTI 0003中的聚类点云:
    
    结论
    我相信,在生活质量和整体生产力方面,自动驾驶将是人类的一次飞跃。
    参考资料:
    
    感谢阅读!