“悉灵杯”课题研究-基于点云配准算法GICP的3D抓取方案研究
  • 948
  • 0
  • 分享
  • 2023-09-13 12:41
  • RGB-D相机
基于点云配准算法GICP，对采集到的3D点云数据进行配准，计算物体6自由度的位姿

一. 开发环境

（1）visual studio 2022

（2）OpenCV 4.8.0

（3）libtorch 1.13.1

（4）PCL 1.13.0

二. 模板点云数据扣取

使用 CloudCompare 工具扣取模板点云，本课题以“锤子”为例，扣取“锤子”的点云数据保存为“target.txt”。扣取前如图2.1所示，扣取后点云数据如图2.2所示。


                                                                    图2.1 原始点云数据

                                                                      图2.2 扣取的模板点云数据

三. 输入点云数据预处理

1. 基于YOLO获取目标所在矩形框

使用目标检测算法YOLO获取目标（“锤子”）的矩形框，目标检测结果如图 3.1 所示。


                                                                            图3.1 锤子目标检测结果

2. 扣取输入目标点云数据

基于YOLO的检测结果矩形框，扣取“锤子”的点云数据，该步骤代码如下：

// -------------------- source -----------------------------------------------------
    std::string source_img_path = "./data/RGB/118.bmp";
    std::string source_ply_path = "./data/pointCloud/118.ply";
    cv::Mat img_source = cv::imread(source_img_path, -1);
    std::vector<std::vector<Detection>> result_template = m_yolo.predect(img_source);
    cv::Rect m_box;
    bool isExist = false;
    for (int i = 0; i < result_template.size(); i++)
    {
        std::vector<Detection> res = result_template[i];
        for (int j = 0; j < res.size(); j++)
        {
            Detection det = res[j];
            int res_class = det.class_idx;
            if (0 == res_class)
            {
                m_box = det.bbox;
                isExist = true;
                cv::rectangle(img_source, m_box, cv::Scalar(255, 0, 0));
            }
        }
    }
    if (false == isExist)
    {
        std::cerr << "the hammer is not exixt" << std::endl;
        return 0;
    }
    std::cout << "source box: " << m_box << std::endl;
    int x_min = m_box.x;
    int y_min = m_box.y;
    int x_max = m_box.width + x_min;
    int y_max = m_box.height + y_min;

    // PCL
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPLYFile(source_ply_path, *cloud_source);

    // cut
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cut(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = y_min; i < y_max; i++)
    {
        for (int j = x_min; j < x_max; j++)
        {
            if (x_min < j && j < x_max && y_min < i && i < y_max)
            {
                int current_i = i * img_source.cols + j;
                if (600 < cloud_source->points[current_i].z && cloud_source->points[current_i].z < 700)
                    source_cut->push_back(cloud_source->points[current_i]);
            }
        }
    }

扣取的点云数据如图3.2所示，其中红色点云为输入源扣取的数据，蓝色点云为模板点云数据。


                                             图3.2 输入源和模板点云图

四. 点云配准

基于点云配准算法GICP, 对输入源与模板进行配准计算，相关代码如下：

// -------------------配准----------------------------------------------------
    // === gicp
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
    gicp.setInputSource(source_cut);            // 源点云
    gicp.setInputTarget(p_target);            // 目标点云
    gicp.setTransformationEpsilon(1e-5);   // 为终止条件设置最小转换差异
    gicp.setMaxCorrespondenceDistance(100);  // 设置对应点对之间的最大距离（此值对配准结果影响较大）。
    gicp.setEuclideanFitnessEpsilon(0.0001);  // 设置收敛条件是均方误差和小于阈值，停止迭代；
    gicp.setMaximumIterations(1000);           // 最大迭代次数
    gicp.setUseReciprocalCorrespondences(true);//设置为true,则使用相互对应关系

    pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
    gicp.align(aligned_cloud);

    Eigen::Matrix4f final_transformation = gicp.getFinalTransformation();

五. 显示结果

