當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

ORB-SLAM2系统的实时点云地图构建

發布時間：2023/11/27 生活经验 27 豆豆

生活随笔收集整理的這篇文章主要介紹了 ORB-SLAM2系统的实时点云地图构建小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

ORB-SLAM2系統的實時點云地圖構建

這篇博客
點云地圖構建的流程
代碼介紹
- 點云地圖構建類對象
- 小調整
- 獲取關鍵幀
- 點云地圖構建與疊加
- 在地圖中設置當前相機位置
- 點云地圖到Octomap的轉換
地圖效果
結尾

這篇博客

（PS:修改于2020-9-21，添加了關于System和Tracking類中相關函數的修改(“小調整”中的內容)）
?這篇博客介紹如何給ORB-SLAM2系統添加實時點云地圖構建環節。這個項目高翔博士已經做過，在GitHub上也能找到相關代碼。在參考這個項目之后，我嘗試用自己的方式來實現地圖構建環節，并添加了一些新的東西。下面來看一下修改后的整個環節的工作流程。

點云地圖構建的流程

?構建點云地圖需要用到彩色圖，深度圖和幀的位姿信息。為了防止構建的地圖過大，降低內存負擔，系統只使用關鍵幀來構建點云地圖（也能減少點云地圖中重疊的部分）。整個地圖構建的流程如下：

?最后兩步是自己做的修改部分，目的在于增強地圖的可視效果，以及給后續導航操作提供便利。整個系統是在ROS下運行的，相關代碼實現如下：

代碼介紹

?在代碼中，將地圖構建環節定義為一個類對象，通過其中的成員函數來完成流程圖中的操作。原來的項目是單獨拿出一個線程來實現點云地圖的更新和顯示操作。但為了適應后面新加的部分操作，這里就不使用單獨的線程來實現，而是在Tracking的線程中進行該類對象的函數調用。
?先來看看構建地圖的類包含了哪些成員。

點云地圖構建類對象

class MyPclMapping
{
public:typedef pcl::PointXYZRGBA PointT;typedef pcl::PointCloud<PointT> PointCloud;//類對象的構建函數MyPclMapping();//獲取新關鍵幀的函數void insertKF(KeyFrame* kf,Mat &color,Mat &depth);//構建關鍵幀對應的點云地圖，并將新舊地圖疊加void generatepcl(KeyFrame *kf,Mat &color,Mat &depth);//返回以構建的完整點云地圖。這個在進行八叉樹轉換時使用PointCloud::Ptr Getglobalmap(){unique_lock<mutex> locker(generation);return globalMap;}//設置當前相機在地圖中的位置void setcurrentCamera(Mat Rwc,Mat twc);
private://保存構建所使用的彩色圖像，深度圖和關鍵幀vector<Mat> rgbs;vector<Mat> depthes;vector<KeyFrame*> kfs;//構建的完整點云地圖PointCloud::Ptr globalMap;Eigen::Vector3d kfplace;//使用PCLVisualizer對象來顯示地圖pcl::visualization::PCLVisualizer showcamera;Eigen::Vector3d currentcamera;cv::Mat currentTwc;
};

小調整

?在System類和Tracking類中定義上述類對象的指針，以便其獲得關鍵幀和對應的圖像信息（在初始化System和Tracking類對象時同時初始化地圖構建的指針即可）:
1、對System類中成員和函數的一些修改：

//加在System.h中，聲明一些新成員和函數
MyPclMapping* pointcloud;
MyPclMapping* Getpcl()
{return pointcloud;
}

//加在System類構造函數體中，用于初始化MyPclMapping和Tracking的類對象
pointcloud=new MyPclMapping();
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpMap,pointcloud ,mpKeyFrameDatabase, strSettingsFile, mSensor);

2、對Tracking類中成員和函數的一些修改：

//在Tracking.h中添加MyPclMapping類對象指針，并調整Trakcing類的構造函數中的形參
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,MyPclMapping* pCloud,KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
MyPclMapping* pointmapping;

//在Tracking類構造函數中使用成員初始化表初始化MyPclMapping類對象指針
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap,MyPclMapping* pCloud, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0),pointmapping(pCloud)

（這一部分是添加的內容）
下面介紹如何一步步實現流程中的操作，首先是獲得關鍵幀：

獲取關鍵幀

?因為關鍵幀是由Tracking類中的CreateNewKeyFrame成員函數產生的，所以該函數的最后添加下述代碼：
?pointmapping->insertKF(pKF,this->currentrgb,this->currentdepth);
?pointmapping是Tracking中定義的地圖構建指針對象。關于它的insertKF函數的代碼如下：

void MyPclMapping::insertKF(KeyFrame* kf,cv::Mat &color,cv::Mat &depth)
{//在更新已有地圖時加鎖，防止和Getglobalmap函數沖突unique_lock<mutex> locker(generation);cv::Mat rgb=color.clone();cv::Mat D=depth.clone();//構建關鍵幀對應的點云地圖generatepcl(kf,rgb,D);//將這些信息保存下來rgbs.push_back(color.clone());depthes.push_back(depth.clone());kfs.push_back(kf);
}

