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)
  1. 根據當前位姿,將點雲從相機坐標系轉換至世界坐標系。
  • 調用rtabmap的util3d::transformPointCloud()實現。
  1. 機器人范圍檢測與環境高度檢測
  • 分別采用util3d::cropBox和util3d::passThrough方法實現,在util3d_filtering.cpp中。
  1. 檢測地面點雲
  • util3d::segmentObstaclesFromGround,來自util3d_mapping.hpp

  • 使用util3d::normalFitering方法濾波獲取地面點雲,指標為與垂直法向量的夾角大小,默認值為45°。首先,通過pcl::NormalEstimationOMP方法,使用KdTree作為搜索方法,並通過setViewPoint方法設置視角,根據公式計算點雲所有點的法向量。通過pcl::getAngle3D獲得點雲每個點法向量與地面垂直向量的夾角。實現方法在util3d_filtering.cpp

  • 提取聚類分離地面與平坦障礙物,方法為util3d::extractClusters,來自util3d_filtering.cpp。具體算法為pcl::EuclideanClusterExtraction

  1. 對地面點雲濾波,分離地面與非地面點雲

通過地面與障礙物高度排除三維空間外點,采用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范圍來排除三維空間外點

  1. 生成柵格地圖
  • 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項目也很有意思

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


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM