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項目也很有意思
願我不斷修煉 讓自己有如此的全棧能力 面對問題都能第一時間想到解決辦法