通过上述步骤得到两组点云的六自由度位姿关系，基于该结果可以用于机械臂抓取，完整代码如下：

// Grasp3D.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include "YOLOV5Handle.h"

#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
//#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/registration/gicp.h>

#include <boost/thread/thread.hpp>

#include <pcl/registration/icp.h>

typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> PCLHandler;

//-------------------------------从txt文件中读取三维坐标-------------------------------------
void CreateCloudFromTxt(const std::string file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    std::ifstream file(file_path.c_str());
    std::string line;
    pcl::PointXYZ point;
    while (getline(file, line)) {
        std::stringstream ss(line);
        ss >> point.x;
        ss >> point.y;
        ss >> point.z;
        cloud->push_back(point);
    }
    file.close();
}

// YOLO
YOLOv5Handle m_yolo("./models/YOLOv5.torchscript");

void mPreprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_target)
{
    // -------------------预处理------------------------------------------------------
    // Preprocess the cloud by...
    // ... and downsampling the point cloud 降采样点云, 减少计算量
    // 定义体素大小 3mm
    std::cout << "downsampling" << std::endl;
    const float voxel_grid_size = 3.0f;
    pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
    vox_grid.setInputCloud(cloud_target);
    // 设置叶子节点的大小lx, ly, lz
    vox_grid.setLeafSize(voxel_grid_size, voxel_grid_size, voxel_grid_size);
    //vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html
    pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud1(new pcl::PointCloud<pcl::PointXYZ>);
    vox_grid.filter(*tempCloud1);
    cloud_target = tempCloud1;
}

int main()
{
    // ---------------------------- target ------------------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr p_target(new pcl::PointCloud<pcl::PointXYZ>);
    CreateCloudFromTxt("./models/model.txt", p_target);
    mPreprocess(p_target);
    
    // -------------------- source -----------------------------------------------------
    std::string source_img_path = "./data/RGB/118.bmp";
    std::string source_ply_path = "./data/pointCloud/118.ply";
    cv::Mat img_source = cv::imread(source_img_path, -1);
    std::vector<std::vector<Detection>> result_template = m_yolo.predect(img_source);
    cv::Rect m_box;
    bool isExist = false;
    for (int i = 0; i < result_template.size(); i++)
    {
        std::vector<Detection> res = result_template[i];
        for (int j = 0; j < res.size(); j++)
        {
            Detection det = res[j];
            int res_class = det.class_idx;
            if (0 == res_class)
            {
                m_box = det.bbox;
                isExist = true;
                cv::rectangle(img_source, m_box, cv::Scalar(255, 0, 0));
            }
        }
    }
    if (false == isExist)
    {
        std::cerr << "the hammer is not exixt" << std::endl;
        return 0;
    }
    std::cout << "source box: " << m_box << std::endl;
    int x_min = m_box.x;
    int y_min = m_box.y;
    int x_max = m_box.width + x_min;
    int y_max = m_box.height + y_min;

    // PCL
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPLYFile(source_ply_path, *cloud_source);

    // cut
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cut(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = y_min; i < y_max; i++)
    {
        for (int j = x_min; j < x_max; j++)
        {
            if (x_min < j && j < x_max && y_min < i && i < y_max)
            {
                int current_i = i * img_source.cols + j;
                if (600 < cloud_source->points[current_i].z && cloud_source->points[current_i].z < 700)
                    source_cut->push_back(cloud_source->points[current_i]);
            }
        }
    }
    mPreprocess(source_cut);

    // -------------------配准----------------------------------------------------
    // === gicp
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
    gicp.setInputSource(source_cut);            // 源点云
    gicp.setInputTarget(p_target);            // 目标点云
    gicp.setTransformationEpsilon(1e-5);   // 为终止条件设置最小转换差异
    gicp.setMaxCorrespondenceDistance(100);  // 设置对应点对之间的最大距离（此值对配准结果影响较大）。
    gicp.setEuclideanFitnessEpsilon(0.0001);  // 设置收敛条件是均方误差和小于阈值，停止迭代；
    gicp.setMaximumIterations(1000);           // 最大迭代次数
    gicp.setUseReciprocalCorrespondences(true);//设置为true,则使用相互对应关系

    pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
    gicp.align(aligned_cloud);

    Eigen::Matrix4f final_transformation = gicp.getFinalTransformation();

    // === result 
    std::cout << "Transformation matrix:" << std::endl << final_transformation << std::endl;
    // Print the rotation matrix and translation vector
    Eigen::Matrix3f rotation = final_transformation.block<3, 3>(0, 0);
    Eigen::Vector3f translation = final_transformation.block<3, 1>(0, 3);

    Eigen::Vector3f euler_angles = rotation.eulerAngles(2, 1, 0) * 180 / M_PI;

    printf("\n");
    printf("    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2));
    printf("R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2));
    printf("    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2));
    printf("\n");
    cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl;
    printf("\n");
    printf("t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2));

    // Save the aligned template for visualization
    //pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
    // 将模板中保存的点云图进行旋转矩阵变换，把变换结果保存到transformed_cloud
    //pcl::transformPointCloud(*source_cut, transformed_cloud, final_transformation);
    
    // ---------------------------- view_0 -------------------------------------------------------
    pcl::visualization::PCLVisualizer viewer0("viewer0");
    // 设置坐标系系统
    viewer0.addCoordinateSystem(0.5, "cloud", 0);
    // 设置背景色
    viewer0.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey

    // 1. target 
    PCLHandler template_cloud_handler(p_target, 100, 255, 255);
    viewer0.addPointCloud(p_target, template_cloud_handler, "target_cloud");
    // 设置渲染属性（点大小）
    viewer0.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target_cloud");

    // 2.  裁剪后
    PCLHandler source_cut_handler(source_cut, 255, 100, 100);
    viewer0.addPointCloud(source_cut, source_cut_handler, "source_cut");
    // 设置渲染属性（点大小）
    viewer0.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source_cut");

    // ---------------------------- view -------------------------------------------------------
    pcl::visualization::PCLVisualizer viewer("viewer");
    // 设置坐标系系统
    viewer.addCoordinateSystem(0.5, "cloud", 0);
    // 设置背景色
    viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey

    // 4. target 
    //PCLHandler template_cloud_handler(p_target, 100, 255, 255);
    viewer.addPointCloud(p_target, template_cloud_handler, "target_cloud");
    // 设置渲染属性（点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target_cloud");

    // 5.  旋转后的点云rotated
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(&aligned_cloud);
    PCLHandler target_cloud_handler(t_cloud, 255, 100, 100);
    viewer.addPointCloud(t_cloud, target_cloud_handler, "rotated_cloud");
    // 设置渲染属性（点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "rotated_cloud");

    // ==== 
    cv::namedWindow("img",0);
    cv::imshow("img", img_source);
    while (!viewer0.wasStopped() && !viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
        viewer0.spinOnce();
        viewer.spinOnce();
    }

    cv::destroyAllWindows();
    
    return (0);
}

计算结果如图5.1所示


                                                                            图5.1 六自由度位姿关系计算结果

版权声明：本文为V社区用户原创内容，转载时必须标注文章的来源（V社区），文章链接、文章作者等基本信息，否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件至：v-club@hikrobotics.com 进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容。
点赞 4
收藏 4
上一篇

“悉灵杯”课题研究-基于YOLO和GGCNN的物品平面抓取方案研究

下一篇

“悉灵杯”课题研究报告-基于RGB-D相机的2D和3D抓取定位方案研究

评论请先登录 登录
全部评论 0
Lv.0
Vuser_193018
0
关注
8
粉丝
3
创作
10
获赞
所属专题
“悉灵杯”3D课题研究活动成果
39篇
  • 使用3D相机MV-EB435i基于OpenCV的客流检测与异常识别的实现
  • 悉灵杯”课题研究进展（一）通过海康RGB-D相机获取带RGB信息的点云
  • “悉灵杯”课题研究-基于RGB_D相机的物品位置计算
  • “悉灵杯”课题研究-对RGB_D相机采集的三维点云处理
  • “悉灵杯”课题研究-RGB_D相机SDK集成及Open3d点云基本处理
  • “悉灵杯”课题研究-集成RGB_D相机SDK的Open3d点云可视化
  • “悉灵杯”课题研究-集成RGB_D相机SDK的Open3d点云功能UI项目开发（项目demo）
  • “悉灵杯”课题研究-RGB_D相机SDK三维点云存储
  • “悉灵杯”课题研究-OpenNI集成及示例代码开发
  • 悉灵杯”课题研究-MV-EB435i立体相机基于opencv图像处理使用yolov5的物体识别
  • “悉灵杯”课题研究-基于opecv的集成RGB_D相机SDK的基础小样物品颜色检测及人脸识别
  • OpenCV中利用knn进行数字(0-9)识别--RGB-D相机采集
  • “悉灵杯”课题研究-基于MV-EB435i的落差边缘检测算法开发记录
  • 悉灵杯”课题研究-LabVIEW集成及示例代码开发
  • “悉灵杯”课题研究-MV-EB435i立体相机集成Apriltags发布相机位姿
  • “悉灵杯”课题研究-MV-EB435i立体相机图像处理UI界面开发
  • “悉灵杯”课题研究-基于ROS1的RGB-D相机SDK集成及示例代码开发
  • 第二届“悉灵杯”课题移动机器人感知研究进展
  • “悉灵杯”课题研究—手眼标定方案
  • 第二届“悉灵杯”课题研究-基于RGB_D相机的室内环境建模
  • 悉灵杯”课题研究进展（二）-基于三维模型/场景点云的虚拟仿真数据生成
  • 悉灵杯”课题研究进展（一）-实例分割网络模型搭建与实验场景介绍
  • “悉灵杯”课题研究报告-基于RGB-D相机的2D和3D抓取定位方案研究
  • “悉灵杯”课题研究-基于点云配准算法GICP的3D抓取方案研究
  • “悉灵杯”课题研究-基于YOLO和GGCNN的物品平面抓取方案研究
  • 动态手势控制音频播放器-成果物
  • 第二届“悉灵杯”课题研究报告-动态手势控制音频播放器设计
  • 动态手势控制音频播放器（五）动态手势控制音频播放器exe
  • 动态手势控制音频播放器（四）动态手势识别系统的设计
  • 动态手势控制音频播放器（三）音频播放器设计
  • 动态手势控制音频播放器（二）动态手势识别系统的设计
  • 动态手势控制音频播放器（一）总体方案设计
  • 悉灵杯”课题研究进展（四）RGB-D相机引导机械臂分拣物料
  • 悉灵杯”课题研究进展（三）RGB-D相机引导机械臂分拣物料
  • 悉灵杯”课题研究进展（二）RGB-D相机引导机械臂分拣物料
  • ”悉灵杯”课题研究报告-基于RGB-D相机的机械臂物料分拣系统研究
  • 悉灵杯”课题研究报告-基于深度学习方法和虚拟仿真数据的机械臂引导方案
  • 第二届“悉灵杯”课题研究机械臂引导研究报告
  • 第二届“悉灵杯”课题研究机械臂引导研究进展（二）
  • 第二届“悉灵杯”课题研究机械臂引导研究进展（一）
相关阅读
  • 海康移动机器人技术资料目录
    2024-09-21 浏览 0
  • VM算法平台-PLCS7通讯-二维码字符串接收及发送至S7的string类型相关问题-二维码命名存图
    2024-09-13 浏览 0
  • usb共享网络更改路由
    2024-09-24 浏览 0
  • 【MVS4.0新功能一览】第三篇采集卡相关功能
    2024-08-29 浏览 0
©️2024杭州海康机器人股份有限公司 版权所有
服务协议 | 隐私政策 | 社区管理规范

请升级浏览器版本

您正在使用的浏览器版本过低，请升级最新版本以获得更好的体验。

推荐使用以下浏览器

Chrome

Edge

Firefox