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
pclPoint_local_pub = n.advertise
我们把所有帧的稠密点云赋予给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
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
p.g = imRGB.ptr
p.r = imRGB.ptr
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::Ptr pcE;
// Eigen::Isometry3d T;
// int pcID;
PointCloude pointcloude;
ros::NodeHandle n;
pclPoint_pub = n.advertise
pclPoint_local_pub = n.advertise
ros::Rate r(5);
/// 一直在执行
while(true)
{
KeyFrame* kf;
cv::Mat colorImg, depthImg;
{
std::unique_lock
1.如果没有关键帧(还没有进入追踪线程,等待关键帧的加入)
while(mvKeyFrames.empty() && !mbShutdown)
{
mKeyFrameUpdatedCond.wait(locker);
}
{
unique_lock
}
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
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
在主目录下就可以看到我们的导航地图啦！
相关阅读
您阅读本篇文章共花了：
发表评论