Как сделать обнаружение столкновений между двумя облаками точек или облаком точек и моделью эффектора конца робота, используя гибкую библиотеку столкновений? - PullRequest
Новая
       48

Как сделать обнаружение столкновений между двумя облаками точек или облаком точек и моделью эффектора конца робота, используя гибкую библиотеку столкновений?

0 голосов
/ 11 июня 2019

Я создаю демонстрацию сбора мусора, и мне нужно выполнить проверку столкновения между облаком точек, которое генерируется лазерным сканером, и конечным эффектором робота.

Я планируюсделать эту работу с fcl (гибкая библиотека столкновений) и pcl (библиотека облаков точек).но примеры или учебник по fcl очень ограничены.Я прочитал демо в исходном коде fcl на их странице github и написал пример кода, но я не могу понять это правильно.Ниже приведен код, который я написал: 

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

Этот код может пропустить компиляцию, но всегда выдает ошибку при запуске.

"initialize error Ошибка сегментации"

Может кто-нибудь помочь мне сделать это правильно?Спасибо за вашу помощь!

...