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

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

願我不斷修煉 讓自己有如此的全棧能力 面對問題都能第一時間想到解決辦法