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。

相关文章

Python xmltodict实现简化XML数据处理

Python xmltodict实现简化XML数据处理

《Pythonxmltodict实现简化XML数据处理》Python社区为提供了xmltodict库,它专为简化XML与Python数据结构的转换而设计,本文主要来为大家介绍一下如何使用xmltod... 目录一、引言二、XMLtodict介绍设计理念适用场景三、功能参数与属性1、parse函数2、unpa
阅读更多...
C#实现获得某个枚举的所有名称

C#实现获得某个枚举的所有名称

《C#实现获得某个枚举的所有名称》这篇文章主要为大家详细介绍了C#如何实现获得某个枚举的所有名称,文中的示例代码讲解详细,具有一定的借鉴价值,有需要的小伙伴可以参考一下... C#中获得某个枚举的所有名称using System;using System.Collections.Generic;usi
阅读更多...
Go语言实现将中文转化为拼音功能

Go语言实现将中文转化为拼音功能

《Go语言实现将中文转化为拼音功能》这篇文章主要为大家详细介绍了Go语言中如何实现将中文转化为拼音功能,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 有这么一个需求：新用户入职 创建一系列账号比较麻烦，打算通过接口传入姓名进行初始化。想把姓名转化成拼音。因为有些账号即需要中文也需要英
阅读更多...
C# 读写ini文件操作实现

C# 读写ini文件操作实现

《C#读写ini文件操作实现》本文主要介绍了C#读写ini文件操作实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习吧... 目录一、INI文件结构二、读取INI文件中的数据在C#应用程序中，常将INI文件作为配置文件，用于存储应用程序的
阅读更多...
C#实现获取电脑中的端口号和硬件信息

C#实现获取电脑中的端口号和硬件信息

《C#实现获取电脑中的端口号和硬件信息》这篇文章主要为大家详细介绍了C#实现获取电脑中的端口号和硬件信息的相关方法,文中的示例代码讲解详细,有需要的小伙伴可以参考一下... 我们经常在使用一个串口软件的时候，发现软件中的端口号并不是普通的COM1，而是带有硬件信息的。那么如果我们使用C#编写软件时候，如
阅读更多...
Python使用qrcode库实现生成二维码的操作指南

Python使用qrcode库实现生成二维码的操作指南

《Python使用qrcode库实现生成二维码的操作指南》二维码是一种广泛使用的二维条码,因其高效的数据存储能力和易于扫描的特点,广泛应用于支付、身份验证、营销推广等领域,Pythonqrcode库是... 目录一、安装 python qrcode 库二、基本使用方法1. 生成简单二维码2. 生成带 Log
阅读更多...
Go语言使用Buffer实现高性能处理字节和字符

Go语言使用Buffer实现高性能处理字节和字符

《Go语言使用Buffer实现高性能处理字节和字符》在Go中,bytes.Buffer是一个非常高效的类型,用于处理字节数据的读写操作,本文将详细介绍一下如何使用Buffer实现高性能处理字节和... 目录1. bytes.Buffer 的基本用法1.1. 创建和初始化 Buffer1.2. 使用 Writ
阅读更多...
基于WinForm+Halcon实现图像缩放与交互功能

基于WinForm+Halcon实现图像缩放与交互功能

《基于WinForm+Halcon实现图像缩放与交互功能》本文主要讲述在WinForm中结合Halcon实现图像缩放、平移及实时显示灰度值等交互功能,包括初始化窗口的不同方式,以及通过特定事件添加相应... 目录前言初始化窗口添加图像缩放功能添加图像平移功能添加实时显示灰度值功能示例代码总结最后前言本文将
阅读更多...
Redis延迟队列的实现示例

Redis延迟队列的实现示例

《Redis延迟队列的实现示例》Redis延迟队列是一种使用Redis实现的消息队列,本文主要介绍了Redis延迟队列的实现示例,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习... 目录一、什么是 Redis 延迟队列二、实现原理三、Java 代码示例四、注意事项五、使用 Redi
阅读更多...
C#实现WinForm控件焦点的获取与失去

C#实现WinForm控件焦点的获取与失去

《C#实现WinForm控件焦点的获取与失去》在一个数据输入表单中,当用户从一个文本框切换到另一个文本框时,需要准确地判断焦点的转移,以便进行数据验证、提示信息显示等操作,本文将探讨Winform控件... 目录前言获取焦点改变TabIndex属性值调用Focus方法失去焦点总结最后前言在一个数据输入表单
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2