CUDAPCL 计算点云与点云之间的距离

2024-09-01 23:28
文章标签 计算 距离 之间 点云 cudapcl

本文主要是介绍CUDAPCL 计算点云与点云之间的距离，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

  • 一、简介
  • 二、实现代码
  • 三、实现效果
  • 参考资料

一、简介

这里使用CUDA实现一种计算计算点云与点云之间的欧式距离，其思路很简单，就是计算每个点到另一个点集之间的最小距离，最终保存结果到一个数组中，通过这种方式可以快速的计算出点云与点云之间的距离，这里充分利用CUDA的并行机制，并使用色带显示距离的远近关系。

二、实现代码

ComputeDistances.cuh

#ifndef COMPUTE_DISTANCES_GPU_CUH
#define COMPUTE_DISTANCES_GPU_CUH#include <vector>
#include <math.h>
#include <Eigen/Dense>#include <cuda_runtime.h>// 主机端函数声明
void computeDisByGpu(std::vector<Eigen::Vector3f>& points1, std::vector<Eigen::Vector3f>& points2, std::vector<float>& dis1, std::vector<float>& dis2);int getCudaDeviceCount();#endif // COMPUTE_DISTANCES_GPU_CUH

CalculateDistances.cu

#include "ComputeDistances.cuh"#include <cuda_runtime.h>
#include <device_launch_parameters.h>// CPU和GPU端都可以使用
struct Point3f
{float x, y, z;// 构造函数__host__ __device__ Point3f() : x(0), y(0), z(0) {}__host__ __device__ Point3f(float px, float py, float pz) : x(px), y(py), z(pz) {}// 向量加法__host__ __device__ Point3f operator+(const Point3f& p) const {return Point3f(x + p.x, y + p.y, z + p.z);}// 向量减法__host__ __device__ Point3f operator-(const Point3f& p) const {return Point3f(x - p.x, y - p.y, z - p.z);}// 标量乘法__host__ __device__ Point3f operator*(float s) const {return Point3f(x * s, y * s, z * s);}// 向量叉乘__host__ __device__ Point3f Cross(const Point3f& p) const {return Point3f(y * p.z - z * p.y, z * p.x - x * p.z, x * p.y - y * p.x);}// 向量点乘__host__ __device__ float Dot(const Point3f& p) const {return (x * p.x + y * p.y + z * p.z);}// 向量模__host__ __device__ float Module() const {return sqrtf(x * x + y * y + z * z);}
};__global__ void calculateDistances(Point3f* points1, Point3f* points2,float* dis1, float* dis2, int num1, int num2)
{// 通过 blockIdx.x 和 threadIdx.x 计算当前线程的索引 idx// ，然后分别对 points1 和 points2 中的每个点，计算它们到另一个点云中所有点的最小距离int idx = blockIdx.x * blockDim.x + threadIdx.x;if (idx < num1){float minDistance = INFINITY;for (int i = 0; i < num2; ++i){float distance = (points1[idx] - points2[i]).Module();if (distance < minDistance) {minDistance = distance;}}dis1[idx] = minDistance;}if (idx < num2){float minDistance = INFINITY;for (int i = 0; i < num1; ++i){float distance = (points2[idx] - points1[i]).Module();if (distance < minDistance) {minDistance = distance;}}dis2[idx] = minDistance;}
}void computeDisByGpu(std::vector<Eigen::Vector3f>& points1, std::vector<Eigen::Vector3f>& points2, std::vector<float>& dis1, std::vector<float>& dis2)
{int maxNum = std::max(points1.size(), points2.size());int num1 = points1.size();int num2 = points2.size();Point3f* d_points1;Point3f* d_points2;float* d_dis1;float* d_dis2;// 分配GPU设备内存cudaMalloc(&d_points1, points1.size() * sizeof(Point3f));cudaMalloc(&d_points2, points2.size() * sizeof(Point3f));cudaMalloc(&d_dis1, points1.size() * sizeof(float));cudaMalloc(&d_dis2, points2.size() * sizeof(float));// 复制数据到设备cudaMemcpy(d_points1, points1.data(), points1.size() * sizeof(Point3f), cudaMemcpyHostToDevice);cudaMemcpy(d_points2, points2.data(), points2.size() * sizeof(Point3f), cudaMemcpyHostToDevice);dim3 blockSize(256);dim3 gridSize((maxNum + blockSize.x - 1) / blockSize.x);calculateDistances <<<gridSize, blockSize>>>(d_points1, d_points2, d_dis1, d_dis2, num1, num2);cudaDeviceSynchronize();dis1.resize(num1);dis2.resize(num2);cudaMemcpy(dis1.data(), d_dis1, num1 * sizeof(float), cudaMemcpyDeviceToHost);cudaMemcpy(dis2.data(), d_dis2, num2 * sizeof(float), cudaMemcpyDeviceToHost);cudaFree(d_points1);cudaFree(d_points2);cudaFree(d_dis1);cudaFree(d_dis2);
}int getCudaDeviceCount()
{int count;cudaGetDeviceCount(&count);return count;
}