?至此第一步就完成了。需要注意的是代碼中的 “currentdepth” 是原深度圖經過一定變化后的Mat對象。變換的方法如下（代碼添加在Tracking中的GrabImageRGBD函數后面）：

if(mDepthMapFactor!=1 || imDepth.type()!=CV_32F)//一定要注意調節這里！！！！
{imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
}
currentdepth=imDepth;

?深度圖要經過轉換后（根據RGB-D相機的深度單位進行變換）再用于構建點云地圖，否則構建出的地圖會像千層餅一樣（血的教訓）。彩色圖像使用原圖即可。之后介紹點云地圖的構建和疊加操作。
PS：這里的 “currentrgb” 和 “currentdepth” 是在Tracking類中新加的對象，專門用來構建點云地圖的。

點云地圖構建與疊加

?地圖的構建和疊加通過generatepcl函數實現：

void MyPclMapping::generatepcl(KeyFrame* kf,cv::Mat &color,cv::Mat &depth)
{//用temp保存關鍵幀對應的點云地圖PointCloud::Ptr temp(new PointCloud());//獲得關鍵幀的估計位姿Eigen::Isometry3d T=ORB_SLAM2::Converter::toSE3Quat(kf->GetPoseInverse());//遍歷彩色圖像，構建點云地圖（在關鍵幀的坐標系下）for(int v=0;v<color.rows;v+=3)//為了降低地圖占用的內存大小，遍歷的步長定為3{for(int u=0;u<color.cols;u+=3){float d=depth.ptr<float>(v)[u];if(d<0) continue;PointT p;//計算地圖點的坐標p.z=d;p.x=p.z*(u-kf->cx)/kf->fx;p.y=p.z*(v-kf->cy)/kf->fy;//給地圖點上色p.b=color.data[v*color.step+u*color.channels()];p.g=color.data[v*color.step+u*color.channels()+1];p.r=color.data[v*color.step+u*color.channels()+2];temp->points.push_back(p);}   }PointCloud::Ptr cloud(new PointCloud());//將新構建的地圖變換到世界坐標系下pcl::transformPointCloud(*temp,*cloud,T.matrix());cloud->is_dense=false;//完成新舊地圖的疊加(*globalMap)+=(*cloud);
}

?此函數完成了點云地圖的構建和更新。之后要做的是在地圖中顯示出相機的位置，以及完成地圖到Octomap（八叉樹地圖）的轉換。

在地圖中設置當前相機位置

?原來的項目使用pcl::visualization::CloudViewer類來顯示點云地圖。這個類的功能較少，幾乎就只有顯示地圖這個用途。因此，在這里用pcl::visualization::PCLVisualizer類來顯示地圖和相機的位置。下面是相關代碼：

//參數：從相機坐標系到世界坐標系的旋轉矩陣和平移向量
void MyPclMapping::setcurrentCamera(Mat Rwc,Mat twc)
{Mat Twc=Mat::eye(4,4,Rwc.type());//獲得當前相機的位姿Rwc.copyTo(Twc(Rect(0,0,3,3)));twc.copyTo(Twc(Rect(3,0,1,3)));currentTwc=Twc.clone();g2o::SE3Quat q=ORB_SLAM2::Converter::toSE3Quat(currentTwc);Eigen::Isometry3d T=Eigen::Isometry3d::Identity();T.rotate(q.rotation());T.pretranslate(q.translation());Eigen::Vector3d currentpoint(0.1,0.1,0.1);//獲得相機在世界坐標系下的坐標位置currentcamera=T*currentpoint;PointCloud::Ptr temp(Getglobalmap());//獲得完整的點云地圖//刷新PCLVisualizer對象中保存的內容showcamera.removeAllPointClouds();showcamera.removeAllShapes();showcamera.addPointCloud(temp);Eigen::Quaternionf qf(float(q.rotation().coeffs()[3]),float(q.rotation().coeffs()[0]),float(q.rotation().coeffs()[1]),float(q.rotation().coeffs()[2]));//用一個立方體來表示相機showcamera.addCube(Eigen::Vector3f(q.translation()[0],q.translation()[1],q.translation()[2]),qf,0.7,0.7,1.2);//顯示一次當前地圖中的內容showcamera.spinOnce();
}

?函數通過不斷刷新PCLVisualizer類對象保存的內容，達到顯示點云地圖和實時相機位置的功能。相機的實時位姿要依據Tracking線程的跟蹤結果，所以在Tracking類的GrabImageRGBD函數的最后加上這一行調用代碼：
pointmapping->setcurrentCamera(mCurrentFrame.GetRotationInverse(),mCurrentFrame.GetCameraCenter());
?到此關于點云地圖部分的操作就弄完了，之后就是如何將其轉換成Octomap。

點云地圖到Octomap的轉換

