如何使用灵活的碰撞库在两个点云或点云与机器人末端执行器模型之间进行碰撞检测

Question

我正在构建一个料框抓取演示，我需要在激光扫描仪生成的点云和机器人的末端执行器之间进行碰撞检查。

我打算用fcl(灵活的碰撞库(和pcl(点云库(来完成这项工作。 但是FCL的示例或教程非常有限。我在他们的 github 页面阅读了 fcl 源代码中的演示，并编写了一个代码示例，但我无法正确理解。以下是我编写的代码：

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

这段代码可以通过编译，但在运行时总是给我错误。

"初始化错误分段错误">

谁能帮我把事情做好？感谢您的帮助！

Answer 1

AFAIK 来自 fcl 的源代码，MeshCollisionTraversalNode应该用于ModelType BVH_MODEL_TRIANGLES BVHModel。在你的程序中，你只把点云的所有顶点都加到BVHModel，所以它的ModelType是BVH_MODEL_POINTCLOUD的，这不符合MeshCollisionTraversalNode的要求。

我认为您有两种方法可以解决问题：

一种是在点云上运行三角测量(参考：http://pointclouds.org/documentation/tutorials/greedy_projection.php(并构造BVH_MODEL_TRIANGLES的BVHModel。如果从pcl生成的网格模型是凹的，则可能还需要在碰撞检测之前使用凸分解。

另一种方法是使用八叉树作为点云的表示。您可以参考答案以获取更多详细信息。