利用FCL实现更加精准的碰撞检测

一，需求

利用OSG结合FCL实现实现精准的碰撞检测。

二，效果  看这里

利用FCL实现更加精准的碰撞检测 – Qt hello

三，分析

我们看如下这张图，碰撞的逻辑就是，在一个三维场景中，构造一个实体，比如下边的BoxA，然后在物理引擎比如bullet中，或者专用的碰撞检测库中也构造一个对应的实体，比如BoxB。之后在BoxA位姿改变时后，将BoB的位姿也做相应的更新。之后发生碰撞时，物理引擎或者FCL就会给出信号。而这个场景，可以是VTK或者OSG。而碰撞检测可以用Bullet也可以用FCL。

之前用bullet做个尝试，基本的图形能满足需求，比如球，盒子，但是项目中涉及到点云的碰撞，而bullet中处理点云，没有找到好的处理方式。但是FCL可以将点云转变成fcl中对应的实体，因此最终选择了FCL进行碰撞检测，这里列出FCL中大概的步骤。

1，FCL中构造实体。这里构造了一个 盒子。

auto box_geometry = std::make_shared<fcl::Boxf>(w, d, h);
auto ob = new fcl::CollisionObjectf(box_geometry);

2，更新FCL实体的位姿矩阵。

void FCLManager::updateTrans(const std::string &name, const fcl::Transform3f &trans)
{
    fcl::CollisionObjectf *ob=getCollisionObject(name);
    if(ob){
        ob->setTransform(trans);
    }
}

//OSG 矩阵 需要进行转换 才能给到FCL使用
//osg 矩阵转fcl矩阵
osg::Vec3 osgTrans = mt.getTrans(); // 获取平移分量
osg::Quat osgQuat = mt.getRotate(); // 获取旋转分量
 
fcl::Quaternionf rotation(osgQuat.w(), osgQuat.x(), osgQuat.y(), osgQuat.z());
fcl::Vector3f translation(osgTrans.x(), osgTrans.y(), osgTrans.z());
fcl::Transform3f fclTrans=fcl::Transform3f::Identity();
fclTrans.translation() = translation;
fclTrans.linear()=rotation.toRotationMatrix();
FCLManager::getInstance()->updateTrans(this->getName(),fclTrans);

3，碰撞检测

我是检测机器人和其它障碍物的碰撞，这里把机器人关节放到一个集合中，把其它障碍物放到另一个集合中

 
bool FCLManager::detectCollision()
{
    fcl::CollisionRequestf request;
    fcl::CollisionResultf result;
    for(auto &ob1:jointMap){
        for(auto &ob2:obstacleMap){
            collide(ob1.second, ob2.second, request, result);
            if(result.isCollision()){
                return true;
            }
        }
    }
    return false;
}

4，FCL支持三角面检测。因此我们在FCL中构造对应实体的时候，可以直接用三角面。这样不管OSG中构造的时盒子还是球，还是导入的stl，对应FCL中都是统一用三角面处理。

void FCLManager::addTriMesh(const std::string &name, osg::Node *node)
{
     fcl::CollisionObjectf *obj = createNodeCollisionObject(node);
     obstacleMap.emplace(name,obj);
}
 
fcl::CollisionObjectf *FCLManager::createNodeCollisionObject(osg::Node *node)
{
    MyComputeTriMeshVisitor visitor;
    node->accept( visitor );
    osg::Vec3Array* vertices = visitor.getTriMesh();
 
    typedef fcl::BVHModel<fcl::OBBRSSf> Model;
    Model* model = new Model();
    std::shared_ptr<fcl::CollisionGeometryf> m1_ptr(model);
    model->beginModel();
    osg::Vec3 p1, p2, p3;
    for( size_t i = 0; i + 2 < vertices->size(); i += 3 )
    {
        p1 = vertices->at( i );
        p2 = vertices->at( i + 1 );
        p3 = vertices->at( i + 2 );
 
        fcl::Vector3<float> pp1{p1.x(),p1.y(),p1.z()};
        fcl::Vector3<float> pp2{p2.x(),p2.y(),p2.z()};
        fcl::Vector3<float> pp3{p3.x(),p3.y(),p3.z()};
 
        model->addTriangle(pp1, pp2, pp3);
    }
    model->endModel();
    model->computeLocalAABB();
 
    return new  fcl::CollisionObjectf(m1_ptr);
}

5，点云的碰撞。点云的碰撞 使用了一种叫做八叉树的算法。首先将点云转成pcl的点云 格式，然后可以直接构造出fcl实体，这也是选用FCL的原因。

fcl::CollisionObjectf* FCLManager::createPointCloudCollisionObject(const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr, const octomap::point3d &origin_3d)
{
    // 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(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<float>>(octomap_octree);
    std::shared_ptr<fcl::CollisionGeometryf> fcl_geometry = fcl_octree;
    return new fcl::CollisionObjectf(fcl_geometry);
}

四，总结  看这里

利用FCL实现更加精准的碰撞检测 – Qt hello