1 octomap的安装及官方文档

        这里我们用ROS自带的安装方式即可：

sudo apt install ros-melodic-octomap-msgs ros-melodic-octomap-ros ros-melodic-
octomap-rviz-plugins ros-melodic-octomap-server

        如上图就是安装成功了：

        如果安装失败了，尝试用小鱼ROS换一下源再去安装：

        一些官方的文档如下，大家感兴趣可以学习一下：https://octomap.github.io/octomap/doc/index.html#gettingstarted_sechttps://octomap.github.io/octomap/doc/index.html#gettingstarted_sec

2 如何利用ORBSLAM3生成的地图点通过octomap构造可以用来导航的栅格地图

2.1 octomap节点的编写

        在我们装了octomap后，我们建立一个launch文件，这里都是固定的，我就来给大家解释一下文件的各个参数的含义吧。

        我们建立一个slam.launch文件：

<launch><!-- Octomap Server Node --><node pkg="octomap_server" type="octomap_server_node" name="octomap_server"><param name="resolution" value="0.05" /><param name="frame_id" type="string" value="/orb_cam_link" /><param name="sensor_model/max_range" value="5.0" /><remap from="cloud_in" to="/ORB_SLAM3/Point_Clouds" /><param name="sensor_model/max_range" value="5000.0" /><param name="latch" value="true" /><param name="pointcloud_min_z" type="double" value="-1.5" /><param name="pointcloud_max_z" type="double" value="10" /><param name="occupancy_min_z" type="double" value="0.1" /><param name="occupancy_max_z" type="double" value="2" /><param name="height_map" type="bool" value="False" /><param name="colored_map" value="true" /></node><node pkg="tf" type="static_transform_publisher" name="orb_cam_link" args="0 0 0.15 0 0 0 /orb_cam_link /pointCloud 70" /><!-- rviz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find akm_pnc)/rviz/grid.rviz" />
</launch>

        建立一个Octomap Server Node节点。

        这个参数文件是一个ROS launch文件，它定义了启动和配置了几个节点和参数，主要是针对 Octomap Server、静态 TF 变换发布器和 RViz 可视化工具的配置。

让我解释其中的一些关键部分：

1. **Octomap Server Node**：
    - `pkg="octomap_server"` 和 `type="octomap_server_node"` 指定了要运行的节点以及其所在的软件包。
    - `name="octomap_server"` 定义了节点的名称。
    - `param` 标签下的各个 `name` 参数设置了 Octomap Server 的一些参数：
        - `resolution` 设置了地图分辨率为 0.05。
        - `frame_id` 设置了地图的坐标系为 `/orb_cam_link`。
        - `sensor_model/max_range` 设置了传感器模型的最大范围为 5.0。
        - `latch` 设为 `true`，意味着参数会被持久化，即在重新加载时保留先前设置的参数值。
        - 其他参数如 `pointcloud_min_z`、`pointcloud_max_z`、`occupancy_min_z`、`occupancy_max_z` 用于设置点云和占据地图的高度范围等参数。
        - `colored_map` 设置了地图是否包含颜色信息。

2. **TF 静态变换发布器**：
    - `node` 标签下定义了一个 `static_transform_publisher` 节点，用于发布静态的 TF 变换。
    - `name="orb_cam_link"` 定义了发布节点的名称。
    - `args` 包含了发布的静态变换的参数：位置 (0, 0, 0.15)、旋转 (0, 0, 0) 以及目标坐标系和源坐标系的名称 `/orb_cam_link` 和 `/pointCloud`。
 
3. **RViz**：
    - 最后一个节点启动了 RViz 工具，指定了加载一个配置文件 `grid.rviz`。

总体而言，这个 launch 文件配置了 Octomap Server 用于构建地图，并设置了一些传感器模型、地图分辨率以及静态 TF 变换的发布，最后启动了 RViz 工具以可视化地图和其他相关数据。

        这里需要注意的！！非常重要的参数有两个！！

        第一个是：to后面要放入自己的点云话题

        <remap from="cloud_in" to="/ORB_SLAM3/Point_Clouds" />

        第二个是frame_id：看一下ROS官方给的说明（“地图将被发布的静态全局坐标系。在动态构建地图时，需要从传感器数据到该坐标系的变换信息可用。”，也就是说，地图会被发布到一个固定的全局坐标系中。在创建地图的过程中，需要能够获得传感器数据与这个全局坐标系之间的转换信息。）

octomap_server - ROS Wikihttp://wiki.ros.org/octomap_server

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

        下面我们来看ORB-SLAM3的部分怎么修改吧！

2.2 ORB-SLAM3发布栅格地图数据

2.2.1 理解坐标系/orb_cam_link、/odom

        我们控制仿真程序向前走。

        这是初始的状态：

        目前的坐标系为orb_cam_link。我们控制仿真程序向前走一段距离。

        我们发现，栅格地图生成了一部分。有尾部的绿线是我们的轨迹。它的话题为/RGBD/Path

        但是我们如果换成坐标系为odom呢？？一直在原点不动了。

        因此，我们估计到的Tcw其实就是orb_cam_link到odom坐标系的变换矩阵。

        这里的track_point和all_point是追踪的地图点和所有的地图点，如上图彩色的部分和白色的部分。

