octomap使用及与PCL中pointcloud之间的相互转换处理

目录

一、octomap安装及基本介绍;

二、octomap与OccupancyGrid，OccupancyMap;

三、octomap与pointcloud类型相互转换;

---

一、octomap安装及基本介绍；

  octomap是专门开发用于三维导航的栅格地图格式，其采用了八叉树数据结构。其可以用来做三维空间中的导航、避障，也可以用来实现3D激光SLAM中的动态障碍物消除，动态地图更新等操作，是一个比较实用的功能包。

具体的可以参考高博写的一篇博客：

https://www.cnblogs.com/gaoxiang12/p/5041142.html

octomap官网：

https://octomap.github.io/

octomap源码安装：

1.编译octomap

如果没有git请安装git：
sudo apt-get install git

sudo git clone https://github.com/OctoMap/octomap

git会把代码拷贝到当前目录/octomap下。进入该目录，进行安装：

mkdir build
cd build
sudo cmake ..
sudo make

octomap_server 安装：octomap_server是ROS中的一个基于octomap的功能包。

sudo apt-get install ros-kinetic-octomap-ros #安装octomap-ros
sudo apt-get install ros-kinetic-octomap-msgs
sudo apt-get install ros-kinetic-octomap-server
 
安装octomap 在 rviz 中的插件
sudo apt-get install ros-kinetic-octomap-rviz-plugins

安装成功后，打卡rviz：

二、octomap中OccupancyGrid, OccupancyMap ;

在octomap中提供了很多转换函数可以使用：

1、ROS中提供了关于octomap的转换接口：

pcl::toROSMsg(*current_pointcloud, filtered_points);
sensor_msgs::PointCloud2 filtered_points;
octomap::Pointcloud octo_cloud;  
octomap::pointCloud2ToOctomap(filtered_points, octo_cloud);

2、octomap中转为二维栅格地图OccupancyMap ；

//octomap二维占据栅格地图;
ros::Publisher _octomap_pub;
_octomap_pub = node.advertise<octomap_msgs::Octomap>("/octomap", 5);
if(_octomap_pub.getNumSubscribers() != 0){
    octomap_msgs::Octomap octomap_msg;
    octomap::OcTree *octo_map_all;
    octomap_msgs::binaryMapToMsg(*octo_map_all, octomap_msg);
    octomap_msg.header.frame_id = "map";
    octomap_msg.header.stamp = stamp_now;
    octomap_msg.id = 1;
    octomap_msg.binary = 1;
    octomap_msg.resolution = octo_map_all->getResolution();
    _octomap_pub.publish(octomap_msg);
//    std::cout << "DistMap:: update distance map. map leaf node size " << octo_map_all->getNumLeafNodes() << std::endl;
}

 

3、octomap中发布三维栅格地图OccupancyGrid ；

//三维栅格地图;
ros::Publisher _octomap3D_pub;
_octomap3D_pub = node.advertise<octomap_msgs::Octomap>("/octo3Dmap", 5);
if(_octomap3D_pub.getNumSubscribers() != 0){
  octomap::OcTree *octo_map_all;
  octomap_msgs::Octomap octomap_fullmsg;
  octomap_msgs::fullMapToMsg(*octo_map_all, octomap_fullmsg);
  octomap_fullmsg.header.frame_id = "map";
  octomap_fullmsg.header.stamp = stamp_now;
  _octomap3D_pub.publish(octomap_fullmsg);
}

三、octomap 与 pointcloud 类型相互转换;

1、octomap转pcd;

pcl::PointCloud<pcl::PointXYZI>::Ptr occupied_nodes(new pcl::PointCloud<pcl::PointXYZI>());

for(octomap::OcTree::leaf_iterator it = _map_ptr->begin_leafs(), end = _map_ptr->end_leafs();it != end; ++it){
    pcl::PointXYZI cube_center;
    cube_center.x = it.getX();
    cube_center.y = it.getY();
    cube_center.z = it.getZ();
    cube_center.intensity = it.getDepth();

    if(_map_ptr->isNodeOccupied(*it)){
      occupied_nodes->points.push_back(cube_center);
    }
  }
  occupied_nodes->header.frame_id = "map";
  mapUpdate_pub.publish(occupied_nodes);

2、pcd转octomap;

void pcd2octomap(const pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,octomap::OcTree* tree)
{
    //cout<<"convert pcl pointcloud to octomap, point size = " << cloud->points.size() <<endl;
    for (auto p : cloud->points){
     // 将点云里的点插入到octomap中
        tree->updateNode( octomap::point3d(p.x, p.y, p.z), float(20.0));
    }
    // 更新octomap
    tree->updateInnerOccupancy();
}

int main()
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>());
    octo_map_all = new octomap::OcTree(_octree_resolution);
    pcd2octomap(cloud_filter, octo_map_all);
}

推荐一个可以用来学习octomap基础使用的github代码包,里面有很多基础操作：

https://github.com/dan-riley/octomap_merger