一. 开发环境

（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 六自由度位姿关系计算结果