ICP算法加速优化--多线程和GPU

LZ之前的文章ICP算法实现（C++）用C++实现了基础的ICP算法，由于该算法是一种迭代的优化算法，里面含有大量循环操作以及矩阵运算，可以通过使用多线程或者GPU硬件来进行加速，具体分别可以通过OpenMP和CUDA编程实现。这里给出的代码是根据github地址：https://github.com/alex-van-vliet/icp的代码改写的。原作者的代码质量还是不错的，有许多值得借签和学习的地方。但是考虑到使用的第三方库太多不便于配置和使用，LZ把这份代码重构了一下。原作者在代码里造了很多轮子，比如自己实现了Point3D、matrix以及vp-tree（也是一种搜索树，比PCL库中的kd-tree出现时间略晚）的数据结构，但是SVD分解还是调用了Eigen库（可能他也觉得底层实现太麻烦了吧~），LZ把这里面的矩阵结构统一用Eigen库实现了。另外去掉了一些不方便编译的第三方库，并简化了CmakeLists的内容以及程序的结构。如果只编译libcpu库只需依赖Eigen，编译libgpu库的话需要CUDA。

该工程的运行效果如下所示：
lib_icp.exe line1.pcd line2.pcd --cpu 100 1e-6 32 （CPU单核，0ms）
lib_icp.exe line1.pcd line2.pcd --cpu 100 1e-6 32 （CPU多核，2ms）
lib_icp.exe line1.pcd line2.pcd --gpu 100 1e-6 1024 （GPU，81ms）
lib_icp.exe bunny1.pcd bunny2.pcd --cpu 100 1e-6 32 （CPU单核，1407ms）
lib_icp.exe bunny1.pcd bunny2.pcd --cpu 100 1e-6 32 （CPU多核，246ms）
lib_icp.exe bunny1.pcd bunny2.pcd --gpu 100 1e-6 1024 （GPU，156ms）
lib_icp.exe horse1.pcd horse2.pcd --cpu 100 1e-6 32 （CPU单核，12585ms）
lib_icp.exe horse1.pcd horse2.pcd --cpu 100 1e-6 32 （CPU多核，1603ms）
lib_icp.exe horse1.pcd horse2.pcd --gpu 100 1e-6 1024 （GPU，363ms）
Release模式下编译，测试平台为Windows10系统，内存32G，CPU是i7-12700（20线程），GPU是NVIDIA GeForce RTX 3070 Laptop GPU（显存8G，每个block最大线程数为1024），计算的运行时间为三次求平均值。修改后的代码在linux系统也能编译，本人在WSL（Ubuntu20.04）的docker中测试过和Windows本地差别不大，但是调用CUDA版本代码计算结果有误。line1.pcd和line2.pcd点数为10，bunny1.pcd和bunny2.pcd点数为35947，horse1.pcd和horse2.pcd点数为193940；另外LZ调用PCL库计算这三组点云ICP配准耗时分别为0ms，758ms，30550ms。可以看出多核和GPU的加速作用随着点云点数增加优势还是非常明显的，但是点数非常少的话，GPU由于需要和CPU进行数据传输，此时运算速度会不太理想。

自己写了测试main函数，可以直接读取.pcd格式的点云文件（读取二进制储存的pcd文件更为高效，原作者代码是读.txt文件），并添加了时间计算部分和可视化部分，需要另外配置PCL库。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>#ifdef _WIN32#include <ctime>#include <pcl/visualization/pcl_visualizer.h>
#endif#ifdef __linux__#include <sys/time.h>
#endif#include "libcpu/icp.h"
#include "libgpu/icp.h"int main(int argc, char* argv[])
{std::cout << "\033[31mUsage:    .exe <source .pcd> <target .pcd> --cpu|gpu <iterations> <error> <capacity>\n"<< "for example: ./lib_icp.exe bunny1.pcd bunny2.pcd  --gpu 100 1e-6 1024\033[0m" << std::endl;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile(argv[1], *cloud_src);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile(argv[2], *cloud_tgt);bool use_gpu = (std::string(argv[3]) == "--gpu");if (use_gpu) std::cout << "use gpu" << std::endl;else  std::cout << "use cpu" << std::endl;size_t iterations = std::atoi(argv[4]);std::cout << "max iterations:" << iterations << std::endl;float error = std::atof(argv[5]);std::cout << "error:" << error << std::endl;int capacity = std::atoi(argv[6]);std::cout << "capacity:" << capacity << std::endl;std::vector<Eigen::Vector3f> p(cloud_src->size());std::vector<Eigen::Vector3f> q(cloud_tgt->size());for (size_t i = 0; i < cloud_src->size(); i++){p[i] = cloud_src->points[i].getVector3fMap();}for (size_t i = 0; i < cloud_tgt->size(); i++){q[i] = cloud_tgt->points[i].getVector3fMap();}#ifdef _WIN32clock_t start = clock();
#endif#ifdef __linux__struct timeval start, end;gettimeofday(&start, 0);
#endifauto [transformation, new_p] = (use_gpu ? libgpu::icp(q, p, iterations, error, capacity): libcpu::icp(q, p, iterations, error, capacity));#ifdef _WIN32clock_t end = clock();std::cout << "time cost:" << end - start << "ms" << std::endl;
#endif#ifdef __linux__gettimeofday(&end, 0);std::cout << "time cost:" << (1000000.0 * (end.tv_sec - start.tv_sec) + end.tv_usec - start.tv_usec) / 1000.0 << "ms" << std::endl;
#endifstd::cout << "transformation matrix: \n" << transformation << std::endl;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud_src, *cloud_icp, transformation);pcl::io::savePCDFile("cloud_icp.pcd", *cloud_icp);#ifdef _WIN32pcl::visualization::PCLVisualizer viewer("registration Viewer");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud_src, 0, 255, 0);  //原始点云绿色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(cloud_tgt, 255, 0, 0);  //目标点云红色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(cloud_icp, 0, 0, 255);    //匹配好的点云蓝色viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud(cloud_src, src_h, "source cloud");viewer.addPointCloud(cloud_tgt, tgt_h, "target cloud");viewer.addPointCloud(cloud_icp, final_h, "result cloud");while (!viewer.wasStopped()){viewer.spinOnce(100);}
#endifreturn 0;
}

工程结构如下：

运行结果截图：

附：调用PCL库ICP配准API的测试代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>int main(int argc, char* argv[])
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile(argv[1], *cloud_src);pcl::io::loadPCDFile(argv[2], *cloud_tgt);clock_t start = clock();pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_registration(new pcl::PointCloud<pcl::PointXYZ>);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);tree1->setInputCloud(cloud_src);tree2->setInputCloud(cloud_tgt);icp.setSearchMethodSource(tree1);icp.setSearchMethodTarget(tree2);icp.setInputSource(cloud_src);icp.setInputTarget(cloud_tgt);icp.setEuclideanFitnessEpsilon(1e-6);icp.setMaximumIterations(100);Eigen::Matrix4f transformation = Eigen::MatrixXf::Identity(4, 4);icp.align(*cloud_icp_registration, transformation);clock_t end = clock();std::cout << end - start << "ms" << std::endl;Eigen::Matrix4f transformation_icp = icp.getFinalTransformation();std::cout << transformation_icp << std::endl;std::cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;pcl::transformPointCloud(*cloud_src, *cloud_icp_registration, transformation_icp);pcl::io::savePCDFileBinary("icp.pcd", *cloud_icp_registration);return 0;
}

代码和数据集下载地址如下：
工程下载链接

ICP算法加速优化--多线程和GPU相关推荐

ICP算法实现（C++）
用C++实现的ICP(Iterative Closest Point,迭代最近点)算法,借助了PCL库实现点云基本变换.KD-tree以及可视化部分,核心迭代部分没有调用PCL的api.代码在KD-t ...
论文翻译—ICP算法论文（节选1~4章）
标题:A Method for Registration of 3-D Shapes 作者: J. Besl, Neil D. McKay 目录: Ⅰ Introduction Ⅱ Literatur ...
嵌入式算法移植优化学习笔记5——CPU,GPU,TPU,NPU都是什么
嵌入式算法移植优化学习笔记5--CPU,GPU,TPU,NPU都是什么一.什么是CPU? 二.什么是GPU? 三.什么是NPU? 四.什么是TPU? 附: 随着AI的广泛应用,深度学习已成为当前AI ...
关于实现Halcon算法加速的基础知识（多核并行/GPU）
一.提高Halcon的运算速度,有以下几种方法: 1.Multithreading(多线程) 2.Automatic Parallelization(自动操作并行化) 3.Compute device ...
OpenCV算法加速（4）官方源码v4.5.5的默认并行和优化加速的编译选项是什么？请重点关注函数cv::getBuildInformation()的返回值
举例opencv v4.5.5版本源码,Windows x64,VS2019,CMake https://github.com/opencv/opencv/tree/4.5.5 https://sou ...
用进化算法来优化SVM的参数C和Gamma——利用SCOOP库进行分布式加速计算
该案例展示了如何利用SCOOP库进行分布式加速计算Geatpy进化算法程序, 本案例和soea_demo6类似,同样是用进化算法来优化SVM的参数C和Gamma, 不同的是,本案例选用更庞大的数据集, ...
如何用 GPU 硬件层加速优化 Android 系统的游戏流畅度
作为一款VR实时操作游戏App,我们需要根据重力感应系统,实时监控手机的角度,并渲染出相应位置的VR图像,因此在不同 Android 设备之间,由于使用的芯片组和不同架构的GPU,游戏性能会因此受到影 ...
如何用 GPU硬件层加速优化Android系统的游戏流畅度—应用性能管理
作为一款VR实时操作游戏App,我们需要根据重力感应系统,实时监控手机的角度,并渲染出相应位置的VR图像,因此在不同 Android 设备之间,由于使用的芯片组和不同架构的GPU,游戏性能会因此受到影 ...
OpenCV算法加速的一些学习总结
一.概述算法加速在实际软件层面应用来说大数据和复杂计算的过程中算法优化,指降低算法计算复杂度,设计新算法快速求解,比如Hungarian匹配算法.或牺牲一些内存,预计算一些重复计算的过程,减少程 ...

ICP算法加速优化--多线程和GPU

ICP算法加速优化--多线程和GPU相关推荐

最新文章

热门文章