?點云地圖轉換到Octomap的代碼實現請參考這個博客。但這里是使用ROS中的octomap_server節點，將點云地圖轉成Octomap。這需要把已構建的點云地圖發布到一個topic上。因此對原來系統的ros_rgbd.cpp文件進行修改，主要修改其中定義的ImageGrabber類對象：

class ImageGrabber
{
public:ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){//修改：創建一個線程實時地向ROS發布已構建的點云地圖pclpub=make_shared<thread>( bind(&ImageGrabber::pclpublish, this ) );}void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);//修改：發布點云地圖的函數void pclpublish();ORB_SLAM2::System* mpSLAM;//修改：地圖發布線程對象shared_ptr<thread> pclpub;
};

?給ImageGrabber類對象添加了用于實時發布點云地圖信息的函數和線程。發布地圖信息的pclpublish函數代碼如下：

void ImageGrabber::pclpublish()
{ros::NodeHandle nh;//將點云地圖發布到 “pcltext” 話題上ros::Publisher pub=nh.advertise<sensor_msgs::PointCloud2>("/pcltext",1);string frameid="camera";sensor_msgs::PointCloud2 msg;while(1){//獲得已構建的點云地圖PointCloud::Ptr pointmap((mpSLAM->Getpcl())->Getglobalmap());pcl::PointCloud<pcl::PointXYZ> pclmsg;//octomap_server轉換需要的是沒有顏色的點云地圖//所以將以構建的彩色點云地圖變成無色的for(int i=0;i<pointmap->points.size();i++){pcl::PointXYZ point;point.x=pointmap->points[i].x;point.y=pointmap->points[i].y;point.z=pointmap->points[i].z;pclmsg.points.push_back(point);}//將點云地圖轉換成ROS中的信息數據形式pcl::toROSMsg(pclmsg,msg);msg.header.stamp=ros::Time::now();msg.header.frame_id=frameid;//發布地圖信息pub.publish(msg);usleep(2000);}
}

?通過這個函數就將構建好的點云地圖發布到了對應的ROS話題上。在這里使用了多線程的方法。
?之后用octomap_server接受 “pcltext” 話題的信息就能完成地圖的轉換。為了方便啟動octomap_server節點，編寫一個launch文件：

<launch><!-- 使用的功能包的名稱；啟動的節點；啟動后節點的名字--><node pkg="octomap_server" type="octomap_server_node" name="octomap_server"><!--分辨率（米/像素） --><param name="resolution" value="0.05" /><!-- name of the fixed frame, needs to be "/map" for SLAM這里需要和發布的點云地圖中的frameid一致 --><param name="frame_id" type="string" value="/camera" /><!-- max range / depth resolution of the kinect in meter超出100的讀數，它的值用100代替 --><param name="sensor_model/max_range" value="100.0" /><param name="latch" value="true" /><!-- max/min height for occupancy map, should be in meters --><param name="pointcloud_max_z" value="1000" /><param name="pointcloud_min_z" value="0" /><!-- 讓節點訂閱點云地圖發布的話題 --><remap from="/cloud_in" to="/pcltext" /></node>
</launch>

?在操作過程中，先啟動帶有點云地圖發布的系統，再啟動這個launch文件（要讓 “pcltext” 話題上有點云地圖的信息）。

地圖效果

?上述就是整個地圖構建環節的實現代碼。在操作過程中，先啟動帶有點云地圖發布的系統，再啟動關于octomap_server的launch文件。最后通過Rviz顯示構建的Octomap（點云地圖已設置了顯示窗口）。關于如何使用Rviz來顯示八叉樹地圖大家可以參考這個博客。這個博客也介紹了一種構建地圖的方法（🐂）。
?最終整個系統在仿真環境（仿真環境在GitHub上開源，是其他前輩寫的）中工作的效果圖如下所示：

?左下角的是實時構建的點云地圖。地圖中使用一個立方體來表示相機當前的位置，放大來看就是：

?使用立方體的長軸表示相機的前后兩個方向，短軸則是左右方向。這樣的好處是可以只通過這個地圖來控制機器在環境中移動，躲避障礙。這個圖的上方就是轉換后的Octomap。

結尾

?至此整個修改后的點云地圖構建環節就介紹完了。之前想著再在點云地圖中顯示所有關鍵幀的位置，以表明相機的運動軌跡。但這樣會消耗挺大內存，所以需要考慮其他方法。此外，這個系統還存在不足的地方：
?1、在檢測到閉環以后，已構建的點云地圖不會根據閉環進行地圖的修復；
?2、立方體前后方向、左右方向區分度很差。
?看來還是任重而道遠，慢慢來吧。

參考資料：
?1、https://blog.csdn.net/crp997576280/article/details/74605766
?2、https://blog.csdn.net/fb_941219/article/details/89002257

PS:轉載請標明出處

總結

以上是生活随笔為你收集整理的ORB-SLAM2系统的实时点云地图构建的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。