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

Question

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

Answer 1

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

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

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

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

Answer 2

只是为了扩展@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 在, You can also use the triangulation functions in pcl to create mesh from point cloud. However, you should know that the GJK/EPA algorithm cannot support concave objects. So properly you might need to process the mesh by convex decomposition (e.g. You can use CGAL) 中所说，在使用fcl 进行碰撞检测之前。

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

不幸的是，fcl 中 CCD 接口的当前实现不完全支持 octree/pointcloud（即使是简单的 CCD 求解器）。但您可以通过以下任一方式解决此问题：

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

• 构造运动障碍物扫掠体的凸包。使用 DCD 确保凸包和静态点云无碰撞。运动几何的凸包构造方法，可以看the trajopt paper.

中的第五章