ORB-SLAM2栅格地图构建_八叉树地图生成二维栅格地图

ORB-SLAM2栅格地图构建

过程

栅格地图的构建是基于稠密点云地图的构建和保存实现的，需要了解可以看我们前面的博客

基于ORB-SLAM2实时构建稠密点云

在点云地图的基础上构建包含占据信息的八叉树地图和二维栅格地图，便于后续避障、导航等功能的实现

点云转八叉树可以参考下面的文章

(1条消息) Octomap 在ROS环境下实时显示_熊猫飞天的博客-CSDN博客_rviz显示map

中间可能遇到的问题：

1. 点云与grid垂直
2. 八叉树地图显示不完整
3. 地面也显示为占据状态

对应的解决可以参考下面的文章

https://blog.csdn.net/sylin211/article/details/93743724

注意下面的几个点

• yaml文件应为深度相机的参数，因为计算世界坐标用的是深度图
• 点云拼接后显示时需要翻转，绕X轴旋转-pi/2使得Z轴朝上
• 实际转化Octomap时一定要注意remap的topic是自己发布点云信息的topic
• Octomap无法显示的时候检查各个话题的Publisher和Subscriber是否正确
• /pointcloud_output的Publisher应为PointCloud节点，Subscriber应为octomap_server节点
• 实际在Rviz中显示时，OccupancyGrid的话题应为/octomap_binary或是/octomap_full

我的转化代码：

#include <iostream>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl-1.7/pcl/io/pcd_io.h>

#include <ros/ros.h>
#include <Eigen/Geometry>
#include <pcl/common/transforms.h>

using namespace std;

int main(int argc,char** argv)
{
    ros::init(argc,argv,"PointCloud");
    ros::NodeHandle nh;

    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud_output",10);

    //加载点云图像
    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    sensor_msgs::PointCloud2 output;
    pcl::io::loadPCDFile("/home/redwall/ORB_SLAM2_modified/map/pointcloud/map.pcd",cloud);
    
    //旋转点云
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate(Eigen::AngleAxisf(-M_PI/2,Eigen::Vector3f(1,0,0)));     //-M_PI*5/12
    pcl::transformPointCloud(cloud,cloud,transform);

    //将PCL点云转化为ROS消息的格式
    pcl::toROSMsg(cloud,output);    
    output.header.stamp = ros::Time::now();
    output.header.frame_id = "world";

    double dt=0.1;
    ros::Rate loop_rate(1/dt);
    while(ros::ok())
    {
        cout<<"output pointclud height:"<<output.height<<endl;
        cout<<"output pointcloud width:"<<output.width<<endl<<endl;
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

我的octomap_server launch文件如下：

<?xml version="1.0"?>
<launch>
    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <param name="frame_id" type="string" value="/world"/>

    <param name="resolution" value="0.05"/>

    <param name="sensor_model/max_range" value="1000.0"/>
    <param name="latch" value="true"/>

    <param name="pointcloud_max_z" value="1000.0"/>
    <param name="pointcloud_min_z" value="-0.1"/>

    <remap from="/cloud_in" to="/pointcloud_output"/>

    </node>   
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

octomap_server生成二维栅格地图可以参考下面的文章

octomap_server使用－－生成二维占据栅格地图和三维概率地图_sru_alo的博客-CSDN博客_点云地图转换成二维栅格地图

结果

首先查看生成的pcd点云图，如下只跑了实验室的一个角落

在Rviz中查看八叉树和栅格地图

注意：

• 点云需要旋转，将相机的Z轴与Octomap的Z轴对齐，不然会影响栅格地图的生成
• 地面点云不应被投影，故应该设置好pointcloud_min_z值