PCL中的ICP

深藏阁楼爱情的钟 2022-03-08 09:29 226阅读 0赞

### [ICP（迭代最近点）算法][ICP]：[https://www.cnblogs.com/21207-iHome/p/6038853.html][ICP] ###

#  ICP的简单应用程序 #

#include <iostream>
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    #include <pcl/registration/icp.h>
    
    int main(int argc, char** argv)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
    
        // Fill in the CloudIn data
        cloud_in->width = 5;
        cloud_in->height = 1;
        cloud_in->is_dense = false;
        cloud_in->points.resize(cloud_in->width * cloud_in->height);
        //构建点云A
        for (size_t i = 0; i < cloud_in->points.size(); ++i)
        {
            cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
            cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
            cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
        }
        std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
            << std::endl;
        for (size_t i = 0; i < cloud_in->points.size(); ++i) std::cout << "    " <<
            cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
            cloud_in->points[i].z << std::endl;
        //点云B在A的基础上稍作改动
        *cloud_out = *cloud_in;
        std::cout << "size:" << cloud_out->points.size() << std::endl;
        //准确来说只是在x周上动一动
        for (size_t i = 0; i < cloud_in->points.size(); ++i)
            cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
        std::cout << "Transformed " << cloud_in->points.size() << " data points:"
            << std::endl;
        for (size_t i = 0; i < cloud_out->points.size(); ++i)
            std::cout << "    " << cloud_out->points[i].x << " " <<
            cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
        //构建icp
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        //将A 和 B喂入 icp
        icp.setInputCloud(cloud_in);
        icp.setInputTarget(cloud_out);
        //这个Final 有啥用？
        pcl::PointCloud<pcl::PointXYZ> Final;
        icp.align(Final);
        //如果点云B 只是由 A进行一些简单的刚体变换得来的，icp.hasConverged()值1，如果存在形变则不为1
        std::cout << "has converged:" << icp.hasConverged() << " score: " <<
            icp.getFitnessScore() << std::endl;
        //输出刚体变换矩阵信息
        std::cout << icp.getFinalTransformation() << std::endl;
    
        return (0);
    }

# CMakeList.txt  #

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
    project(icp)
    
    find_package(PCL 1.2 REQUIRED)
    
    include_directories(${PCL_INCLUDE_DIRS})
    link_directories(${PCL_LIBRARY_DIRS})
    add_definitions(${PCL_DEFINITIONS})
    
    add_executable (icp src/icp.cpp)
    target_link_libraries (icp ${PCL_LIBRARIES})

# 运行结果  #

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70][]

[ICP]: https://www.cnblogs.com/21207-iHome/p/6038853.html
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]: /images/20220308/a0f5dd5ad3ab4a54a87b228e60dd53d1.png