ROS::点云PCL(2)八叉树

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

理解

建立空间索引在点云数据处理中已被广泛应用，常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构，比较有代表性的包括 BSP 树、KD 树、KDB 树、 R树、R+树、CELL 树、四叉树和八叉树等索引结构，而在这些结构中 KD 树和八叉树在 3D点云数据排列中应用较为广泛。 PCL 对八叉树的数据结构建立和索引方法进行了实现，以方便在此基础上对点云进行处理操作 。
在这里插入图片描述
八叉树（Octree）的定义是：若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。那么，这要用来做什么？想象一个立方体，我们最少可以切成多少个相同等分的小立方体？答案就是8个。再想象我们有一个房间，房间里某个角落藏着一枚金币，我们想很快的把金币找出来，聪明的你会怎么做？我们可以把房间当成一个立方体，先切成八个小立方体，然后排除掉没有放任何东西的小立方体，再把有可能藏金币的小立方体继续切八等份….如此下去，平均在log8(n)
（n表示房间内的所有物体数）的时间内就可找到金币。因此，八叉树就是用在3D空间中的场景管理，可以很快地知道物体在3D场景中的位置，或侦测与其它物体是否有碰撞以及是否在可视范围内。

-这里引入了一个概念：Voxel翻译为体积元素，简称体素。描述了一个预设的最小单位的正方体
pcl的octree库提供了从点云数据创建具有层次的数据结构的方法。这样就可以对点数据集进行空间分区，下采样和搜索操作。每个八叉树节点有八个子节点或没有子节点。根节点描述了一个包围所有点的3维包容盒子。

pcl_octree实现提供了有效的最近邻居搜索（邻域搜索）API，例如“ 体素（Voxel）邻居搜索”，“ K最近邻居搜索”和“半径搜索邻居”。叶子节点类也提供其他功能，例如空间“占用率”和“每个体素（Voxel）的点密度”检查；序列化和反序列化功能可将八叉树结构有效地编码为二进制格式；此外，内存池实现减少了昂贵的内存分配和释放操作，以便快速创建八叉树。

在这里插入图片描述

方式一：“体素近邻搜索”

#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 <iostream>
#include <vector>
#include <ctime>
#include <pcl/octree/octree_search.h>



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::PointXYZ> cloud2; 
    //pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    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;
		   		}

			}
		}
	}



    // 设置分辨率为2
    float resolution = 3.0f;
    // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGBA> octree(resolution);
    // 设置输入点云
    octree.setInputCloud(cloud);
    // 通过点云构建octree
    octree.addPointsFromInputCloud();
    
    pcl::PointXYZRGBA searchPoint;

	searchPoint.x = -0.5;
    searchPoint.y = -0.5;
    searchPoint.z = -0.5;

// Neighbors within voxel search
    // 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回,
    // 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数
    std::vector<int> pointIdxVec;
    //通过searchPoint确定其所在的体素，返回体素中所有的点的索引保存在pointIdxVec中
    if (octree.voxelSearch(searchPoint, pointIdxVec)) {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x
                  << " " << searchPoint.y
                  << " " << searchPoint.z << ")"
                  << std::endl;

        for (size_t i = 0; i < pointIdxVec.size(); ++i){
			if(cloud->points[pointIdxVec[i]].x==-0.5 && cloud->points[pointIdxVec[i]].y==-0.5 && cloud->points[pointIdxVec[i]].z==-0.5){
			    cloud->points[pointIdxVec[i]].r=0;
			    cloud->points[pointIdxVec[i]].g=255;
			    cloud->points[pointIdxVec[i]].b=0; 
			}else{
			    cloud->points[pointIdxVec[i]].r=255;
			    cloud->points[pointIdxVec[i]].g=0;
			    cloud->points[pointIdxVec[i]].b=0; 
			} 
            std::cout << "    " << cloud->points[pointIdxVec[i]].x
                      << " " << cloud->points[pointIdxVec[i]].y
                      << " " << cloud->points[pointIdxVec[i]].z << 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;
}

在这里插入图片描述

方式二：K 近邻搜索

 	// 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中,
    // 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉
    // 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。
    int K = 20;

    std::vector<int> pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;

    std::cout << "K nearest neighbor search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with K=" << K << std::endl;
    //searchPoint为搜索的点云对象、K为搜索返回的近邻个数、pointIdxNKNSearch返回近邻索引向量、pointNKNSquaredDistance 存储近邻点对应的距离平方向量
    if (octree.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
                      << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
		}

    }

K nearest neighbor search at (-0.5 -0.5 -0.5) with K=20
    -0.5 -0.5 -0.5 (squared distance: 0)
    -0.5 -0.5 -1 (squared distance: 0.25)
    -0.5 -1 -0.5 (squared distance: 0.25)
    -1 -0.5 -0.5 (squared distance: 0.25)
    -1 -1 -0.5 (squared distance: 0.5)
    -0.5 -1 -1 (squared distance: 0.5)
    -1 -0.5 -1 (squared distance: 0.5)
    -1 -1 -1 (squared distance: 0.75)
    -0.5 -0.5 0.5 (squared distance: 1)
    -0.5 0.5 -0.5 (squared distance: 1)
    0.5 -0.5 -0.5 (squared distance: 1)
    -1.5 -0.5 -0.5 (squared distance: 1)
    -0.5 -0.5 -1.5 (squared distance: 1)
    -0.5 -1.5 -0.5 (squared distance: 1)
    -1 -0.5 -1.5 (squared distance: 1.25)
    -1.5 -1 -0.5 (squared distance: 1.25)
    0.5 -1 -0.5 (squared distance: 1.25)
    0.5 -0.5 -1 (squared distance: 1.25)
    -0.5 -1 0.5 (squared distance: 1.25)
    -1 -1.5 -0.5 (squared distance: 1.25)

在这里插入图片描述

方式三：半径内近邻搜索

    // 方式三：半径内近邻搜索
    // “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中,
    // 这两个向量分别存储结果点的索引和对应的距离平方
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    float radius = 1;

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

    //searchPoint要搜索的点、radius半径、pointIdxRadiusSearch结果索引、pointRadiusSquaredDistance距离平方
    if (octree.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
                      << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
		}

    }

Neighbors within radius search at (-0.5 -0.5 -0.5) with radius=1
    -1.5 -0.5 -0.5 (squared distance: 1)
    -1 -1 -1 (squared distance: 0.75)
    -1 -1 -0.5 (squared distance: 0.5)
    -1 -0.5 -1 (squared distance: 0.5)
    -1 -0.5 -0.5 (squared distance: 0.25)
    -0.5 -1.5 -0.5 (squared distance: 1)
    -0.5 -1 -1 (squared distance: 0.5)
    -0.5 -1 -0.5 (squared distance: 0.25)
    -0.5 -0.5 -1.5 (squared distance: 1)
    -0.5 -0.5 -1 (squared distance: 0.25)
    -0.5 -0.5 -0.5 (squared distance: 0)
    -0.5 -0.5 0.5 (squared distance: 1)
    -0.5 0.5 -0.5 (squared distance: 1)
    0.5 -0.5 -0.5 (squared distance: 1)

在这里插入图片描述

wx:pjcoder
关注
  • 4
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
ROS——从深度图转换到octomap（C++）
如果想成为中心，那么就到中心去吧。
06-06 678
八叉树是用于在3D视觉中细分空间的数据结构。每个立方体都可以逐级地细分为8个子立方体，直到达到了给定的最小体积（体素）尺寸。且该最小体积（体素）决定了八叉树的分辨率。通过点云生成占用空间小、内容紧凑的地图，节省空间合并了融合后的点云出现的重叠细节，提高运算效率方便用于进行路径规划、导航、碰撞检测一般场景比较大时，采用较低的分辨率，意味着方块更少，且更大。场景较小时，采用高分辨率，提高精度。以上是一个八叉树的存储示例，左侧为体积模型，右侧为树状表示结构。
【运动规划算法项目实战】八叉树地图（附ROS C++代码）
xiewf的博客
05-06 6073
八叉树地图（Octomap）是一种使用八叉树数据结构来表示三维空间物体和环境信息的开源软件，可以高效地处理和存储传感器数据，并生成高分辨率的三维地图。在机器人领域中，Octomap在ROS中被广泛应用于感知、路径规划和导航等任务中。本文将介绍Octomap的基本原理、优点和缺点、应用场景以及使用方法，以帮助读者更好地了解和应用这一技术。八叉树地图（Octomap）是一种基于八叉树数据结构实现的三维地图，主要用于机器人感知、路径规划和环境建模等领域。
couldn’t connect to server_ROS 八叉树地图构建 使用 octomap_server 建图过程总结！
weixin_42127369的博客
12-22 432
登龙这是我的第 146篇原创之前的构建语义地图工作，最开始用的是 octomap_server：https://github.com/OctoMap/octomap_mapping后面换成了 semantic_slam octomap_generator：https://github.com/floatlazer/semantic_slam)不过还是整理下之前的学习笔记。一、增量构建八...
pcl_495:一个点云库 - 使用 Nodelets 的 ROS 应用程序
06-17
项目介绍 使用 RGB-D 传感器和点云库，我们将地平面与场景的其余部分分开。 如果我们完成了我们所有的主要项目目标，我们将实现一个使用 nodelet 的版本。 如果我们运行项目的 nodelet 版本，我们将使用欧几里德聚类来查找场景中的对象，并可能计算出很酷的东西，例如滚动圆柱体的速度。 项目目标 测试不同的 RGB-D 传感器 测试不同的 RGB-D 传感器驱动程序（openni 与 openni2） 在rviz中可视化RGB-D传感器产生的点云 使用传感器和 PCL（点云库），将地板（或任何平行于相机框架的平面）与场景的其余部分分开 附加目标： 实现使用 ROS nodelet 的版本 在场景中查找对象 对一个对象执行额外的计算（比如找到对象的中心并随着时间的推移跟踪它） 测试硬件 可选择 Microsoft XBOX 360 Kinect 或 Asus Xtion PRO
ROS 八叉树地图构建 - 给 octomap_server 增加半径滤波器！
登龙的专栏
08-09 1603
为了在每帧点云中滤除噪声点，选择了半径滤波器，也用高斯滤波器测试过，但是没有半径效果好，这里记录下在 octomap_server 中增加半径滤波器的步骤，并在 launch 中配置滤波器参数。 一、半径滤波器基本原理 放一张汇报用的 PPT 截图： 原理很简单就是判断一个点云周围（半径 R）有没有足够多（K）的邻居点，如果没有就删除这个点，否则就保留。 二、基本用法 我一般学习技术喜欢到官网看最原始的教程：Removing outliers using a Conditional or RadiusOu
对数据包的点云数据构建八叉树地图
qq_49756908的博客
02-27 427
安装octomap，并构建八叉树地图
pcl::octree::OctreePointCloudSearch：pcl点云图临近点搜索 使用以及报错处理
v俊逸
04-29 1840
Octreepointcloud search classMore... #include <pcl/octree/octree_search.h> Inheritance diagram for pcl::octree::OctreePointCloudSearch< PointT, LeafT, OctreeT >: Collaboration di...
被鱼粉直呼小鱼牛逼的代码,来看ROS2如何进行点云PCL处理(订阅、转换、保存)
生活不止眼前的苟且
07-15 3548
昨天在交流群里有个鱼粉问小鱼关于ROS2订阅点云数据和使用PCL相关的问题。记得小鱼最早接触PCL是大一的时候，至今已经过去了多年。 ROS1里PCL相关程序很多，但是在ROS2相关的代码和例程目前还没有写过，针对这一问题，小鱼接着国外大佬的代码，做了一个叫做ROS2代码模板文档，复制粘贴，不要太爽。 接着说回点云，主要分享点云订阅，发布，生成，以及Cmake配置。 完整的相关代码可以到社区（fishros.org.cn）搜索点云。 前置安装 sudo apt install ros-你的ROS版本代号-p
PCL使用教程-使用八叉树（Octrees）进行空间分割和搜索操作
DayDayUp
05-25 3169
原文链接：https://pcl.readthedocs.io/projects/tutorials/en/latest/octree.html#octree-search 使用八叉树进行空间划分和搜索操作 八叉树是用于管理稀疏3-D数据的基于树的数据结构。 每个内部节点都有八个子节点。 在本教程中，我们将学习如何使用八叉树在点云数据中进行空间分区和邻域搜索。 特别地，我们说明如何执行“体素内近邻搜索（Neighbors within Voxel Search）”，“ K最近邻搜索（K Neares.
pcl基于八叉树进行空间划分和搜索操作
Scarlett的博客
06-30 814
点云分解之octree八叉树
pcl_localization_ros2：使用PCL的基于3D LIDAR的ROS2软件包（非SLAM）
02-04
pcl_localization_ros2 使用PCL的基于3D LIDAR的ROS2软件包。 绿色：路径，红色：地图（5x5网格大小为50m×50m）IO 输入/ cloud（sensor_msgs / PointCloud2） /地图（sensor_msgs / PointCloud2） / initialpose...
pointcloud_to_laserscan:将3D点云转换为2D激光扫描
05-02
ROS 2 pointcloud <-> laserscan转换器 这是ROS 2软件包，提供用于将sensor_msgs/msg/PointCloud2消息转换为sensor_msgs/msg/LaserScan消息并返回的组件。 它实质上是原始ROS 1软件包的端口。 pointcloud_to_...
ros::spin() 和 ros::spinOnce()函数的区别及详解
09-01
主要介绍了ros::spin() 和 ros::spinOnce()函数的区别及详解的相关资料，本文介绍的非常详细，具有参考借鉴价值，感谢兴趣的朋友一起看看吧
apriltag_ros：用于AprilTag检测的ROS2节点
02-10
AprilTag ROS2节点该ROS2节点使用AprilTag库检测图像中的AprilTag，并发布其姿态，ID和其他元数据。 有关AprilTag，本文和参考实现的更多信息，请访问： ://april.eecs.umich.edu/software/apriltag.html话题订阅： ...
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
ROS教程 Gazebo仿真(4)-Kinect深度相机
weixin_43928944的博客
04-20 3313
接上一篇 ROS教程 Gazebo仿真(3)-摄像头 https://blog.csdn.net/weixin_43928944/article/details/115904765 配置Kinect kinect_sensors.xacro <robot name="kinect_sensors" xmlns:xacro="http://wiki.ros.org/xacro"> <gazebo reference="camera"> <sensor ty
八叉树路径规划rrt ros
最新发布
11-30
八叉树路径规划是一种常用的算法，用于在给定环境中规划机器人的路径。RRT（Rapidly-exploring Random Tree，快速扩展随机树）是一种基于树的路径规划算法，适用于复杂的非完整环境。 在ROS（Robot Operating System，机器人操作系统）中，可以使用八叉树路径规划算法和RRT算法进行机器人的路径规划。ROS提供了强大的路径规划库，如move_base，可以方便地实现路径规划功能。 八叉树路径规划算法在ROS中的实现过程如下：首先，通过传感器获取机器人所在环境的地图信息，并将其转化为八叉树地图表示。然后，在八叉树中，以机器人当前位置为起始点，在地图中随机选择一个点作为目标点。接下来，在八叉树中利用RRT算法进行快速扩展，通过不断将树向目标点扩展，直到找到一条可行的路径。 在ROS中实现八叉树路径规划可以使用近似最近邻搜索算法，如KD-树，来加速RRT算法的扩展过程，并提高路径规划的效率。 八叉树路径规划的优势在于能够在复杂的环境中找到有效的路径，充分考虑机器人的运动限制和环境的障碍物分布情况。同时，通过RRT算法的随机性，可以有效避免陷入局部最优解的问题。 总之，八叉树路径规划和RRT算法是在ROS中常用的路径规划方法，能够实现机器人的自主导航和路径规划功能，提高机器人的移动效率和安全性。

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

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

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值