博客

代码展示:

#include <string>
#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc

using namespace std;

using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;

bool next_iteration = false;


void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing) {
    if (event.getKeySym() == "space" && event.keyDown())
        next_iteration = true;
}

int main() {
    // 声明需要用到的点云（读入的，转换的，ICP调整的三个点云）
    PointCloudT::Ptr cloud_in(new PointCloudT(10, 1));  // 原始点云
    PointCloudT::Ptr cloud_tr(new PointCloudT);  // 转换后的点云
    PointCloudT::Ptr cloud_icp(new PointCloudT);  // ICP输出后的点云
    int iterations = 0;  // 缺省的迭代次数
    // 随机填充点云
    for (auto &point : *cloud_in) {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    // 定义旋转平移的转换矩阵
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
    double theta = M_PI / 4;  // 旋转角度(弧度)
    transformation_matrix(0, 0) = cos(theta);
    transformation_matrix(0, 1) = -sin(theta);
    transformation_matrix(1, 0) = sin(theta);
    transformation_matrix(1, 1) = cos(theta);
    // Z轴平移0.4米
    transformation_matrix(2, 3) = 0.4;
    //打印出旋转矩阵R和平移T
    cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
    //转移后
    pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
    *cloud_tr = *cloud_icp;  //
    // ICP算法调用
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setMaximumIterations(iterations);
    icp.setInputSource(cloud_icp);
    icp.setInputTarget(cloud_in);
    icp.align(*cloud_icp);
    icp.setMaximumIterations(1);

    if (icp.hasConverged()) {
        cout << "ICP已经收敛, 分数为:  " << icp.getFitnessScore() << endl;
        transformation_matrix = icp.getFinalTransformation().cast<double>();
        cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
    } else {
        PCL_ERROR("ICP没有收敛.");
        system("pause");
        return (-1);
    }
    // 可视化
    pcl::visualization::PCLVisualizer viewer("viewer");
    int v1(0);
    int v2(1);
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    // 原始点云为白色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, 255, 255, 255);
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
    // 转换后的点云为绿色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 0, 255, 0);
    viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
    // ICP得到的点云为红色
    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 255, 0, 0);
    viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
    // 在每个视口中添加说明文字
    viewer.addText("white: cloud_raw \n green: cloud_tr", 10, 10, "icp_info_1", v1);
    viewer.addText("white: cloud_raw \n red: cloud_ICP", 10, 10, "icp_info_2", v2);
    // 设置点云的显示大小
    int pointSize(10);
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_in_v1");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_in_v2");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_tr_v1");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_icp_v2");

    stringstream ss;
    ss << iterations;
    string iterations_cnt = "ICP iterations = " + ss.str();
    viewer.addText(iterations_cnt, 10, 60, "iterations_cnt", v2);
    // 设置背景颜色
    viewer.setBackgroundColor(0.1, 0.1, 0.1, v1);
    viewer.setBackgroundColor(0.1, 0.1, 0.1, v2);
    // 设置相机的位置
    viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
    viewer.setSize(1280, 1024);
    // 注册回调函数
    viewer.registerKeyboardCallback(&keyboardEventOccurred, (void *) NULL);
    while (!viewer.wasStopped()) {
        viewer.spinOnce();
        // 按空格建进入下一次配准 :
        if (next_iteration) {
            icp.align(*cloud_icp);
            if (icp.hasConverged()) {
                cout << "ICP已经收敛, 分数为:  " << icp.getFitnessScore() << endl;
                cout << "\nICP从cloud_icp到cloud_in迭代次数:  " << ++iterations << endl;
                transformation_matrix *= icp.getFinalTransformation().cast<double>();
                cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
                ss.str("");
                ss << iterations;
                iterations_cnt = "ICP iterations = " + ss.str();
                viewer.updateText(iterations_cnt, 10, 60, "iterations_cnt");
                viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
            } else {
                PCL_ERROR("ICP没有收敛.");
                system("pause");
                return (-1);
            }
        }
        next_iteration = false;
    }
    system("pause");
    return (0);
}

效果展示: