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文件：

建立一个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后面要放入自己的点云话题

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

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

下面我们来看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

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

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

pclPoint_pub = n.advertise("/ORB_SLAM3/Point_Clouds",1000000);pclPoint_local_pub = n.advertise("/ORB_SLAM3/Point_local_Clouds",1000000);

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

/** * @brief 根据关键帧生成点云 * @param kf * @param imRGB * @param imD * @param pose * @return */pcl::PointCloud::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(v)[u];if(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(v)[u*3];p.g = imRGB.ptr(v)[u*3+1];p.r = imRGB.ptr(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 PointCloud;// PointCloud::Ptr pcE;// Eigen::Isometry3d T;// int pcID;PointCloude pointcloude;ros::NodeHandle n;pclPoint_pub = n.advertise("/ORB_SLAM3/Point_Clouds",1000000);pclPoint_local_pub = n.advertise("/ORB_SLAM3/Point_local_Clouds",1000000);ros::Rate r(5);/// 一直在执行while(true){KeyFrame* kf;cv::Mat colorImg, depthImg;{std::unique_lock locker(mKeyFrameMtx); 1.如果没有关键帧(还没有进入追踪线程,等待关键帧的加入)while(mvKeyFrames.empty() && !mbShutdown){mKeyFrameUpdatedCond.wait(locker);}{unique_lock lck( keyframeMutex );} 2.更新点云(这里代码逻辑有问题)if(lastKeyframeSize== LoopKfId)updatecloud();if (!(mvDepthImgs.size() == mvColorImgs.size() && mvKeyFrames.size() == mvColorImgs.size())) {std::cout << RED << "这是不应该出现的情况！" <cx;mCy = kf->cy;mFx = kf->fx;mFy = kf->fy;}{std::unique_lock 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=" <points.size() <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

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