ROS::点云PCL(1)kd-tree

最新推荐文章于 2023-04-03 16:18:40 发布
最新推荐文章于 2023-04-03 16:18:40 发布
阅读量628 收藏 12
点赞数
分类专栏： ROS 文章标签： ROS PCL 点云 3D 视觉
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_43928944/article/details/116129367
版权

理解

通过3D相机（雷达、激光扫描、立体相机）获取到的点云，一般数据量较大，分布不均匀，数据主要表征了目标物表面的大量点的集合，这些离散的点如果希望实现基于邻域关系的快速查找比对功能，就必须对这些离散的点之间建立拓扑关系。常见的空间索引一般是自上而下逐级划分空间的各种索引结构，包括BSP树，k-d tree、KDB tree、R tree、CELL tree、八叉树等。有了这些关系，我们就可以实现点云的降采样，计算特征向量，点云匹配，点云拆分等功能。

原理概述
k-d tree（ k-dimensional tree）是计算机科学中用于在k维空间中一些点建立关系的数据结构。它是一个包含特定约束的二叉搜索树。k-d tree对于范围搜索和最近邻居搜索非常有用。我们通常只处理三维空间的点云，因此我们所有的k-d树都是三维空间的。

k-d树的每个级别都使用垂直于相应轴的超平面沿特定维度拆分所有子级。在树的根部，所有子项都将根据第一维进行拆分（即，如果第一维坐标小于根，则它将位于左子树中，如果大于根，则显然位于右边的子树）。树中向下的每个级别都在下一个维度上划分，其他所有元素都用尽后，将返回到第一个维度。他们构建k-d树的最有效方法是使用一种分区方法，例如快速排序所用的一种方法，将中值点放置在根上，所有具有较小一维值的东西都放置在根部，而右侧则更大。然后，在左右两个子树上都重复此过程，直到要分区的最后一棵树仅由一个元素组成。

方式一

#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>

main (int argc, char **argv) 
{ 

    ros::init (argc, argv, "pcl_create"); 

    ros::NodeHandle nh; 
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);  
    
    // 用系统时间初始化随机种子
    srand(time(NULL));
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    sensor_msgs::PointCloud2 output; 

    int point_num;
    float table[8]={-2,-1.5,-1,-0.5,0.5,1,1.5,2};

    cloud->width = 512;
    cloud->height = 1; 
    cloud->points.resize(cloud->width * cloud->height); 

    for(int a=0;a<8;++a){
        float width = table[a];
        for(int i=0;i<8;++i){
	    float length = table[i];
  	    for(int c=0;c<8;++c){
		point_num = a*64+i*8+c;
		cloud->points[point_num].x = width; 
		cloud->points[point_num].y = length; 
		cloud->points[point_num].z = table[c];
		std::cout <<"x="<< cloud->points[point_num].x <<"   y="<< cloud->points[point_num].y <<"   z="<< cloud->points[point_num].z  << std::endl;
		if(width==-0.5 && length==-0.5 && table[c]==-0.5){
		    cloud->points[point_num].r=0;
		    cloud->points[point_num].g=255;
		    cloud->points[point_num].b=255;
		}else{
		   cloud->points[point_num].r=0;
		   cloud->points[point_num].g=0;
		   cloud->points[point_num].b=255;
   		}

	    }
	}
    }

    pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    kdtree.setInputCloud(cloud);

    pcl::PointXYZRGBA searchPoint;
    searchPoint.x = -0.5;
    searchPoint.y = -0.5;
    searchPoint.z = -0.5;
    // 方式一：搜索K个最近邻居

    // 创建K和两个向量来保存搜索到的数据
    // K = 10 表示搜索10个临近点
    // pointIdxNKNSearch        保存搜索到的临近点的索引
    // pointNKNSquaredDistance  保存对应临近点的距离的平方
    int K = 50;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);

    std::cout << "K nearest neighbor search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with K=" << K << std::endl;
   //参数依次为：需查询的点、领域个数、搜索完成对应点的索引、结果中每个点对应查询点的欧氏距离(欧几里德度量)
   if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
        for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){
			
			if(cloud->points[pointIdxNKNSearch[i]].x==-0.5 && cloud->points[pointIdxNKNSearch[i]].y==-0.5 && cloud->points[pointIdxNKNSearch[i]].z==-0.5){
			    cloud->points[pointIdxNKNSearch[i]].r=0;
			    cloud->points[pointIdxNKNSearch[i]].g=255;
			    cloud->points[pointIdxNKNSearch[i]].b=0; 
			}else{
			    cloud->points[pointIdxNKNSearch[i]].r=255;
			    cloud->points[pointIdxNKNSearch[i]].g=0;
			    cloud->points[pointIdxNKNSearch[i]].b=0; 
			}

            std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
                      << " " << cloud->points[pointIdxNKNSearch[i]].y
                      << " " << cloud->points[pointIdxNKNSearch[i]].z
                      << " (欧氏距离: " << pointNKNSquaredDistance[i] << ")" << std::endl;
		}
   }else{
       std::cout << "nothing......" << std::endl;
   }

    pcl::toROSMsg(*cloud, output); 
    output.header.frame_id = "odom"; 
    ros::Rate loop_rate(1); 

    while (ros::ok()) { 
	pcl_pub.publish(output);
	ros::spinOnce(); 
	loop_rate.sleep(); 
    } 
    return 0;
}

output

K nearest neighbor search at (-0.5 -0.5 -0.5) with K=10
    -0.5 -0.5 -0.5 (欧氏距离: 0)
    -0.5 -0.5 -1 (欧氏距离: 0.25)
    -0.5 -1 -0.5 (欧氏距离: 0.25)
    -1 -0.5 -0.5 (欧氏距离: 0.25)
    -1 -1 -0.5 (欧氏距离: 0.5)
    -0.5 -1 -1 (欧氏距离: 0.5)
    -1 -0.5 -1 (欧氏距离: 0.5)
    -1 -1 -1 (欧氏距离: 0.75)
    -0.5 -0.5 -1.5 (欧氏距离: 1)
    -0.5 -1.5 -0.5 (欧氏距离: 1)

在这里插入图片描述

欧氏距离

欧氏距离（EUCLIDEAN DISTANCE）
欧氏距离定义： 欧氏距离（ Euclidean distance）是一个通常采用的距离定义，它是在m维空间中两个点之间的真实距离。

在二维和三维空间中的欧式距离的就是两点之间的距离，二维的公式是
d = sqrt((x1-x2)+(y1-y2))
三维的公式是
d=sqrt(x1-x2)+(y1-y2)+(z1-z2)^)
推广到n维空间，欧式距离的公式是
d=sqrt( ∑(xi1-xi2)^ ) 这里i=1,2…n
xi1表示第一个点的第i维坐标,xi2表示第二个点的第i维坐标
n维欧氏空间是一个点集,它的每个点可以表示为(x(1),x(2),…x(n)),其中x(i)(i=1,2…n)是实数,称为x的第i个坐标,两个点x和y=(y(1),y(2)…y(n))之间的距离d(x,y)定义为上面的公式.

其实就是我们学的最简单的公式：a2+b2=c2,因此，两点距离其实就是：sqrt((x1-x2)+(y1-y2)
参考：http://www.blogjava.net/spec-second/archive/2008/08/17/222609.html

方式二

    pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    kdtree.setInputCloud(cloud);

    pcl::PointXYZRGBA searchPoint;
    searchPoint.x = -0.5;
    searchPoint.y = -0.5;
    searchPoint.z = -0.5;
    // 方式二：通过指定半径搜索

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    // 创建一个随机[0,256)的半径值
    float radius = 1.2;

    std::cout << "Neighbors within radius search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with radius=" << radius << std::endl;

    if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i){
			if(cloud->points[pointIdxRadiusSearch[i]].x==-0.5 && cloud->points[pointIdxRadiusSearch[i]].y==-0.5 && cloud->points[pointIdxRadiusSearch[i]].z==-0.5){
			    cloud->points[pointIdxRadiusSearch[i]].r=0;
			    cloud->points[pointIdxRadiusSearch[i]].g=255;
			    cloud->points[pointIdxRadiusSearch[i]].b=0; 
			}else{
			    cloud->points[pointIdxRadiusSearch[i]].r=255;
			    cloud->points[pointIdxRadiusSearch[i]].g=0;
			    cloud->points[pointIdxRadiusSearch[i]].b=0; 
			} 
            std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
                      << " " << cloud->points[pointIdxRadiusSearch[i]].y
                      << " " << cloud->points[pointIdxRadiusSearch[i]].z
                      << " (欧氏距离:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
		}
    }

