ORB-SLAM2栅格地图构建

过程

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

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

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

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

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

中间可能遇到的问题：

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

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

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;
}

我的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>