使用 PCL 的两个点云之间的碰撞检测

Question

给定两个点云，一个点云是静态的，而另一个是移动的障碍物。我们想在空间中移动移动点云障碍物，并记下它是否与该位置的静态点云相交。 PCL 中是否有可用的函数来自动执行此操作，还是我们必须编写自己的函数才能执行此操作？

Answer 1

只是为了扩展@fferri 关于如何使用fcl（灵活碰撞库）来检查您的案例的碰撞的答案。

1。从点云创建fcl::CollisionObject。

这里我使用八叉树作为点云的概率表示。

std::shared_ptr<fcl::CollisionObject> createCollisionObject(const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr, const octomap::point3d& sensor_origin_wrt_world)
{
  // octomap octree settings
  const double resolution = 0.01;
  const double prob_hit = 0.9;
  const double prob_miss = 0.1;
  const double clamping_thres_min = 0.12;
  const double clamping_thres_max = 0.98;

  std::shared_ptr<octomap::OcTree> octomap_octree = std::make_shared<octomap::OcTree>(resolution);
  octomap_octree->setProbHit(prob_hit);
  octomap_octree->setProbMiss(prob_miss);
  octomap_octree->setClampingThresMin(clamping_thres_min);
  octomap_octree->setClampingThresMax(clamping_thres_max);

  octomap::KeySet free_cells;
  octomap::KeySet occupied_cells;

#if defined(_OPENMP)
#pragma omp parallel
#endif
  {
#if defined(_OPENMP)
    auto thread_id = omp_get_thread_num();
    auto thread_num = omp_get_num_threads();
#else
    int thread_id = 0;
    int thread_num = 1;
#endif
    int start_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * thread_id;
    int end_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * (thread_id + 1);
    if (thread_id == thread_num - 1)
    {
      end_idx = pointcloud_ptr->size();
    }

    octomap::KeySet local_free_cells;
    octomap::KeySet local_occupied_cells;

    for (auto i = start_idx; i < end_idx; i++)
    {
      octomap::point3d point((*pointcloud_ptr)[i].x, (*pointcloud_ptr)[i].y, (*pointcloud_ptr)[i].z);
      octomap::KeyRay key_ray;
      if (octomap_octree->computeRayKeys(sensor_origin_3d, point, key_ray))
      {
        local_free_cells.insert(key_ray.begin(), key_ray.end());
      }

      octomap::OcTreeKey tree_key;
      if (octomap_octree->coordToKeyChecked(point, tree_key))
      {
        local_occupied_cells.insert(tree_key);
      }
    }

#if defined(_OPENMP)
#pragma omp critical
#endif
    {
      free_cells.insert(local_free_cells.begin(), local_free_cells.end());
      occupied_cells.insert(local_occupied_cells.begin(), local_occupied_cells.end());
    }
  }

  // free cells only if not occupied in this cloud
  for (auto it = free_cells.begin(); it != free_cells.end(); ++it)
  {
    if (occupied_cells.find(*it) == occupied_cells.end())
    {
      octomap_octree->updateNode(*it, false);
    }
  }

  // occupied cells
  for (auto it = occupied_cells.begin(); it != occupied_cells.end(); ++it)
  {
    octomap_octree->updateNode(*it, true);
  }

  auto fcl_octree = std::make_shared<fcl::OcTree>(octomap_octree);
  std::shared_ptr<fcl::CollisionGeometry> fcl_geometry = fcl_octree;
  return std::make_shared<fcl::CollisionObject>(fcl_geometry);
}

正如@fferri 在the comment 中所说，您还可以使用pcl 中的三角测量功能从点云创建网格。但是，您应该知道 GJK/EPA 算法不支持凹面对象。因此，在使用fcl 进行碰撞检测之前，您可能需要通过凸分解处理网格（例如，您可以使用CGAL）。

2。关于移动障碍物与静态点云之间的CCD（连续碰撞检测）。

不幸的是，fcl 中 CCD 接口的当前实现不完全支持八叉树/点云（即使是幼稚的 CCD 求解器）。但您可以通过以下任一方式解决此问题：

• 对移动障碍物的轨迹进行采样，并使用 DCD（离散碰撞检测）来检查碰撞。如果障碍物的运动只是平移，很容易证明我们可以生成足够数量的样本，以确保它与使用 CCD 等效。但是，如果运动包含方向，则使用采样+DCD方法的无碰撞情况可能实际上是在碰撞中。

• 构造移动障碍物扫过体积的凸包。使用 DCD 确保凸包和静态点云无碰撞。关于如何为移动几何构造凸包，可以看the trajopt paper的第五章。

Answer 2

fcl（灵活碰撞库）库可以进行快速碰撞检测。

以下是支持的不同对象形状：

• 球体
• 盒子
• 锥体
• 气缸
• 网格
• 八叉树（可选，八叉树使用八图库http://octomap.github.com表示）

我假设您的点云是从在 3D 空间中占据一定体积的物体表面抽取的样本。因此，您必须将点云转换为网格或占用八叉树。