JVM虚拟机的组成

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云数据函数

2.1.2 执行 NDT 算法配准函数

2.1.3 可视化配准结果函数

2.2完整代码

三、实现效果

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云数据函数

pcl::PointCloud<pcl::PointXYZ>::Ptr load_point_cloud(const std::string& file_path)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1)
    {
        PCL_ERROR("无法读取文件 %s\n", file_path.c_str());
        return nullptr;
    }
    std::cout << "从 " << file_path << " 读取了 " << cloud->size() << " 个点." << std::endl;
    return cloud;
}

2.1.2 执行 NDT 算法配准函数

void run_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned_cloud)
{
    // 初始化 NDT 对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setResolution(1.0);  // 设置 NDT 的分辨率
    ndt.setStepSize(0.1);  // 设置优化步长
    ndt.setTransformationEpsilon(1e-6);  // 设置终止条件
    ndt.setMaximumIterations(35);  // 设置最大迭代次数

    // 设置输入点云
    ndt.setInputSource(source);
    ndt.setInputTarget(target);

    // 执行 NDT 配准
    ndt.align(*aligned_cloud);
    final_transform = ndt.getFinalTransformation();  // 获取最终变换矩阵

    std::cout << "NDT 配准完成，最终得分: " << ndt.getFitnessScore() << std::endl;
}

2.1.3 可视化配准结果函数

void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("NDT 配准结果"));

    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    // 目标点云为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    // 源点云为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");

    // 配准后的点云为蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>    // NDT 头文件
#include <pcl/visualization/pcl_visualizer.h> // 可视化
#include <boost/thread/thread.hpp>

using namespace std;

// NDT配准函数
void ndt_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned_cloud)
{
    // 初始化NDT对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    // 设置NDT参数
    ndt.setResolution(1.0);                  // 设置分辨率
    ndt.setMaximumIterations(35);            // 设置最大迭代次数
    ndt.setTransformationEpsilon(1e-8);      // 为终止条件设置最小转换差异
    ndt.setStepSize(0.1);                    // 设置步长
    ndt.setInputSource(source_cloud);        // 设置输入源点云
    ndt.setInputTarget(target_cloud);        // 设置输入目标点云

    // 设置初始变换矩阵
    Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity();

    // 执行NDT配准
    ndt.align(*aligned_cloud, initial_guess);

    // 检查配准是否成功
    if (ndt.hasConverged()) {
        final_transform = ndt.getFinalTransformation();
        std::cout << "NDT配准成功，配准得分: " << ndt.getFitnessScore() << std::endl;
    }
    else {
        std::cerr << "NDT配准失败！" << std::endl;
    }
}

// 可视化函数
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    /*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

int main()
{
    // --------------------加载点云数据-----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);

    // 输出点云数据个数
    std::cout << "源点云点数: " << source->points.size() << std::endl;
    std::cout << "目标点云点数: " << target->points.size() << std::endl;

    // NDT配准
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f final_transform;
    ndt_registration(source, target, final_transform, aligned);

    // 输出变换矩阵
    std::cout << "最终变换矩阵:\n" << final_transform << std::endl;

    // 可视化配准结果
    visualize_registration(source, target, aligned);

    return 0;
}

三、实现效果

NDT配准成功，配准得分: 2.24138e-05
最终变换矩阵:
  0.923224   -0.37732   0.072716  0.0179218
  0.353408   0.908046   0.224846 -0.0605567
 -0.150868  -0.181885   0.971677  0.0367289
         0          0          0          1