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