使用PCL在两点云之间进行Collison检测

Question

鉴于两点云，一点云是静态的，而另一点是移动障碍。我们想要移动空间中的移动点云障碍并记下它是否与该位置处的静态点云相交。 PCL中是否有自动执行此功能的功能，或者我们是否必须编写自己的功能来执行此操作？

Answer 1

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

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

• 球
• 框
• 锥
• 气缸
• 目
• octree（可选，使用octomap库http://octomap.github.com表示八叉树）

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

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在the comment中所说的那样，您也可以在pcl中使用三角函数从点云创建网格。但是，您应该知道GJK / EPA算法不能支持凹面对象。因此，在使用fcl进行碰撞检测之前，您可能需要通过凸分解处理网格（例如，可以使用CGAL）。

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

不幸的是，fcl中的CCD接口的当前实现不完全支持octree / pointcloud（甚至是朴素的CCD求解器）。但是您可以通过以下两种方法之一解决此问题：

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

• 构造移动障碍物的扫掠体的凸包。使用DCD确保凸包和静态点云没有碰撞。有关如何为运动的几何构造凸包，请查看the trajopt paper中的第五章。