ICP算法实现（C++）

2024-03-11 02:38
文章标签 算法 c++ 实现 icp

本文主要是介绍ICP算法实现（C++），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

用C++实现的ICP（Iterative Closest Point，迭代最近点）算法，借助了PCL库实现点云基本变换、KD-tree以及可视化部分，核心迭代部分没有调用PCL的api。代码在KD-tree搜索部分采用了openmp加速优化，在适当的数据集下运行速度能超过PCL自带的ICP算法配准api。

ICP算法的原理和推导，网上有很多，在此不赘述。下面链接是我觉得讲的比较详细的，可以参考：
三维点云配准 – ICP 算法原理及推导
ICP算法公式推导和PCL源码解析

另外，本文的代码实现过程还参考了：
迭代最近点 ICP 详细推导（C++实现）（借助Eigen库）
C++实现ICP点云匹配（借助OpenCV库和Eigen库）

废话不多说，下面贴代码。

myicp.h

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp头文件
** @Ver : V1.0.0
*********************************************************************************/#ifndef MYICP_H_
#define MYICP_H_#include <iostream>
#include <fstream>
#include <ctime>
#include <cmath>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <omp.h>/*** @brief The MyICP class*/
class MyICP
{
public:MyICP();~MyICP();/*** @brief setSourceCloud 设置输入点云* @param cloud 输入点云*/void setSourceCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);/*** @brief setTargetCloud 设置目标点云* @param cloud 目标点云*/void setTargetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);/*** @brief setLeafSize 设置体素滤波网格尺寸* @param size 体素滤波网格尺寸*/void setLeafSize(float size);/*** @brief setMinError 设置最小误差* @param error 最小误差*/void setMinError(float error);/*** @brief setMaxIters 设置最大迭代次数* @param iters 最大迭代次数*/void setMaxIters(int iters);/*** @brief setEpsilon 设置配准误差* @param eps 配准误差*/void setEpsilon(float eps);/*** @brief downsample 下采样*/void downsample();/*** @brief registration 配准*/void registration();/*** @brief saveICPCloud 保存配准得到的点云* @param filename 点云文件名*/void saveICPCloud(const std::string filename);/*** @brief getTransformationMatrix 得到变换矩阵*/void getTransformationMatrix();/*** @brief getScore 得到配准得分*/void getScore();/*** @brief visualize 配准结果可视化（输入点云为绿色，目标点云为红色，配准点云为蓝色）*/void visualize();private:/*** @brief source_cloud 输入点云*/pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud;/*** @brief target_cloud 目标点云*/pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud;/*** @brief source_cloud_downsampled 输入点云下采样得到的点云*/pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_downsampled;/*** @brief target_cloud_downsampled 目标点云下采样得到的点云*/pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_downsampled;/*** @brief icp_cloud 配准得到的点云*/pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud;/*** @brief leaf_size 体素网格尺寸*/float leaf_size;/*** @brief min_error 最小误差*/float min_error;/*** @brief max_iters 最大迭代次数*/int max_iters;/*** @brief epsilon 配准误差*/float epsilon;/*** @brief transformation_matrix 变换矩阵*/Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
};#endif // MYICP_H

myicp_helpers.h

/********************************************************************************
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp_helpers头文件
** @Ver : V1.0.0
*********************************************************************************/#ifndef MYICP_HELPERS_H_
#define MYICP_HELPERS_H_#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> /*** @brief calNearestPointPairs 计算最邻近点对* @param H 变换矩阵* @param source_cloud 源点云* @param target_cloud 目标点云* @param target_cloud_mid 中间点云* @param kdtree kd树* @param error 误差*/
void calNearestPointPairs(Eigen::Matrix4f H, pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud_mid, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree, double &error);#endif // MYICP_HELPERS_H_

myicp.cpp

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp源文件
** @Ver : V1.0.0
*********************************************************************************/#include "myicp.h"
#include "myicp_helpers.h"MyICP::MyICP()
{}MyICP::~MyICP()
{}void MyICP::setSourceCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) 
{source_cloud = cloud;
}void MyICP::setTargetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{target_cloud = cloud;
}void MyICP::setLeafSize(float size)
{leaf_size = size;
}void MyICP::setMinError(float error)
{min_error = error;
}void MyICP::setMaxIters(int iters)
{max_iters = iters;
}void MyICP::setEpsilon(float eps)
{epsilon = eps;
}void MyICP::downsample()
{pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);voxel_grid.setInputCloud(source_cloud);source_cloud_downsampled.reset(new pcl::PointCloud<pcl::PointXYZ>);voxel_grid.filter(*source_cloud_downsampled);voxel_grid.setInputCloud(target_cloud);target_cloud_downsampled.reset(new pcl::PointCloud<pcl::PointXYZ>);voxel_grid.filter(*target_cloud_downsampled);std::cout << "down size *cloud_src_o from " << source_cloud->size() << " to " << source_cloud_downsampled->size() << endl;std::cout << "down size *cloud_tgt_o from " << target_cloud->size() << " to " << target_cloud_downsampled->size() << endl;
}void MyICP::registration()
{std::cout << "icp registration start..." << std::endl;Eigen::Matrix3f R_12 = Eigen::Matrix3f::Identity();Eigen::Vector3f T_12 = Eigen::Vector3f::Zero();Eigen::Matrix4f H_12 = Eigen::Matrix4f::Identity();pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>());//建立kd树pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);kdtree->setInputCloud(target_cloud_downsampled);double error = INT_MAX, score = INT_MAX;Eigen::Matrix4f H_final = H_12;int iters = 0;//开始迭代，直到满足条件while (error > min_error && iters < max_iters){iters++;double last_error = error;//计算最邻近点对calNearestPointPairs(H_12, source_cloud_downsampled, target_cloud_downsampled, target_cloud_mid, kdtree, error);if (last_error - error < epsilon)break;//计算点云中心坐标Eigen::Vector4f source_centroid, target_centroid_mid;pcl::compute3DCentroid(*source_cloud_downsampled, source_centroid);pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid);//去中心化Eigen::MatrixXf souce_cloud_demean, target_cloud_demean;pcl::demeanPointCloud(*source_cloud_downsampled, source_centroid, souce_cloud_demean);pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean);//计算W=q1*q2^TEigen::Matrix3f W = (souce_cloud_demean*target_cloud_demean.transpose()).topLeftCorner(3, 3);//SVD分解得到新的旋转矩阵和平移矩阵Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3f U = svd.matrixU();Eigen::Matrix3f V = svd.matrixV();if (U.determinant()*V.determinant() < 0){for (int x = 0; x < 3; ++x)V(x, 2) *= -1;}R_12 = V* U.transpose();T_12 = target_centroid_mid.head(3) - R_12*source_centroid.head(3);H_12 << R_12, T_12, 0, 0, 0, 1;H_final = H_12*H_final; //更新变换矩阵std::cout << "iters:"  << iters << "  "<< "error:" << error << std::endl;}transformation_matrix << H_final;
}void MyICP::saveICPCloud(const std::string filename)
{icp_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix); //点云变换pcl::io::savePCDFileBinary(filename, *icp_cloud);
}void MyICP::getTransformationMatrix()
{std::cout << "transformation_matrix:" << std::endl << transformation_matrix << std::endl;
}void MyICP::getScore()
{double fitness_score = 0.0;pcl::KdTreeFLANN <pcl::PointXYZ> kdtree;kdtree.setInputCloud(target_cloud);#pragma omp parallel for reduction(+:fitness_score) //采用openmmp加速for (int i = 0; i < icp_cloud->points.size(); ++i){std::vector<int> nn_indices(1);std::vector<float> nn_dists(1);kdtree.nearestKSearch(icp_cloud->points[i], 1, nn_indices, nn_dists);fitness_score += nn_dists[0];}std::cout << "score:" << std::endl << fitness_score / icp_cloud->points.size() << std::endl;
}void MyICP::visualize()
{pcl::visualization::PCLVisualizer viewer("registration Viewer");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色viewer.setBackgroundColor(255, 255, 255);viewer.addPointCloud(source_cloud, src_h, "source cloud");viewer.addPointCloud(target_cloud, tgt_h, "target cloud");viewer.addPointCloud(icp_cloud, final_h, "result cloud");while (!viewer.wasStopped()){viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}

myicp_helpers.cpp

/******************************************************************************** 
** @Copyright(c) $year$ $registered organization$ All Rights Reserved.
** @auth： taify
** @date： 2021/01/12
** @desc： myicp_helpers源文件
** @Ver : V1.0.0
*********************************************************************************/#include "myicp_helpers.h"void calNearestPointPairs(Eigen::Matrix4f H, pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud_mid, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree, double &error)
{double err = 0.0;pcl::transformPointCloud(*source_cloud, *source_cloud, H);std::vector<int>indexs(source_cloud->size());#pragma omp parallel for reduction(+:err) //采用openmmp加速for (int i = 0; i < source_cloud->size(); ++i){std::vector<int>index(1);std::vector<float>distance(1);kdtree->nearestKSearch(source_cloud->points[i], 1, index, distance);err = err + sqrt(distance[0]);indexs[i] = index[0];}pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid);error = err / source_cloud->size();
}

贴一张效果图：在这里插入图片描述
完整工程文件见我的GitHub.
由于代码写的比较仓促，不恰当的地方还请指教。
后续文章更新：
ICP算法的三种实现方法（SVD分解、四元数、BA）
ICP算法加速优化-多线程和GPU
点到面的ICP算法
KinectFusion中的ICP算法
只用Eigen库实现的ICP算法

这篇关于ICP算法实现（C++）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

原文地址:
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.chinasem.cn/article/796338。

相关文章

使用zip4j实现Java中的ZIP文件加密压缩的操作方法

使用zip4j实现Java中的ZIP文件加密压缩的操作方法

《使用zip4j实现Java中的ZIP文件加密压缩的操作方法》本文介绍如何通过Maven集成zip4j1.3.2库创建带密码保护的ZIP文件,涵盖依赖配置、代码示例及加密原理,确保数据安全性,感兴趣的... 目录1. zip4j库介绍和版本1.1 zip4j库概述1.2 zip4j的版本演变1.3 zip4
阅读更多...
python生成随机唯一id的几种实现方法

python生成随机唯一id的几种实现方法

《python生成随机唯一id的几种实现方法》在Python中生成随机唯一ID有多种方法,根据不同的需求场景可以选择最适合的方案,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来一起学习学习... 目录方法 1：使用 UUID 模块（推荐）方法 2：使用 Secrets 模块（安全敏感场景）方法
阅读更多...
C++中全局变量和局部变量的区别

C++中全局变量和局部变量的区别

《C++中全局变量和局部变量的区别》本文主要介绍了C++中全局变量和局部变量的区别,全局变量和局部变量在作用域和生命周期上有显著的区别,下面就来介绍一下,感兴趣的可以了解一下... 目录一、全局变量定义生命周期存储位置代码示例输出二、局部变量定义生命周期存储位置代码示例输出三、全局变量和局部变量的区别作用域
阅读更多...
C++中assign函数的使用

C++中assign函数的使用

《C++中assign函数的使用》在C++标准模板库中,std::list等容器都提供了assign成员函数,它比操作符更灵活,支持多种初始化方式,下面就来介绍一下assign的用法,具有一定的参考价... 目录​1.assign的基本功能​​语法​2. 具体用法示例​​​(1) 填充n个相同值​​(2)
阅读更多...
Spring StateMachine实现状态机使用示例详解

Spring StateMachine实现状态机使用示例详解

《SpringStateMachine实现状态机使用示例详解》本文介绍SpringStateMachine实现状态机的步骤,包括依赖导入、枚举定义、状态转移规则配置、上下文管理及服务调用示例,重点解... 目录什么是状态机使用示例什么是状态机状态机是计算机科学中的​​核心建模工具​​，用于描述对象在其生命
阅读更多...
Spring Boot 结合 WxJava 实现文章上传微信公众号草稿箱与群发

Spring Boot 结合 WxJava 实现文章上传微信公众号草稿箱与群发

《SpringBoot结合WxJava实现文章上传微信公众号草稿箱与群发》本文将详细介绍如何使用SpringBoot框架结合WxJava开发工具包,实现文章上传到微信公众号草稿箱以及群发功能,... 目录一、项目环境准备1.1 开发环境1.2 微信公众号准备二、Spring Boot 项目搭建2.1 创建
阅读更多...
IntelliJ IDEA2025创建SpringBoot项目的实现步骤

IntelliJ IDEA2025创建SpringBoot项目的实现步骤

《IntelliJIDEA2025创建SpringBoot项目的实现步骤》本文主要介绍了IntelliJIDEA2025创建SpringBoot项目的实现步骤,文中通过示例代码介绍的非常详细,对大家... 目录一、创建 Spring Boot 项目1. 新建项目2. 基础配置3. 选择依赖4. 生成项目5.
阅读更多...
Linux下删除乱码文件和目录的实现方式

Linux下删除乱码文件和目录的实现方式

《Linux下删除乱码文件和目录的实现方式》：本文主要介绍Linux下删除乱码文件和目录的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux下删除乱码文件和目录方法1方法2总结Linux下删除乱码文件和目录方法1使用ls -i命令找到文件或目录
阅读更多...
SpringBoot+EasyExcel实现自定义复杂样式导入导出

SpringBoot+EasyExcel实现自定义复杂样式导入导出

《SpringBoot+EasyExcel实现自定义复杂样式导入导出》这篇文章主要为大家详细介绍了SpringBoot如何结果EasyExcel实现自定义复杂样式导入导出功能,文中的示例代码讲解详细,... 目录安装处理自定义导出复杂场景1、列不固定，动态列2、动态下拉3、自定义锁定行/列，添加密码4、合并
阅读更多...
mybatis执行insert返回id实现详解

mybatis执行insert返回id实现详解

《mybatis执行insert返回id实现详解》MyBatis插入操作默认返回受影响行数,需通过useGeneratedKeys+keyProperty或selectKey获取主键ID,确保主键为自... 目录 两种方式获取自增 ID：1. ​​useGeneratedKeys+keyProperty（推
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2