使用opencv ppf icp 进行模型和场景的匹配

Lioia

已于 2024-02-23 10:29:41 修改

阅读量229

点赞数 3

文章标签： opencv 人工智能计算机视觉 c++

于 2024-02-23 10:23:43 首次发布

本文链接：https://blog.csdn.net/qq_43624318/article/details/136249311

版权

了解ppf icp

此链接可查看 ppf icp 意义

结果

程序

在这里插入图片描述

模型

场景模型
在这里插入图片描述
拟合后模型

实现代码

// 讲提取的点云和法相量转化为opencv mat格式
static cv::Mat get_mat(pcl::PointCloud<pcl::PointXYZ>& points, pcl::PointCloud<pcl::Normal>& normals)
{
    cv::Mat mat(points.size(),6, CV_32F);
    int c;
    for (size_t i = 0; i < points.size(); ++i)
    {
        c = i;
        mat.at<float>(i, 0) = points.at(i).x;
        mat.at<float>(i, 1) = points.at(i).y;
        mat.at<float>(i, 2) = points.at(i).z;
        mat.at<float>(i, 3) = normals.at(i).normal_x;
        mat.at<float>(i, 4) = normals.at(i).normal_y;
        mat.at<float>(i, 5) = normals.at(i).normal_z;
    }
    return mat;
}

static Eigen::Matrix4f get_rt(cv::Matx44d& mat)
{
    Eigen::Matrix4f res{};
    for (size_t i = 0; i < 4; ++i)
    {
        res(i, 0) = mat(i, 0);
        res(i, 1) = mat(i, 1);
        res(i, 2) = mat(i, 2);
        res(i, 3) = mat(i, 3);
    }
    return res;
}
static void ppf_test()
{
    using namespace std::string_literals;
    // 模型文件opencv源码里有
    auto modelPath = "D:\\0tmp\\ppfdata\\parasaurolophus_low_normals2.ply"s;
    auto scenePath = "D:\\0tmp\\ppfdata\\rs1_normals.ply"s;

    // 使用 pcl读取模型
    pcl::PointCloud<pcl::PointXYZ>::Ptr modelCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr modelCloudNormals(new pcl::PointCloud<pcl::Normal>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr sceneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr sceneCloudNromals(new pcl::PointCloud<pcl::Normal>);

    pcl::PLYReader re{};
    re.read(modelPath, *modelCloud);
    re.read(modelPath, *modelCloudNormals);
    re.read(scenePath, *sceneCloud);
    re.read(scenePath, *sceneCloudNromals);

    cv::Mat sMat = get_mat(*sceneCloud, *sceneCloudNromals);
    cv::Mat mMat = get_mat(*modelCloud, *modelCloudNormals);
    cv::ppf_match_3d::PPF3DDetector ppf{};
    ppf.trainModel(mMat);
    std::vector<cv::ppf_match_3d::Pose3DPtr> rt;
    double timeStart, timeEnd, timeUse;
    timeStart = static_cast<double>(cv::getTickCount());
    ppf.match(sMat,rt);
    timeEnd = static_cast<double>(cv::getTickCount());
    timeUse = (timeEnd - timeStart) / cv::getTickFrequency();
    std::cout << std::format("ppf time use{}\n", timeUse);
    cv::ppf_match_3d::ICP icp(20);
    timeStart = static_cast<double>(cv::getTickCount());
    auto res = icp.registerModelToScene(mMat, sMat, rt);
    timeEnd = static_cast<double>(cv::getTickCount());
    timeUse = (timeEnd - timeStart) / cv::getTickFrequency();
    std::cout << std::format("icp res{}, time use{}\n", res, timeUse);
    Eigen::Matrix4f pose = get_rt(rt[0]->pose);
    pcl::PointCloud<pcl::PointXYZ>::Ptr targetPoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*modelCloud, *targetPoints, pose);
    pcl::PLYWriter pw{};
    // 对模型文件应用rt时期能拟合场景
    pw.write("D:\\MoveModel.ply", *targetPoints);
}