RTABMAP-ROS RGB-D的建图原理

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函数

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

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

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

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

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

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

检测地面点云

util3d::segmentObstaclesFromGround，来自util3d_mapping.hpp
使用util3d::normalFitering方法滤波获取地面点云，指标为与垂直法向量的夹角大小，默认值为45°。首先，通过pcl::NormalEstimationOMP方法，使用KdTree作为搜索方法，并通过setViewPoint方法设置视角，根据公式计算点云所有点的法向量。通过pcl::getAngle3D获得点云每个点法向量与地面垂直向量的夹角。实现方法在util3d_filtering.cpp
提取聚类分离地面与平坦障碍物，方法为util3d::extractClusters，来自util3d_filtering.cpp。具体算法为pcl::EuclideanClusterExtraction

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

通过地面与障碍物高度排除三维空间外点，采用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范围来排除三维空间外点

生成栅格地图

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项目也很有意思

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

转载于:https://www.cnblogs.com/severnvergil/p/6671879.html