2.2.2 稠密建图代码详解 如何发送全部稠密点云给octomap

        <remap from="cloud_in" to="/ORB_SLAM3/Point_Clouds" />

        这里我们接收/ORB_SLAM3/Point_Clouds类型的点云进行稠密重建，那么需要稠密点云进行输入。

        我们在稠密建图的线程中新添加一个话题：

        pclPoint_pub = n.advertise<sensor_msgs::PointCloud2>("/ORB_SLAM3/Point_Clouds",1000000);pclPoint_local_pub = n.advertise<sensor_msgs::PointCloud2>("/ORB_SLAM3/Point_local_Clouds",1000000);

        我们把所有帧的稠密点云赋予给octomap：

    /*** @brief 根据关键帧生成点云* @param kf* @param imRGB* @param imD* @param pose* @return*/pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame *kf,const cv::Mat& imRGB, const cv::Mat& imD, const cv::Mat& pose){std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();PointCloud::Ptr current(new PointCloud);PointCloud::Ptr loop_points(new PointCloud);for(size_t v = 0; v < imRGB.rows ; v+=3){for(size_t u = 0; u < imRGB.cols ; u+=3){cv::Point2i pt(u,v);bool isDynamic = false;float d = imD.ptr<float>(v)[u];if(d < 0.1 || d>15)continue;PointT p;p.z = d;p.x = ( u - mCx) * p.z / mFx;p.y = ( v - mCy) * p.z / mFy;p.b = imRGB.ptr<uchar>(v)[u*3];p.g = imRGB.ptr<uchar>(v)[u*3+1];p.r = imRGB.ptr<uchar>(v)[u*3+2];current->points.push_back(p);loop_points->points.push_back(p);}}Eigen::Isometry3d T = Converter::toSE3Quat( pose );PointCloud::Ptr tmp(new PointCloud);// tmp为转换到世界坐标系下的点云pcl::transformPointCloud(*current, *tmp, T.inverse().matrix());// depth filter and statistical removal，离群点剔除statistical_filter.setInputCloud(tmp);statistical_filter.filter(*current);(*mPointCloud) += *current;pcl::transformPointCloud(*mPointCloud, *tmp, T.inverse().matrix());// 加入新的点云后，对整个点云进行体素滤波voxel.setInputCloud(mPointCloud);voxel.filter(*tmp);mPointCloud->swap(*tmp);mPointCloud->is_dense = false;return loop_points;}

    /*** @brief 显示点云*/void PointCloudMapping::NormalshowPointCloud(){0.PointCloude数据结构中含有什么// typedef pcl::PointXYZRGBA PointT;// typedef pcl::PointCloud<PointT> PointCloud;// PointCloud::Ptr pcE;// Eigen::Isometry3d T;// int pcID;PointCloude pointcloude;ros::NodeHandle n;pclPoint_pub = n.advertise<sensor_msgs::PointCloud2>("/ORB_SLAM3/Point_Clouds",1000000);pclPoint_local_pub = n.advertise<sensor_msgs::PointCloud2>("/ORB_SLAM3/Point_local_Clouds",1000000);ros::Rate r(5);/// 一直在执行while(true){KeyFrame* kf;cv::Mat colorImg, depthImg;{std::unique_lock<std::mutex> locker(mKeyFrameMtx);1.如果没有关键帧(还没有进入追踪线程,等待关键帧的加入)while(mvKeyFrames.empty() && !mbShutdown){mKeyFrameUpdatedCond.wait(locker);}{unique_lock<mutex> lck( keyframeMutex );}2.更新点云(这里代码逻辑有问题)if(lastKeyframeSize  == LoopKfId)updatecloud();if (!(mvDepthImgs.size() == mvColorImgs.size() && mvKeyFrames.size() == mvColorImgs.size())) {std::cout << RED << "这是不应该出现的情况！" << std::endl;continue;}if (mbShutdown && mvColorImgs.empty() && mvDepthImgs.empty() && mvKeyFrames.empty()) {break;}3.取出我们应该去处理的数据kf = mvKeyFrames.front();colorImg = mvColorImgs.front();depthImg = mvDepthImgs.front();mvKeyFrames.pop();mvColorImgs.pop();mvDepthImgs.pop();}if (mCx==0 || mCy==0 || mFx==0 || mFy==0){mCx = kf->cx;mCy = kf->cy;mFx = kf->fx;mFy = kf->fy;}{std::unique_lock<std::mutex> locker(mPointCloudMtx);4.获得关键帧的位姿cv::Mat mTcw_Mat = kf->GetPoseMat();5.pcE中存放点云数据,已经被转化到世界坐标系下了pointcloude.pcE=generatePointCloud(kf,colorImg, depthImg, mTcw_Mat);6.存放关键帧的IDpointcloude.pcID = kf->mnId;7.存放关键帧的位姿pointcloude.T = ORB_SLAM3::Converter::toSE3Quat(mTcw_Mat);pointcloud.push_back(pointcloude);if(pointcloude.pcE->empty())continue;8.这帧的点云pcl_cloud_local_kf = *pointcloude.pcE;9.所有的点云pcl_cloud_kf = *mPointCloud;10.转换到ROS坐标系下Cloud_transform(pcl_cloud_local_kf,pcl_local_filter);Cloud_transform(pcl_cloud_kf,pcl_filter);11.转化为ROS格式的点云pcl::toROSMsg(pcl_local_filter, pcl_local_point);// TODO 发布给octomappcl::toROSMsg(pcl_filter, pcl_point);12.pclPoint_pub (/ORB_SLAM3/Point_Clouds)pcl_local_point.header.frame_id = "/pointCloud_local";pcl_point.header.frame_id = "/pointCloud";pclPoint_local_pub.publish(pcl_local_point);// TODO 发布给octomappclPoint_pub.publish(pcl_point);std::cout << YELLOW << "show point cloud, size=" << mPointCloud->points.size() << std::endl;lastKeyframeSize++;}}{if(!mPointCloud->empty()){// 存储点云string save_path = "./VSLAMRGBD.pcd";pcl::io::savePCDFile(save_path, *mPointCloud);cout << GREEN << "save pcd files to :  " << save_path << endl;}}mbFinish = true;}

        自适应场景跑，雷达也是一样，建立好了栅格地图：

        我们调用命令去保存：

rosrun map_server map_saver map:=/projected_map

        在主目录下就可以看到我们的导航地图啦！