How to do collision detection between two point clouds or a point cloud and a robot end effector model using flexible collision library?

Question

I am building a bin-picking demo, and I need to do a collision check between the point cloud which is generated by the laser scanner and the end-effector of the robot.

I'm planning to do this job with fcl(flexible collision library) and pcl(point cloud library). but the examples or tutorial of fcl is very limited. I read the demo in the fcl source code in their github page, and wrote a code sample, but I cannot get it right. the following is the code I wrote:

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/narrowphase/collision.h>

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr obj1_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr obj2_cloud(new pcl::PointCloud<pcl::PointXYZ>());

    pcl::PLYReader ply_reader;
    ply_reader.read(argv[1], *obj1_cloud);
    ply_reader.read(argv[2], *obj2_cloud);

    std::shared_ptr<fcl::BVHModel<fcl::AABB<double>>> model1(new fcl::BVHModel<fcl::AABB<double>>);
    std::shared_ptr<fcl::BVHModel<fcl::AABB<double>>> model2(new fcl::BVHModel<fcl::AABB<double>>);

    model1->beginModel();
    for (int i = 0; i < obj1_cloud->points.size(); i++)
    {
        fcl::Vector3d point;
        point(0) = obj1_cloud->points[i].x;
        point(1) = obj1_cloud->points[i].y;
        point(2) = obj1_cloud->points[i].z;
        model1->addVertex(point);
    }
    model1->endModel();
    model1->computeLocalAABB();

    model2->beginModel();
    for (int i = 0; i < obj2_cloud->points.size(); i++)
    {
        fcl::Vector3d point;
        point(0) = obj2_cloud->points[i].x;
        point(1) = obj2_cloud->points[i].y;
        point(2) = obj2_cloud->points[i].z;
        model2->addVertex(point);
    }
    model2->endModel();
    model2->computeLocalAABB();

    fcl::Transform3<double> pose1 = fcl::Transform3<double>::Identity();
    fcl::Transform3<double> pose2 = fcl::Transform3<double>::Identity();

    fcl::CollisionRequest<double> collision_request;
    collision_request.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;

    fcl::CollisionResult<double> collision_result;

    fcl::detail::MeshCollisionTraversalNode<fcl::AABB<double>> traveral_node;

    if (!fcl::detail::initialize(traveral_node, *model1, pose1, *model2, pose2, collision_request, collision_result))
        std::cout << "initialize error" << std::endl;
    fcl::detail::collide(&traveral_node);
}

This code can pass the compile but always gives me the error, when it is running.

"initialize error Segmentation fault"

Can anyone help me to get it right? Thank you for your helping!

Answer 1

AFAIK from the source code of fcl, MeshCollisionTraversalNode is supposed to be used for BVHModel whose ModelType is BVH_MODEL_TRIANGLES. In your program, you only add all the vertices from point cloud to the BVHModel, so its ModelType is BVH_MODEL_POINTCLOUD, which is not meeting the requirement of MeshCollisionTraversalNode.

I think you have two ways to solve the problem:

One is to run triangulation on the point cloud (Refer to: http://pointclouds.org/documentation/tutorials/greedy_projection.php) and construct a BVHModel of BVH_MODEL_TRIANGLES. You might also need to use convex decomposition before collision detection if the mesh model generated from pcl is concave.

The other way is to use octree as the representation of the point cloud. You can refer to the answer for further details.