本人是SLAM初学者,这是本人学习ORBSLAM3的笔记,论文与代码理解难免会有错误,欢迎交流指正。
系统整体方面,直接采用ORB3论文中的系统组成图。
ORB_SLAM3与ORB_SLAM2的整体结构类似,将系统分为三个线程,分别是TRACKING(跟踪)、LOCAL MAPPING(局部建图)、LOOP & MAP MERGING(回环和地图合并)。
在论文中,作者也提到了本次ORB_SLAM3的创新点如下:
代码首先从main函数入手,以ros_rgbd.cc为例。
(1)首先是ros系统的初始化,以及启动相关线程
ros::init(argc,argv,"RGBD")
ros::start();
(2)创建SLAM系统,system会初始化所有的系统进程,并且准备好生成帧,此处会调用system的构造函数System::System(),具体见System.cc
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);
(3)准备捕获图像,并用SLAM类进行初始化
ImageGrabber igb(&SLAM);
//ImageGrabber类如下
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}//类的初始化
void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);//捕获RGBD图像,并进行跟踪
ORB_SLAM3::System* mpSLAM;
};
(4)订阅话题,获取彩色图像和深度图像,并进行时间戳对齐,最后sync.registerCallback()是同步注册回调函数,当接收到图像后便会运行此函数,调用ImageGrabber中的GrabRGBD函数。
如果运行程序时出现没有画面的情形,大概率是因为话题名称不对应,先使用rostopic list或者rviz查看发布的话题,然后更改下面代码中的话题名称。
message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100);
message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 100);
typedef message_filters::sync_policies::ApproximateTime sync_pol;
message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
(5)在回调函数中的GrabRGBD函数作用是,将订阅获取的ros image message转换为矩阵类型,并将rgbd和depth图像以及时间戳参数传递给System中的TrackRGBD函数,进行跟踪。
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
cv_bridge::CvImageConstPtr cv_ptrRGB;
try
{
cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_bridge::CvImageConstPtr cv_ptrD;
try
{
cv_ptrD = cv_bridge::toCvShare(msgD);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
//此处便开始Track线程
}
(6)最后停止所有线程,保存关键帧位姿,结束程序。
SLAM.shutdown();
SLAM.SaveKeyFramesTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
(1)首先根据主函数中ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);第三个参数判断传感器类型,然后加载相机参数。
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
(2)加载ORB词袋向量,并初始化各类系统进程
(3)主函数中调用TrackRGBD函数(部分)
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, string filename)
{
//Check reset
{
unique_lock lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
mbResetActiveMap = false;
}
else if(mbResetActiveMap)
{
mpTracker->ResetActiveMap();
mbResetActiveMap = false;
}
}
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp,filename);
return Tcw;
}
在检查跟踪器是否重置时,使用到了unique_lock,该对象是对线程进行加锁,在加锁时新建一个对象lock,在这个对象生命周期结束之后自动解锁。如果这个线程在运行时,其他线程这时进来了,但此时发现并没有解锁,只能等待,等解锁后才能执行。
随后,每一帧的旋转矩阵Tcw由Tracking.cc中的GrabImageRGBD函数得到。
(4)System类后面便是保存关键帧轨迹函数,以SaveKeyFrameTrajectoryTUM为例
void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
vector vpKFs = mpAtlas->GetAllKeyFrames();
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
ofstream f;
f.open(filename.c_str());
f << fixed;
for(size_t i=0; iisBad())
continue;
cv::Mat R = pKF->GetRotation().t();
vector q = Converter::toQuaternion(R);
cv::Mat t = pKF->GetCameraCenter();
f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at(0) << " " << t.at(1) << " " << t.at(2)
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
}
此处使用了Atlas地图集中的GeyAllKeyFrame函数得到所有的关键帧,将之前计算出的每个关键帧的旋转矩阵R和平移t以及时间戳按顺序保存下来。
Atlas地图集是由一系列不连续的小地图构成,能够无缝连接,实现重定位、回环检测、地点识别等功能。地图集本身也随着时间逐步优化且将新的视觉帧经过挑选作为关键帧。