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_sec
https://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 Wiki
<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.存放关键帧的ID pointcloude.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 发布给octomap pcl::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 发布给octomap pclPoint_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
在主目录下就可以看到我们的导航地图啦！
