Rtab-Map 及 RTABMAP-ROS 学习笔记以及遇到的问题

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

相关参考文章： RTAB-MAP原理详解 RTABMAP-ROS RGB-D的建图原理 SLAM: RtabMap中文解析 RTAB-Map学习和测试详解

在这里插入图片描述

Rtab-Map的思想是：假设更频繁的被访问的定位点比其他的定位点更易于形成闭环。这样一个定位点被连续访问的次数就可以用来衡量其易于形成闭环的权重。当需要从WM(Working Memory)转移定位点到LTM（Long-Term Memory）中时，优先选择具有最低权重的定位点。如果具有最低权重的定位点又有多个时，优先选择被存储时间最长的那一个

内存管理模块：WM(Working Memory)+LTM( Long-Term Memory)

WM：工作内存

LTM: 长期或静态内存

当一个节点被放置到LTM时，它在WM中将不再可用。

RTABMAP-ROS 调用RTABMAP的方法 CoreNode ---> CoreWrapper (MapsManager.cpp: iter; memory->getSignatureDataConst) CoreWrapper.cpp: process() ---> mapsManager_.updateMapCaches

1.CoreNode.cpp

通过launch启动rtabmap_ros主节点

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)">

ros::init(argc, argv, "rtabmap");

首先是设置参数

nodelet::V_string nargv;
	for(int i=1;i<argc;++i)
	{ ... 
		nargv.push_back(argv[i]);
	}

传入参数，启动rtabmap

std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "rtabmap_ros/rtabmap", remap, nargv);

2.CoreWrapper.cpp

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet);

通过特定的宏PLUGINLIB_EXPORT_CLASS完成导出，实现class可动态加载（pluginlib使用总结）

在bashCoreWrapper::onInit()中调用MapsManager的初始化，设置参数

mapsManager_.init(nh, pnh, getName(), true);

初始化 rtabmap

// CoreWrapper.h
rtabmap::Rtabmap rtabmap_;
// CoreWrapper.cpp  line 588
rtabmap_.init(parameters_, databasePath_);

如果需要使用已保存的地图useSavedMap_ = true，则需要从内存中加载

if(rtabmap_.getMemory() && useSavedMap_){
	cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
}

如果需要publishTf ，则调用publishLoop，函数中发出了从mapFrameId_到odomFrameId_的TF变化。

if(publishTf && optimizeIterations != 0){
	tfThreadRunning_ = true;
	transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
}

如果 bashif(!this->isDataSubscribed()) = false

bool isDataSubscribed() const {
return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom();
}

即没设置订阅任何数据，则默认订阅RGBD，通过topic="image"，调用默认回调函数

defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);

接下来就是设置一些public parameters

在默认回调函数defaultCallback中首先确定输入的RGB和深度图类型，然后进入RTABMAP的process接口函数

rtabmap_.process(ptrImage->image.clone(), ptrImage->header.seq)

3.MapsManager.cpp

从发出地图的topic开始找到gridMapPub_.publish(map);

gridMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latching_);

可以发现地图数据来自pixels.data，通过调用bashgetGridMap

cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
...
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);

而getGridMap调用了rtabmap中的getMap

cv::Mat MapsManager::getGridMap(float & xMin,float & yMin,float & gridCellSize)
{
	gridCellSize = occupancyGrid_->getCellSize();
	return occupancyGrid_->getMap(xMin, yMin);
}

在RTABMap的OccupancyGrid.cpp中设置了地图的状态

cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
{
   ...
	cv::Mat map = map_;
	if(occupancyThr_ != 0.0f && !map.empty()){
		for(int i=0; i<map.rows; ++i){
			for(int j=0; j<map.cols; ++j){
				const float * info = mapInfo_.ptr<float>(i, j);
				if(info[3] == 0.0f){
					map.at<char>(i, j) = -1; // unknown
				}
				else if(info[3] >= occThr){
					map.at<char>(i, j) = 100; // unknown
				}
				else{
					map.at<char>(i, j) = 0; // empty
				}
			}
		}
	}
	return map;
}

此时的map = map_，获取全局变量下的数据，而map_由函数setMap和update赋值

setMap的调用在rtabmap_ros中，将下行中的map赋值过去

cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);

update

4.关于参数"Grid/3D" 的设置

    <param name="Grid/3D"                       type="string" value="false"/>

计算机图形学——光线追踪（RayTracing）算法在这里插入图片描述如果将该参数设置为true的话，即3D的RayTracing，此时所生成的栅格地图并不完善，如果设置为false则是直接做2DRayTracing。下图为3D，可以看到，靠近小车内侧的障碍物并没有被保留到地图中，保留障碍物点云可以清楚的对比。在这里插入图片描述当做2D时，障碍物可以被准确的保留下来，如下图所示：