ColorRamp.h

#ifndef ColorRamp_H
#define ColorRamp_H#include <array>
#include <vector>class ColorRamp
{
public:typedef std::array<unsigned char, 3> Color;		//typedef std::pair<double, Color> Step;			//ColorRamp(){m_steps.push_back(std::make_pair(0, Color{ 192, 192, 255 }));m_steps.push_back(std::make_pair(0.2, Color{ 0, 0, 255 }));m_steps.push_back(std::make_pair(0.4, Color{ 0, 255, 0 }));m_steps.push_back(std::make_pair(0.6, Color{ 255, 255, 0 }));m_steps.push_back(std::make_pair(0.8, Color{ 255, 0, 0 }));m_steps.push_back(std::make_pair(1.0, Color{ 128, 0, 0 }));}//Color get(double value) const{if (value < 0.0f) value = 0.0f;if (value > 1.0f) value = 1.0f;std::size_t idx = 0;while (m_steps[idx + 1].first < value)++idx;double v0 = m_steps[idx].first;double v1 = m_steps[idx + 1].first;const Color& c0 = m_steps[idx].second;const Color& c1 = m_steps[idx + 1].second;double ratio = (value - v0) / (v1 - v0);Color out;for (std::size_t i = 0; i < 3; ++i)out[i] = static_cast<unsigned char>((1 - ratio) * c0[i] + ratio * c1[i]);return out;}private:std::vector<Step> m_steps;
};#endif // ColorRamp_H

main.cpp

#include <vector>
#include <iostream>#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>#include <Eigen/Core>// 引入CUDA相关头文件
#include "ComputeDistances.cuh"
#include "ColorRamp.h"// 使用PCL读取点云并转换为Eigen::Vector3f的格式
void convertPCLToEigen(pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud, std::vector<Eigen::Vector3f>& eigen_points) {eigen_points.clear();for (const auto& point : pcl_cloud->points){eigen_points.emplace_back(point.x, point.y, point.z);}
}int main()
{// ---------------------------------------读取数据----------------------------------const std::string fileName1 = R"(C:\Users\23547\Desktop\TEST\data\DemoICPPointClouds\cloud_bin_0.pcd)";const std::string fileName2 = R"(C:\Users\23547\Desktop\TEST\data\DemoICPPointClouds\cloud_bin_2.pcd)";pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile(fileName1, *cloud1) == -1) {std::cerr << "Couldn't read file cloud1.pcd" << std::endl;return -1;}// 加载第二个点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile(fileName2, *cloud2) == -1) {std::cerr << "Couldn't read file cloud2.pcd" << std::endl;return -1;}// 将PCL点云转换为Eigen::Vector3f的格式std::vector<Eigen::Vector3f> points1, points2;convertPCLToEigen(cloud1, points1);convertPCLToEigen(cloud2, points2);// 存储结果的容器std::vector<float> dis1, dis2;// 调用CUDA函数进行距离计算computeDisByGpu(points1, points2, dis1, dis2);//---------------------------------------可视化----------------------------------float minVal = std::numeric_limits<float>::max();float maxVal = -std::numeric_limits<float>::min();for (float d : dis1){if (minVal > d) minVal = d;if (maxVal < d) maxVal = d;}for (float d : dis2){if (minVal > d) minVal = d;if (maxVal < d) maxVal = d;}ColorRamp cr;float range = maxVal - minVal;pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB1(new pcl::PointCloud<pcl::PointXYZRGB>);cloudRGB1->resize(dis1.size());for (int i = 0; i < dis1.size(); ++i){double value = (dis1[i] - minVal) / range;ColorRamp::Color color = cr.get(value);cloudRGB1->points[i].x = cloud1->points[i].x;cloudRGB1->points[i].y = cloud1->points[i].y;cloudRGB1->points[i].z = cloud1->points[i].z;cloudRGB1->points[i].r = color[0];cloudRGB1->points[i].g = color[1];cloudRGB1->points[i].b = color[2];}cloudRGB1->width = cloudRGB1->size();cloudRGB1->height = 1;cloudRGB1->is_dense = true;pcl::io::savePCDFileASCII("cloud1.pcd", *cloudRGB1);pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB2(new pcl::PointCloud<pcl::PointXYZRGB>);cloudRGB2->resize(dis2.size());for (int i = 0; i < dis2.size(); ++i){double value = (dis2[i] - minVal) / range;ColorRamp::Color color = cr.get(value);cloudRGB2->points[i].x = cloud2->points[i].x;cloudRGB2->points[i].y = cloud2->points[i].y;cloudRGB2->points[i].z = cloud2->points[i].z;cloudRGB2->points[i].r = color[0];cloudRGB2->points[i].g = color[1];cloudRGB2->points[i].b = color[2];}cloudRGB2->width = cloudRGB2->size();cloudRGB2->height = 1;cloudRGB2->is_dense = true;pcl::io::savePCDFileASCII("cloud2.pcd", *cloudRGB2);auto ShowPointCloud = [](pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2){boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Windows"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZRGB>(cloud1, "cloud1");viewer->addPointCloud<pcl::PointXYZRGB>(cloud2, "cloud2");viewer->spin();};ShowPointCloud(cloudRGB1, cloudRGB2);return 0;
}

