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项目也很有意思
愿我不断修炼 让自己有如此的全栈能力 面对问题都能第一时间想到解决办法