Neighbors within radius search at (-0.5 -0.5 -0.5) with radius=1.2
    -0.5 -0.5 -0.5 (欧氏距离:: 0)
    -1 -0.5 -0.5 (欧氏距离:: 0.25)
    -0.5 -1 -0.5 (欧氏距离:: 0.25)
    -0.5 -0.5 -1 (欧氏距离:: 0.25)
    -1 -1 -0.5 (欧氏距离:: 0.5)
    -1 -0.5 -1 (欧氏距离:: 0.5)
    -0.5 -1 -1 (欧氏距离:: 0.5)
    -1 -1 -1 (欧氏距离:: 0.75)
    -1.5 -0.5 -0.5 (欧氏距离:: 1)
    -0.5 -1.5 -0.5 (欧氏距离:: 1)
    -0.5 -0.5 -1.5 (欧氏距离:: 1)
    -0.5 -0.5 0.5 (欧氏距离:: 1)
    -0.5 0.5 -0.5 (欧氏距离:: 1)
    0.5 -0.5 -0.5 (欧氏距离:: 1)
    -1.5 -1 -0.5 (欧氏距离:: 1.25)
    -1.5 -0.5 -1 (欧氏距离:: 1.25)
    -1 -1.5 -0.5 (欧氏距离:: 1.25)
    -1 -0.5 -1.5 (欧氏距离:: 1.25)
    -1 -0.5 0.5 (欧氏距离:: 1.25)
    -1 0.5 -0.5 (欧氏距离:: 1.25)
    -0.5 -1.5 -1 (欧氏距离:: 1.25)
    -0.5 -1 -1.5 (欧氏距离:: 1.25)
    -0.5 -1 0.5 (欧氏距离:: 1.25)
    -0.5 0.5 -1 (欧氏距离:: 1.25)
    0.5 -1 -0.5 (欧氏距离:: 1.25)
    0.5 -0.5 -1 (欧氏距离:: 1.25)

在这里插入图片描述

