目前基于orb_slam想做的方向
提升动态建图精度 （ √东西Map就是上不去 KITTI有几个groundtruth官网下架了找不到而且
红外相机退化环境下的点线融合 数据集https://sites.google.com/view/rpmsthereo/
导航思路1 融合二维激光雷达 视觉点投影或切面
导航思路2 稠密建图导航
nerf+slam
写得很烂，渣学校研一，啥也不懂，就看了十四讲和概率机器人，ros会调个包。希望能坚持下来
///???麻了，草稿不保存啊，发布了慢慢更新吧。
//去看每一个库函数、看懂所有代码不是很现实。半个月只看了三个部分。除了主体几个函数其他的应该看流程和大致代码。需要改的时候看具体哪个参数和具体的库函数。但是例如opencv提取特征点之类的函数看完之后应该好好学一下
//???为什么发布不了没显示哇写了好久的没了
//回环部分看的很难受不是很懂。尤其坐标变换啥的基本看不动。可能还需要过一下十四讲。后续可能会过下因子图优化
//看完了 后面的回环草草过了，看了一个多月了真看吐了啊。先看论文吧，如果打算做回环部分的再回头看
//这篇笔记虽然写的很烂，但对我来说收获肯定很大。对别人或者后面的学弟学妹只能当一下参考文档或踩坑笔记。其实应该做一下思维导图和参数名注释为了后面查询和改代码的，但是我懒而且别人做的也很好。
//一个月辣，5w字，润了润了，奖励自己打一晚上游戏。
//麻了，没做思维导图现在全忘辣，为我的错误自罚一杯。
目录
void ExtractorNode::DivideNode
ORBextractor::DistributeOctTree
ORBextractor::ComputeKeyPointsOld/
void MapPoint::Replace(MapPoint* pMP)替换地图点
void MapPoint::ComputeDistinctiveDescriptors()
void MapPoint::UpdateNormalAndDepth()
float MapPoint::GetMaxDistanceInvariance()
void Frame::AssignFeaturesToGrid()
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
vector Frame::GetFeaturesInArea
void Frame::ComputeStereoMatches()
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
void KeyFrame::UpdateBestCovisibles()
void KeyFrame::UpdateConnections(bool upParent)
void Tracking::MonocularInitialization()
void Tracking::CreateInitialMapMonocular()
void Tracking::StereoInitialization()
bool Tracking::TrackLocalMap()
bool Tracking::NeedNewKeyFrame()
void Tracking::CreateNewKeyFrame()
void LocalMapping::ProcessNewKeyFrame()处理关键帧
void LocalMapping::MapPointCulling()剔除不好的地图点
void LocalMapping::CreateNewMapPoints()
void LocalMapping::SearchInNeighbors()融合当前关键帧和其共视帧的地图点
bool LoopClosing::NewDetectCommonRegions()闭环检测
bool LoopClosing::DetectAndReffineSim3FromLastKF
void LoopClosing::CorrectLoop()回环矫正
void LoopClosing::MergeLocal2()惯性模式下的地图融合
前言：
我的环境：ubuntu20、opencv4.5.5、eigen3.4、boodt1.75.python3.8硬件环境：5900hx、3060laptop
nvidia显卡驱动不是很好装、每次装完后都黑屏，按照网上解决办法试了没成功，最后用的社区开源驱动。因为个人原因选择了ubuntu20，但实验室老哥用的ubuntu18遇到的问题比我少很多，虽然官方测试了18与20，但还是 推荐ubuntu18.
参考资料：
1：5小时让你假装大概看懂ORB-SLAM2源码_哔哩哔哩_bilibili
6.这个老哥太强了：https://blog.csdn.net/ncepu_chen/category_9874746.html ORB-SLAM2代码详解01: ORB-SLAM2代码运行流程_ncepu_Chen的博客-CSDN博客_orbslam2代码
7.回环部分：orb_slam代码解析(3)LocalMapping线程 – h_立青人韦 – 博客园
前期准备：
以t265为例，基于ros运行时需要写一个setting，找一个Camera.type: “KannalaBrandt8″的yaml根据自己的相机改写就好，新版中有自带d435i和t265的yaml。在实际运行时候可能需要设置imu发布频率、话题名等。我的左摄像到imu一直提示none，用的同门的setting。
如果运行时候没有画面，需要查看入口函数，如ros_stereo_inertial.cc里订阅的话题名是否一样。并且t265、d435i、奥比中光等相机需要在launch文件中开启imu话题发布。
如果没有画面，显示waiting for images时，可以使用rqt_graph看看相机发布的图像、imu话题是否有被ros_setero_inertial订阅。
笔记：
main函数
【泡泡机器人公开课】第三十六课：ORB-SLAM2源码详解-吴博_哔哩哔哩_bilibili
int main(int argc, char **argv)
{
ros::init(argc, argv, "Stereo_Inertial");//
ros::NodeHandle n("~");//初始化发布节点名和话题名
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);//info级别的调试信息
bool bEqual = false;//是否图像矫正
if(argc < 4 || argc > 5)
{
cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl;
ros::shutdown();
return 1;
}//检查输入参数的个数
std::string sbRect(argv[3]);//？
if(argc==5)
{
std::string sbEqual(argv[4]);//？
if(sbEqual == "true")
bEqual = true;
}
// 初始化线程准备处理帧
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO,true);
ImuGrabber imugb;
ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual);
if(igb.do_rectify)
{
//yaml给到标定
cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "ERROR: Wrong path to settings" << endl;
return -1;
}//读图
//内参、畸变
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;//
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return -1;
}
//计算无畸变和修正转换映射
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);
}
// Msubscriber左右目image imu 这里注意如果自己搭相机的要改setting中的话题名
ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);
ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb);
ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb);
//同步imu和图像
std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);
ros::spin();
return 0;
}
首先定义了图像和imu抓取器，获取imu和图像的sensor_时都用的互斥锁
class ImageGrabber//
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){}
void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg);//https://zhuanlan.zhihu.com/p/310285167
void GrabImageRight(const sensor_msgs::ImageConstPtr& msg);//读取左右目
cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);//ros图片转mat getimage
void SyncWithImu();
queue<sensor_msgs::imageconstptr> imgLeftBuf, imgRightBuf;
std::mutex mBufMutexLeft,mBufMutexRight;//定义互斥锁 用于左目右目
ORB_SLAM3::System* mpSLAM;
ImuGrabber *mpImuGb;
const bool do_rectify;
cv::Mat M1l,M2l,M1r,M2r;
const bool mbClahe;
cv::Ptr<cv::clahe> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
};
</cv::clahe></sensor_msgs::imageconstptr>
orb_slam3:: system在system.h里，主要用于输入imu：初始化slam system，初始化包括定位、回环、查看线程。 首先会根据输入选择rgbd、mono等容器，然后初始化启动定位、关闭定位、换地图、reset地图等。
void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg)
{
mBufMutexLeft.lock();
if (!imgLeftBuf.empty())
imgLeftBuf.pop();
imgLeftBuf.push(img_msg);
mBufMutexLeft.unlock();
}
//
锁，非空pop，空push，右目、imu同理
cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
// ros图像转成cv::mat getimage
cv_bridge::CvImageConstPtr cv_ptr;
void ImageGrabber::SyncWithImu()没咋看懂
void ImageGrabber::SyncWithImu()
{
const double maxTimeDiff = 0.01;两目最大时间差
while(1)
{
cv::Mat imLeft, imRight;
double tImLeft = 0, tImRight = 0;
if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty())//左右目图像imu非空
{
tImLeft = imgLeftBuf.front()->header.stamp.toSec();
tImRight = imgRightBuf.front()->header.stamp.toSec();
this->mBufMutexRight.lock();
while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1)
{
imgRightBuf.pop();
tImRight = imgRightBuf.front()->header.stamp.toSec();
}
this->mBufMutexRight.unlock();
this->mBufMutexLeft.lock();
while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1)
{
imgLeftBuf.pop();
tImLeft = imgLeftBuf.front()->header.stamp.toSec();
}
this->mBufMutexLeft.unlock();
if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff)
{
// std::cout << "big time difference" << std::endl;
continue;
}
if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())
continue;
this->mBufMutexLeft.lock();
//front是 element of the %queue. timleft是左目第一帧给到stamp头，右目同理，左右目差最大时间差0.01时候，左目退到下一帧，直到同步。
imLeft = GetImage(imgLeftBuf.front());
imgLeftBuf.pop();
this->mBufMutexLeft.unlock();
this->mBufMutexRight.lock();
imRight = GetImage(imgRightBuf.front());
imgRightBuf.pop();
this->mBufMutexRight.unlock();
//互斥锁读图片
vector<orb_slam3::imu::point> vImuMeas;
mpImuGb->mBufMutex.lock();
if(!mpImuGb->imuBuf.empty())//imu非空
{
// Load imu measurements from buffer
vImuMeas.clear();
while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=timleft) 非空且时间戳同步 { double t="mpImuGb-">imuBuf.front()->header.stamp.toSec();
cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));//?
mpImuGb->imuBuf.pop();
}
}
mpImuGb->mBufMutex.unlock();
if(mbClahe)
{
mClahe->apply(imLeft,imLeft);
mClahe->apply(imRight,imRight);
}
//自适应直方图均衡的东西
if(do_rectify)
{
cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR);
}//没找到remap在哪 资料说是图像矫正
mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas);
//跟踪 开冲
std::chrono::milliseconds tSleep(1);
std::this_thread::sleep_for(tSleep);
}
}
}</=timleft)></orb_slam3::imu::point>
System.cc
1读取当前传感器类型
2.读取settings
3.加载Vocabulary
4创建关键帧库
5创建多地图
6创建线程 跟踪、局部建图、回环、显示路径。
计算机视觉life的注释：
这里的参数、变量大都在system.h里定义，函数基本在对应功能的头文件，头文件的参数不在system里就在对应的cc中，有时候跳转定义不跳转。
esensor传感器类型，枚举类。
mpViewer:The viewer draws the map and the current camera pose. It uses Pangolin.
Viewer:这里把mpviewer强制转换成空指针，Main thread function. Draw points, keyframes, the current camera pose and the last processed frame. Drawing is refreshed according to the camera fps. We use Pangolin.
接下来 选择传感器类型， 读setting文件、 加载ORB_Vocabulary，这里加载Vocabulary调用了DBow2库（红色加粗标注了大体步骤）
mpVocabulary = new ORBVocabulary();
//新建Vocabulary
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);//读字典
//ORB vocabulary used for place recognition and feature matching.
创建关键帧的DATABASE
pKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
atlas地图：
{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{{
atlas定义在atlas.h中,initkfid初始化时创建第一幅地图，mnLastInitKFidMap是在上一个地图最大关键帧的id +1（？mHasViewer没找到）
Atlas::Atlas(int initKFid): mnLastInitKFidMap(initKFid), mHasViewer(false)
{
mpCurrentMap = static_cast<map*>(NULL);
CreateNewMap();</map*>
析构函数~atlas中创建一个迭代器，使用mspMaps存档当前地图信息，如果当前活跃地图有效，则存储当前地图为不活跃地图，如果当前地图非空或当前地图最新帧id大于当前地图第一帧id时，mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1存储地图
for(std::set<map*>::iterator it = mspMaps.begin(), end = mspMaps.end(); it != end;)
Map* pMi = *it;
if(pMi)</map*>
}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}
创建atlas地图，初始化initKFid，接着判断是否需要imu信息来设置mbIsInertial以此开启追踪和预积分
mpAtlas = new Atlas(0);
if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
mpAtlas->SetInertialSensor();
mpFrameDrawer = new FrameDrawer(mpAtlas);
mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile);
FrameDrawer:
{{{{{{{{{{{{{{{{{
计算机视觉life的注释：
FrameDrawer::FrameDrawer(Atlas* pAtlas):both(false),mpAtlas(pAtlas)
{
mState=Tracking::SYSTEM_NOT_READY;
// 初始化图像显示画布
// 包括：图像、特征点连线形成的轨迹（初始化时）、框（跟踪时的MapPoint）、圈（跟踪时的特征点）
mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0));
mImRight = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0));
}}}}}}}}}}}}}}}}}]
创建跟踪线程，
Tracking：
首先选择相机配置文件，得到相机输入，特征提取和匹配。
属实有点多，先搞system回头看吧。
}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}}
开启local mapping
localmapping：Local Mapper. It manages the local map and performs local bundle adjustment.
mptLocalMapping：The Tracking thread “lives” in the main execution thread that creates the System object.
判断三角测距是否可行，
mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
if(mpLocalMapper->mThFarPoints!=0)
{
cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
mpLocalMapper->mbFarPoints = true;
}
else
mpLocalMapper->mbFarPoints = false;
创建闭环，创建可视化线程
mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); // mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
//Initialize the Viewer thread and launch
if(bUseViewer)
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
mpLoopCloser->mpViewer = mpViewer;
mpViewer->both = mpFrameDrawer->both;
}
创建线程之间的指针
mpLocalMapper mpLoopCloser 地图和回环之间的指针
mpTracker mpLoopCloser 回环和跟踪
mpTracker mpLocalMapper 追踪和地图
刷志愿者时长去了 摸3天鱼刷志愿者时长去了 摸3天鱼刷志愿者时长去了 摸3天鱼
System::Track 以stereo
选择定位模式或者定位建图
{
unique_lock<mutex> lock(mMutexMode);
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();// 请求停止线程
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
mpTracker->InformOnlyTracking(true);//只建图
mbActivateLocalizationMode = false;设置为false避免上述操作循环
}
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false);
mpLocalMapper->Release();
mbDeactivateLocalizationMode = false;
}
}</mutex>
将imu存储到imudata中
if (mSensor == System::IMU_STEREO)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
得到图像深度时间戳文件名，记录mstate，当前的关键帧的mappoint，当前帧的关键点。
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp,filename);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
</mutex>
SaveTrajectoryTUM：
void System::SaveTrajectoryTUM(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
return;
}
vector<keyframe*> vpKFs = mpAtlas->GetAllKeyFrames();//所有关键帧
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//帧排序
// Transform all keyframes so that the first keyframe is at the origin.
// After a loop closure the first keyframe might not be at the origin.
cv::Mat Two = vpKFs[0]->GetPoseInverse();、、第一帧的位姿
ofstream f;
f.open(filename.c_str());
f << fixed;+
// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
// We need to get first the keyframe pose and then concatenate the relative transformation.
// Frames not localized (tracking failure) are not saved.
// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
// which is true when tracking failed (lbL).
list<orb_slam3::keyframe*>::iterator lRit = mpTracker->mlpReferences.begin();
list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
list<bool>::iterator lbL = mpTracker->mlbLost.begin();
for(list<cv::mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
{
if(*lbL)
continue;
KeyFrame* pKF = *lRit;
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
// If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
while(pKF->isBad())
{
Trw = Trw*pKF->mTcp;
pKF = pKF->GetParent();
}
Trw = Trw*pKF->GetPose()*Two;
cv::Mat Tcw = (*lit)*Trw;
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
vector<float> q = Converter::toQuaternion(Rwc);
f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
// cout << endl << "trajectory saved!" << endl;
}
</float></float></float></float></cv::mat></bool></double></orb_slam3::keyframe*></keyframe*>
清明假期 开卷
上面大概走了下程序流程，现在打算走一遍特征点法→定位→建图
ORBextrsvtor.cc
原理：
以某个像素为中心p，取周围16个像素点，近似一个圆。设置一个阈值T，假设这个点附近有N个点的亮度大于或者小于p的亮度+T或者p的亮度-T则认为这个点是特征点。然后算这个圆的灰度质心。链接这个圆的几何中心与质心得到一个向量，这个向量就是fast关键点的方向。
rbrief描述子旋转不变性
brief描述子：
rbrief描述子因为fast关键点有角度，所以计算描述子时以fast关键点的方向进行旋转，故rbeief具有旋转不变性 。
特征匹配
SearchForInitializtion
orb特征提取使用四叉树特征提取比较均匀。特征匹配时在金字塔最底层的图像上进行，以特征点为中心半径一百的方形区域内遍历所有特征点，并计算他们描述子之间的汉明距离（二进制之间的距离）。最优距离小于50，计算最优距离和次优距离之间的比值。之后统计原特征点和匹配到的特征点之间的角度（每30为一个范围列直方图），判断特征点最多的三个方向。此时第二多的特征数量＜0.1最多的方向，则证明第一方向为主方向。若第三多的方向＜0.1最多的方向，则证明第一个第二多的方向为主方向。
SearchByBoW 2种
每个节点中包含很多帧 ，对每个节点中对应的特征点一一匹配
SearchByProjection 4种
利用EPnP计算位姿进行motion-onl-BA优化，如果内点不足则投影匹配
computeSim3 将回环关键帧的共视关系投影到回环关键帧中搜索匹配点，匹配点足够多，则说明回环成功
TrackWithModel
三帧中，某点从第一帧到第二帧之间的变换为T，则假设第二帧到第三帧的变换为T’=T，在第三帧点的区域附近进项匹配，找到汉明距离最小的点，若小于设置的阈值，则匹配成功；若没找到。则提高阈值重试
SearchForTriangulation
跟踪线程中没有跟踪到的特征点在参考关键帧中进行三角测距
Fuse
回环之后将位姿和地图点矫正
SearchBySim3
高斯图像金字塔：图像处理中的高斯金字塔和拉普拉斯金字塔_熊彬程的博客的博客-CSDN博客_高斯金字塔的作用
- 高斯金字塔(Gaussianpyramid): 用来向下采样，主要的图像金字塔
- 拉普拉斯金字塔(Laplacianpyramid): 用来从金字塔低层图像重建上层未采样图像，在数字图像处理中也即是预测残差，可以对图像进行最大程度的还原，配合高斯金字塔一起使用。
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
</int>
首先计算特征点的方向，使用了几何中心和灰度质心的连线方向
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
</uchar>
灰度值指针
定义 m_01为的加权，m_10为的加权，首先得到特征点所在图像块的灰度值指针center，除对称轴外每次可以计算(x,y)(x,-y),计算上方灰度值。轴上坐标加权m_10 += u * (val_plus + val_minus);在这一行上的和按照坐标加权。其中使用了fastAtan
return fastAtan2((float)m_01, (float)m_10);
计算描述子的角度
static void computeOrbDescriptor(const KeyPoint& kpt,
const Mat& img, const Point* pattern,
uchar* desc)
const float factorPI = (float)(CV_PI/180.f);//一度对应的弧度大小
const float factorPI = (float)(CV_PI/180.f);//得到KPT特征点的对应角度
for (int i = 0; i < 32; ++i, pattern += 16)
{
int t0,
t1,
val;
t0 = GET_VALUE(0); t1 = GET_VALUE(1);
val = t0 < t1; //描述子本字节的bit0
t0 = GET_VALUE(2); t1 = GET_VALUE(3);
val |= (t0 < t1) << 1; //描述子本字节的bit1
t0 = GET_VALUE(4); t1 = GET_VALUE(5);
val |= (t0 < t1) << 2; //描述子本字节的bit2
t0 = GET_VALUE(6); t1 = GET_VALUE(7);
val |= (t0 < t1) << 3; //描述子本字节的bit3
t0 = GET_VALUE(8); t1 = GET_VALUE(9);
val |= (t0 < t1) << 4; //描述子本字节的bit4
t0 = GET_VALUE(10); t1 = GET_VALUE(11);
val |= (t0 < t1) << 5; //描述子本字节的bit5
t0 = GET_VALUE(12); t1 = GET_VALUE(13);
val |= (t0 < t1) << 6; //描述子本字节的bit6
t0 = GET_VALUE(14); t1 = GET_VALUE(15);
val |= (t0 < t1) << 7; //描述子本字节的bit7
desc[i] = (uchar)val;
每个描述子是一个二进制字符，有两个灰度值比较得来，一共比32次，每次比较除8bit信息 ，所以是256位比特。使用预先定义好的随机点集
for(int i=1; i<nlevels; i++) { mvscalefactor[i]="mvScaleFactor[i-1]*scaleFactor;" mvlevelsigma2[i]="mvScaleFactor[i]*mvScaleFactor[i];" }< code></nlevels;>
resize CV13 图像分辨率操作（图像金字塔与resize()函数）_什么都只会一点的博客-CSDN博客
计算每层缩放系数的平方，其中nLevels:ORBextractor.nLevels: 8写死在yaml中，又用mnScaleLevels = mpORBextractorLeft->GetLevels();在Frame.cc中得到，在ORBextractor中返回。 可以发现金字塔中第0层的尺度因子是1,然后每向上高一层，图像的尺度因子是在上一层图像的尺度因子 1 1*1.2 1*1.2*1.2 1*1.2*1.2*1.2 ...
loat nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 (float)pow((double)factor, (double)nlevels)); //每个单位缩放系数所希望的特征点个数
for( int level = 0; level < nlevels-1; level++ )
{
//分配 cvRound : 返回个参数最接近的整数值
mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
//累计
sumFeatures += mnFeaturesPerLevel[level];
//乘系数
nDesiredFeaturesPerScale *= factor;
}计算除了最顶层外每层系数，每层根据采样缩放系数的倒数得到期望个数。希望个数减去每层个数和剩下的分配到最高层
计算四分之一愿的边界，利用对称性得到边界
void ExtractorNode::DivideNode
将特征区域分为左上左下右上右下四块。存储边界点并将特征点放入对应的块中
const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
//特征区域取得中间坐标将区域分成四块，
n1.UL = UL;
n1.UR = cv::Point2i(UL.x+halfX,UL.y);
n1.BL = cv::Point2i(UL.x,UL.y+halfY);
n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);//将四块坐标存储
for(size_t i=0;i<vkeys.size();i++) 遍历特征点 const cv::keypoint &kp="vKeys[i];" if(kp.pt.x<n1.ur.x) { if(kp.pt.y<n1.br.y) n1.vkeys.push_back(kp); else n3.vkeys.push_back(kp); } n2.vkeys.push_back(kp); n4.vkeys.push_back(kp);将当前提取其中的特征点放入各自容器中< code></vkeys.size();i++)></float></float>
ORBextractor::DistributeOctTree
使用四叉树对一个金字塔图层中的特征点进行特征分发。
1.计算宽高、长宽比。resize提取器指针。
2.生成提取器节点
ni.UL = cv::Point2i(hX*static_cast<float>(i),0); //UpLeft
ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0); //UpRight
ni.BL = cv::Point2i(ni.UL.x,maxY-minY); //BottomLeft
ni.BR = cv::Point2i(ni.UR.x,maxY-minY); //BottomRight
</float></float>
// S 将特征点分配到子提取器节点中
for(size_t i=0;i<vtodistributekeys.size();i++) 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点 list<extractornode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())</vtodistributekeys.size();i++)>
为提取器的区域分配一个提取器指针，并分配每个区域特征点期望个数。如果个数为1 ，则标志标志位表述不可再分，个数为0则删除。
vector<pair<int,extractornode*> > vSizeAndPointerToNode;//记录了可以继续分裂的节点</pair<int,extractornode*>
将一个初始化节点分为4个。利用四叉树划分区域。然后继续遍历提取器，如果提取器只有一个特征点，则list++，else则分成四个子区域。如果四个子区域中特征点数>0，则把这个子区域的提取器添加到列表前面。添加这个子区域中特征点的数目和节点指针vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));之后再循环分配器、提取器。分裂方式是后加的先分裂，先加的后分裂。
if((int)lNodes.size()>=N|| (int)lNodes.size()==prevSize)
如果当前特征点个数达到了阈值或每个区域已经不能再分则停止
else if(((int)lNodes.size()+nToExpand*3)>N)如果当前特征点和将要展开的点已经超过所要个数。，则想办法使其在达到或者刚超过所需的特征点数量时退出。一直循环到标志位被标志
此时我们使用pair将可以分裂的数量保存后用sort进行排序 ，从后往前遍历这个使用pair的vector对每个点进行分裂。pair的p2位待分裂节点、给到DivideNode（n1、n2、 3、 4）,将需要分裂的点放到前面，并且如果大于1，则放入lnode的前面
vector<cv::keypoint> vResultKeys;//保存将兴趣点
for(list<extractornode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)//遍历</extractornode></cv::keypoint>
for(size_t k=1;k<vnodekeys.size();k++) 遍历容器中的特征点，得到第一个点的最大响应值作为后续的最大响应值 if(vnodekeys[k].response>maxResponse)
若最大响应值大于第一个最大响应值则更新最大响应值
vResultKeys.push_back(*pKP);
将这个区域的最大响应值加入最终结果容器
</vnodekeys.size();k++)>
首先计算图像坐标边界，存储需要进行分配的点和特增点个数。开始遍历，FAST提取FAST角点
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
if(vKeysCell.empty())
{
//那么就使用更低的阈值来进行重新检测
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像
for(vector<cv::keypoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
{
//NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标
//这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标
//在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标
(*vit).pt.x+=j*wCell;
(*vit).pt.y+=i*hCell;</cv::keypoint>
cv::point Opencv KeyPoint类_DXT00的博客-CSDN博客
ORBextractor::ComputeKeyPointsOld/
使用老办法来得到特征点
计算cell个数，保存cell特征点数、位置、是否只有一个特征点等。之后遍历网格。如果cell在第一行则计算初始边界，如果在最后一个则计算增量坐标（有几个像素用几个，如果太小则舍弃），
Mat cellImage = mvImagePyramid[level].rowRange(iniY,iniY+hY).colRange(iniX,iniX+hX);
cellimage给他扣出来，第几层.rosrange.lowrange
如果提取出的特征点小于3个，清空后降低阈值重新提取。如果满足，则保存特征点数目
根据评分使特征点分布均匀。根据响应值保留符合要求的特征点，并且进行坐标的转换
KeyPointsFilter::retainBest(keysCell, //输入输出，用于提供待保留的特征点vector，操作完成后将保留的特征点存放在里面
//其实就是把那些响应值低的点从vector中给删除了
nToRetain[i][j]); //指定要保留的特征点数目
for(size_t k=0, kend=keysCell.size(); k<kend; k++) 坐标变换< code></kend;>
ORBextractor::operator()计算特征点
1.检查是否非空
2，构建图像金字塔ComputePyramid(image);
3.可以使用四叉树或者传统方法进行特征点计算，使特征点均匀化。
4.深拷贝描述子到新矩阵descriptors
4.1统计金字塔每层特征点个数并累加求和。
4.2存储图像金字塔中描述子的矩阵
5.使用高斯模糊避免噪声影响描述子计算
6.计算高斯模糊之后的描述子
7.对高斯模糊后更层上的描述子恢复到原本的图像金字塔坐标上
void ORBextractor::ComputePyramid(cv::Mat image)
MapPoint
‘这部分函数命名很舒服，基本看下名称就知道干啥的，就是变量名有点烦
MapPoint::SetWorldPos 设置地图点坐标
cv::Mat MapPoint::GetWorldPos()返回世界坐标
cv::Mat MapPoint::GetNormal()获取平均观测方向
void MapPoint::AddObservation添加观测点
std::map
void MapPoint::SetBadFlag()先标记后删除，遍历mObservations，标记后直接擦除内存mpMap->EraseMapPoint(this);
void MapPoint::Replace(MapPoint* pMP)替换地图点
同一个点跳过 ，pMP用来替换的那个地图点 ，观测到的点的指针给到obs并清除原有观测
map<keyframe*,tuple<int,int>> obs;
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
obs=mObservations;
for(map<keyframe*,tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
将原有能观测到原来地图点的关键帧复制到要替换的地图点上
不在则替换 if(!pMP->IsInKeyFrame(pKF))
{
// 如果不在，替换特征点与mp的匹配关系
if(leftIndex != -1){
pKF->ReplaceMapPointMatch(leftIndex, pMP);
pMP->AddObservation(pKF,leftIndex);
}
if(rightIndex != -1){
pKF->ReplaceMapPointMatch(rightIndex, pMP);
pMP->AddObservation(pKF,rightIndex);
}
如果在 删除旧的 新的不动
更新原有 可观测此点的帧数 可视点（非特征点和地图点 能够看到的点） 描述子</keyframe*,tuple<int,int></mutex></mutex></keyframe*,tuple<int,int>
void MapPoint::ComputeDistinctiveDescriptors()
- 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子。先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值
1.跳过坏点、
2.遍历点获取描述子放入vDESCRIPTORS
.3.遍历点取得距离给distij
int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
- 选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值，距离中值为INT_MAX
vector<int> vDists(Distances[i],Distances[i]+N);
sort(vDists.begin(),vDists.end());
// 获得中值
int median = vDists[0.5*(N-1)];
// 寻找最小的中值
if(median<bestmedian) { bestmedian="median;" bestidx="i;" }< code></bestmedian)></int>
返回描述子，确定是否在关键帧中并得到这个点在关键帧中对应的id
void MapPoint::UpdateNormalAndDepth()
更新平均观测方向以及观测距离范围。由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要更新相应变量。创建新的关键帧的时候会调用。
1.获得该地图点的相关信息
observations=mObservations; // 获得观测到该地图点的所有关键帧
pRefKF=mpRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧）
Pos = mWorldPos.clone(); // 地图点在世界坐标系中的位置
2.能观测到此点的所有观测方向归一化。得到该点的朝向
主要是用该点的相机坐标减去其世界坐标，normal为法线。首先得到法线。然后计算改点到参考关键帧相机的距离得到这个点的观测距离上下限和地图平均观测方向
float MapPoint::GetMaxDistanceInvariance()
这里没太看懂
// 在进行投影匹配的时候会给定特征点的搜索范围,考虑到处于不同尺度(也就是距离相机远近,位于图像金字塔中不同图层)的特征点受到相机旋转的影响不同,
// 因此会希望距离相机近的点的搜索范围更大一点,距离相机更远的点的搜索范围更小一点,所以要在这里,根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中,
// 会大概处于哪个尺度
int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF)
{
float ratio;
{
unique_lock<mutex> lock(mMutexPos);
// mfMaxDistance = ref_dist*levelScaleFactor 为参考帧考虑上尺度后的距离
// ratio = mfMaxDistance/currentDist = ref_dist/cur_dist
ratio = mfMaxDistance/currentDist;
}
// 同时取log线性化
int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
if(nScale<0) nscale="0;" else if(nscale>=pKF->mnScaleLevels)
nScale = pKF->mnScaleLevels-1;
return nScale;
}
/**
* @brief 根据地图点到光心的距离来预测一个类似特征金字塔的尺度
*
* @param[in] currentDist 地图点到光心的距离
* @param[in] pF 当前帧
* @return int 尺度
*/
int MapPoint::PredictScale(const float ¤tDist, Frame* pF)
{
float ratio;
{
unique_lock<mutex> lock(mMutexPos);
ratio = mfMaxDistance/currentDist;
}
int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
if(nScale<0) nscale="0;" else if(nscale>=pF->mnScaleLevels)
nScale = pF->mnScaleLevels-1;
return nScale;
}
Map* MapPoint::GetMap()
{
unique_lock<mutex> lock(mMutexMap);
return mpMap;
}
void MapPoint::UpdateMap(Map* pMap)
{
unique_lock<mutex> lock(mMutexMap);
mpMap = pMap;
}
} //namespace ORB_SLAM
</mutex></mutex></0)></mutex></0)></mutex>
Frame.cc
麻了呀写完了都，结果发布之后就没了
Frame::Frame
构造frame
for(int i=0;i
Original: https://blog.csdn.net/qq_39533374/article/details/123709139
Author: 菜饭骨头汤无锡总代理（销售王子臧）
Title: ORB-SLAM3笔记（编译、踩坑、论文、看代码）
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/637633/
转载文章受原作者版权保护。转载请注明原作者出处！