RTABMAPROS RGBD的建图原理

RTABMAP-ROS 调用RTABMAP的方法

CoreNode.cpp: new CoreWrapper --(CoreWrapper.cpp: process() -- mapsManager_.updateMapCaches)
Q：CoreNode如何与CoreWrapper建立关联？
MapsManager.cpp: iter; memory->getSignatureDataConst

RTABMAP 源码解析

Rtabmap.cpp: process() -- memory->update()
Memory.cpp: update()

定位点生成：

CoreWrapper.cpp -- process函数

• 首先确定输入的RGB和深度图类型

• 进入RTABMAP的process函数，接口为

　　process(const SensorData& data, Transform odomPose, const cv::Mat& covariance = cv::Mat::eye(6,6, CV_64FC1));

下面为RTABMAP sdk的代码分析

process函数: if RGB-D SLAM is enabled, a pose must be set.

（IncrementalMemory: 增量式构建标志位）

• !odomPose.isNull() && _memory->isIncremental(): 进入纯地图构建的模式

进入Memory.cpp -- update(data, odomPose, covariance)

没有采用OPENCV_NONFREE模块，因此特征提取使用的是ORB特征点。

主要参数：oFAST: ScaleFactor; rBRIEF: PatchSize

Decimated:对图像进行降采样。decimate原意为十中抽一，此处引申为降采样。// 源代码应该没有进行

进入util3d_features.cpp generateWords3DMono(words, prev_words, cameraModles, transform);　　// 此函数中注明使用三角化估计

有极线约束、PnPRANSAC……

RGB-D odometry：

从util3d_features.cpp开始解读：
首先使用极线约束判断3D词典是否具有匹配性：
F = findFundamentalMat并计算P(3*4)，判断是否有inliners
如果有的话，使用码盘数据作为较精确的初始估计，赋值给P
triangulatePoints计算后的P作为3D词典特征点对的初始估计
最后PnP RANSAC计算两帧间视觉变换

3D点云构建：

基于不同定位点内的点云及其全局位姿，拼接生成全局点云。
get3DMap函数实现。
具体例子可看examples/RGBDMapping

点云分割与地面检测：

OccupancyGrid.hpp -- segmentCloud函数

1. 对点云进行体素化与降采样。调用pcl的setLeafSize()实现。

• rtabmap: cloud_voxel_size: 0.05f, gridCellSize = 0.05f. ICP配准中没有启用voxel(parameters.h)

2. 根据当前位姿，将点云从相机坐标系转换至世界坐标系。

• 调用rtabmap的util3d::transformPointCloud()实现。

3. 机器人范围检测与环境高度检测

• 分别采用util3d::cropBox和util3d::passThrough方法实现，在util3d_filtering.cpp中。

4. 检测地面点云

• util3d::segmentObstaclesFromGround，来自util3d_mapping.hpp

• 使用util3d::normalFitering方法滤波获取地面点云，指标为与垂直法向量的夹角大小，默认值为45°。首先，通过pcl::NormalEstimationOMP方法，使用KdTree作为搜索方法，并通过setViewPoint方法设置视角，根据公式计算点云所有点的法向量。通过pcl::getAngle3D获得点云每个点法向量与地面垂直向量的夹角。实现方法在util3d_filtering.cpp

• 提取聚类分离地面与平坦障碍物，方法为util3d::extractClusters，来自util3d_filtering.cpp。具体算法为pcl::EuclideanClusterExtraction

5. 对地面点云滤波，分离地面与非地面点云

通过地面与障碍物高度排除三维空间外点，采用passThrough直通滤波器方法滤波，从之前的步骤获取这两种点云的下标值

    //setNegative: 在min与max范围内的被保留。minGroundHeight为min输入,maxObstacleHeight为max输入
pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		const std::string & axis,
		float min,
		float max,
		bool negative)
{
	UASSERT(max > min);
	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PassThrough<pcl::PointXYZRGB> filter;
	filter.setNegative(negative); 
	filter.setFilterFieldName(axis);
	filter.setFilterLimits(min, max);
	filter.setInputCloud(cloud);
	filter.filter(*output);
	return output;
}

或者通过cropbox方法直接通过移动机器人footprint范围来排除三维空间外点

6. 生成栅格地图

• util3d::occupancy2DFromGroundObstacles，来自util3d_mapping.cpp

-　各种错误

[ WARN] (2017-04-11 09:49:24.355) OdometryF2M.cpp:256::computeTransform() Registration failed: "Variance is too high! (max inlier distance=0.020000, variance=1.393468)"

-------------------------------------闲聊的分割线---------------------------------------------

聊一下谢布克大学的IntroLab吧

这个实验室的工程师氛围之浓 从介绍网页上就能看出来

再加上RTABMAP代码之规范 使我肃然起敬

而且每年都坚持发ICRA IROS

除了RTABMAP+AZIMUT 根据人声跟踪的移动机器人MakeEars项目也很有意思

愿我不断修炼 让自己有如此的全栈能力 面对问题都能第一时间想到解决办法