5.如何利用ORBSLAM3生成可用于机器人/无人机导航的二维/三维栅格地图--以octomap为例

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< 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(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(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 << "这是不应该出现的情况！" << 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 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

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