“悉灵杯”课题研究-基于点云配准算法GICP的3D抓取方案研究
基于点云配准算法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 进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容。
上一篇

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

下一篇

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

评论请先登录 登录
全部评论 0
Lv.0
0
关注
7
粉丝
3
创作
9
获赞
所属专题
  • 使用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相机的机械臂物料分拣系统研究
  • 悉灵杯”课题研究报告-基于深度学习方法和虚拟仿真数据的机械臂引导方案
  • 第二届“悉灵杯”课题研究机械臂引导研究报告
  • 第二届“悉灵杯”课题研究机械臂引导研究进展(二)
  • 第二届“悉灵杯”课题研究机械臂引导研究进展(一)
相关阅读
  • VM算法圆查找工具与基于opencv开发的圆查找对比
    2024-01-31 浏览 0
  • VM和VisionPro使用感受
    2024-02-22 浏览 0
  • ClientView对接cms配置
    2024-01-26 浏览 0
  • 【0402059】RCS低代码组件安装部署操作手册
    2024-01-31 浏览 0
  • 海康机器人软件工具目录
    2024-01-25 浏览 0

请升级浏览器版本

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

推荐使用以下浏览器