Collison Detection between two point clouds using PCL

Question

Given two point clouds such that one point cloud is static whereas other is mobile obstacle. We want to move the mobile point cloud obstacle in space and note down whether it is intersecting with the static point cloud at that position. Is there a function available in PCL to do this automatically or do we have to write our own function to do the same?

Answer 1

The fcl (Flexible Collision Library) library can do fast collision detection.

Here are the supported different object shapes:

• sphere
• box
• cone
• cylinder
• mesh
• octree (optional, octrees are represented using the octomap library http://octomap.github.com)

I assume your point clouds are samples drawn from the surface of objects that occupy a volume in 3D space. So, you'll have to transform your point clouds into a mesh or in an occupancy octree.

Answer 2

Just to extend @fferri 's answer on how to use fcl (Flexible Collision Library) to check collision for your case.

1. Create fcl::CollisionObject from the point cloud.

Here I use octree as the probabilistic representation of the point cloud.

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);
}

As @fferri said in the comment, 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) before you use fcl for collision detection.

2. About the CCD (continuous collision detection) between the moving obstacle and the static point cloud.

Unfortunately, the current implementation of CCD interfaces in fcl doesn't have full support for octree/pointcloud (even the naive CCD solver). But you can work around the problem by either of the following ways:

• sample the trajectory of the moving obstacle and use DCD (discrete collision detection) to check collision. If the motion of obstacle is just translation, it's easy to show that we can generate adequate numbers of samples to make sure it's equivalent to use CCD. However, if the motion contains orientation, the collision free cases using the sampling+DCD method might be actually in collision.

• construct the convex hull of the swept volume of the moving obstacle. Use DCD to make sure the convex hull and the static point cloud is collision free. For how to construct the convex hull for a moving geometry, you can have a look at the Chapter V. in the trajopt paper.