wx:pjcoder
关注
  • 0
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
Kd-Trees算法原理和源码
u011256637的专栏
09-27 1154
kd-tree（k-dimensional-tree）,是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索。下面介绍了如何使用kd-tree实现2维的范围搜索和最近邻搜索。kd-tree有很多的应用场景，例如网络设计、电路设计、数据库查询。在范围搜索和最近邻搜索中，普通的遍历搜索查询算法复杂度为O(N),而kd-tree的复杂度为O(logN)。
pcl_495:一个点云库 - 使用 Nodelets 的 ROS 应用程序
06-17
使用传感器和 PCL（点云库），将地板（或任何平行于相机框架的平面）与场景的其余部分分开 附加目标： 实现使用 ROS nodelet 的版本 在场景中查找对象 对一个对象执行额外的计算（比如找到对象的中心并随着时间的...
基于ROS实现的的激光点云处理，对地面进行拟合分割，采用欧几里得聚类实现障碍物检测。
09-12
基于ROS实现的的激光点云处理，对地面进行拟合分割，采用欧几里得聚类实现障碍物检测。基于ROS实现的的激光点云处理，对地面进行拟合分割，采用欧几里得聚类实现障碍物检测。基于ROS实现的的激光点云处理，对地面进行拟合分割，采用欧几里得聚类实现障碍物检测。
点云处理之KD-Tree
liusscsdn的博客
03-27 1175
例如，在物体识别中，可以将每个物体的特征向量表示为一个点云，然后利用KD-Tree搜索待识别物体的最近邻点云，从而实现物体识别。然后选择一个维度，计算该维度的中位数，并将该中位数作为节点的分割点。其实现方法为：首先选择一个维度，将所有数据按照这个维度的大小排序，然后选择中位数作为分割点，将数据集分为两个子集，小于等于分割点的放在左子树中，大于分割点的放在右子树中。KD-Tree是一种用于加速点云处理的数据结构，其原理是通过递归地分割空间，构建一颗二叉树，以实现高效的最近邻搜索、点云分割、三维重建等操作。
点云数据下的KD-tree
YUE__zy的博客
10-18 1702
kd-tree数据结构，构建方法，以及kd-tree在数据下的最近邻检索。
KD-Tree算法原理
baidu_15113429的博客
06-01 1607
上亿个数据，每个数据20个维度（也就是一个向量），给你一个数据如何快速找到和该数据最相似的前几个（kd-Tree思想）KD-Tree学习链接
KD TREE
weixin_36378508的博客
09-06 310
kd-tree（k-dimensional tree），一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。 主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。 BST是KD Tree在一维数据上的特例 KD Tree就是不停变换特征来建立BST。 构建流程 建立根节点； 选取方差最大的特征作为分割特征； 选择该特征的中位数作为分割点； 将数据集中该特征小于中位数的传递给根节点的左儿子，大于中位数的传递给根节点的右儿子； 递归执行步骤2-4，直到所有数据都被建立到KD.
PCL：KD树
qq_27193031的博客
02-25 622
KDtree一眼懂的使用方法
3D点云学习笔记（PCL安装及配置）
qq_39218707的博客
04-03 4247
PCL简介及安装配置vs2019
ros-melodic-rosbridge-server:ros-melodic-rosbridge-serverAUR软件包
03-22
ros-melodic-rosbridge-server:ros-melodic-rosbridge-serverAUR软件包
ros-melodic-desktop_full.20201012.0.0.2010230001.nupkg
04-03
这个包适用于 在win10上安装ROS系统（melodic版本），在使用chocolatey下载安装ros的时候，速度奇慢。可下载这个包替换。路径：C:\opt\chocolatey\lib\ros-melodic-desktop_full\ros-melodic-desktop_full.nupkg
ros-pcl-ml：用于点云处理的ros wiut pcl和用于对象识别的机器学习方法
02-14
ros-pcl-ml：用于点云处理的ros wiut pcl和用于对象识别的机器学习方法
importRosbag:导入rosbag数据-独立-无需安装ROS
05-25
导入rosbag数据-纯python-独立-无需安装ROS。 importRosbag函数导入一个.rosbag文件。 使用'filePathOrName'参数提供路径。 例如 from importRosbag.importRosbag import importRosbag topics = importRosbag...
ROS::导航参数配置详解
weixin_43928944的博客
08-16 8047
gmapping 实例说明 <launch> <!-- 雷达话题 --> <arg name="scan_topic" default="scan" /> <!-- 机器人基座坐标系 --> <arg name="base_frame" default="base_footprint"/> <!-- 里程计坐标系 --> <arg name="odom_frame" default="odom_c
Gazebo仿真Kinect相机内参获取，以及fx fy cx cy 的理解
weixin_43928944的博客
05-19 4719
sensor_msgs/CameraInfo Message File: sensor_msgs/CameraInfo.msg Raw Message Definition # This message defines meta information for a camera. It should be in a # camera namespace on topic "camera_info" and accompanied by up to five # image topics named: # .
ROS教程 Gazebo仿真(2)-激光雷达
weixin_43928944的博客
04-20 3992
接上一篇 ROS教程 Gazebo仿真(1)–ros_control两轮差速控制 https://blog.csdn.net/weixin_43928944/article/details/115899503 配置激光雷达 laser_sensors.xacro <robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro"> <!-- 雷达 --> <gazebo reference="laser"&
ROS教程 Gazebo仿真(1)--ros_control两轮差速控制
weixin_43928944的博客
04-20 3794
创建机器人描述 catkin_create_pkg
解释这段C++代码 ros::NodeHandle nh; ros::Subscriber pcl_sub1; ros::Subscriber pcl_sub2; // pcl::visualization::CloudViewer viewer; ros::Timer viewer_timer; pcl::visualization::PCLVisualizer viewer; int outviewer=0,shape[500]; int shape_num=0;
最新发布
05-25
- `ros::Subscriber pcl_sub1;` 和 `ros::Subscriber pcl_sub2;` 创建了两个点云消息订阅器，用于接收ROS中发布的点云数据。 - `ros::Timer viewer_timer;` 创建了一个计时器，用于定期更新可视化界面。 - `pcl::...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值