三、实现效果

参考资料

[1]https://bigfish.blog.csdn.net/article/details/141615277?spm=1001.2014.3001.5502

这篇关于CUDAPCL 计算点云与点云之间的距离的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1128349。

相关文章

Java对象和JSON字符串之间的转换方法(全网最清晰)

Java对象和JSON字符串之间的转换方法(全网最清晰)

《Java对象和JSON字符串之间的转换方法(全网最清晰)》：本文主要介绍如何在Java中使用Jackson库将对象转换为JSON字符串,并提供了一个简单的工具类示例,该工具类支持基本的转换功能,... 目录前言1. 引入 Jackson 依赖2. 创建 jsON 工具类3. 使用示例转换 Java 对象为
阅读更多...
Python如何计算两个不同类型列表的相似度

Python如何计算两个不同类型列表的相似度

《Python如何计算两个不同类型列表的相似度》在编程中,经常需要比较两个列表的相似度,尤其是当这两个列表包含不同类型的元素时,下面小编就来讲讲如何使用Python计算两个不同类型列表的相似度吧... 目录摘要引言数字类型相似度欧几里得距离曼哈顿距离字符串类型相似度Levenshtein距离Jaccard相
阅读更多...
java父子线程之间实现共享传递数据

java父子线程之间实现共享传递数据

《java父子线程之间实现共享传递数据》本文介绍了Java中父子线程间共享传递数据的几种方法,包括ThreadLocal变量、并发集合和内存队列或消息队列,并提醒注意并发安全问题... 目录通过 ThreadLocal 变量共享数据通过并发集合共享数据通过内存队列或消息队列共享数据注意并发安全问题总结在 J
阅读更多...
Java文件与Base64之间的转化方式

Java文件与Base64之间的转化方式

《Java文件与Base64之间的转化方式》这篇文章介绍了如何使用Java将文件（如图片、视频）转换为Base64编码,以及如何将Base64编码转换回文件,通过提供具体的工具类实现,作者希望帮助读者... 目录Java文件与Base64之间的转化1、文件转Base64工具类2、Base64转文件工具类3、
阅读更多...
使用C#代码计算数学表达式实例

使用C#代码计算数学表达式实例

《使用C#代码计算数学表达式实例》这段文字主要讲述了如何使用C#语言来计算数学表达式,该程序通过使用Dictionary保存变量,定义了运算符优先级,并实现了EvaluateExpression方法来... 目录C#代码计算数学表达式该方法很长，因此我将分段描述下面的代码片段显示了下一步以下代码显示该方法如
阅读更多...
如何用Java结合经纬度位置计算目标点的日出日落时间详解

如何用Java结合经纬度位置计算目标点的日出日落时间详解

《如何用Java结合经纬度位置计算目标点的日出日落时间详解》这篇文章主详细讲解了如何基于目标点的经纬度计算日出日落时间,提供了在线API和Java库两种计算方法,并通过实际案例展示了其应用,需要的朋友... 目录前言一、应用示例1、天安门升旗时间2、湖南省日出日落信息二、Java日出日落计算1、在线API2
阅读更多...
day-51 合并零之间的节点

day-51 合并零之间的节点

思路 直接遍历链表即可，遇到val=0跳过，val非零则加在一起，最后返回即可 解题过程 返回链表可以有头结点，方便插入，返回head.next Code /*** Definition for singly-linked list.* public class ListNode {* int val;* ListNode next;* ListNode() {}*
阅读更多...
poj 1113 凸包+简单几何计算

poj 1113 凸包+简单几何计算

题意： 给N个平面上的点，现在要在离点外L米处建城墙，使得城墙把所有点都包含进去且城墙的长度最短。 解析： 韬哥出的某次训练赛上A出的第一道计算几何，算是大水题吧。 用convexhull算法把凸包求出来，然后加加减减就A了。 计算见下图： 好久没玩画图了啊好开心。 代码： #include <iostream>#include <cstdio>#inclu
阅读更多...
uva 1342 欧拉定理（计算几何模板）

uva 1342 欧拉定理（计算几何模板）

题意： 给几个点，把这几个点用直线连起来，求这些直线把平面分成了几个。 解析： 欧拉定理： 顶点数 + 面数 - 边数= 2。 代码： #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#inc
阅读更多...
uva 11178 计算集合模板题

uva 11178 计算集合模板题

题意： 求三角形行三个角三等分点射线交出的内三角形坐标。 代码： #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#include <stack>#include <vector>#include <
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2