ORB-SLAM(五)KeyFrame类
KeyFrame类利用Frame类来构造。对于什么样的Frame可以认为是关键帧以及何时需要加入关键帧,是实现在tracking模块中的。
由于KeyFrame中一部分数据会被多个线程访问修改,因此需要在这些成员中加线程锁,保证同一时间只有一个线程有访问权。涉及线程安全的有:
关键帧位姿的设置(lock(mMutexPose));
关键帧间连接关系的设置(lock(mMutexConnections));
关键帧对应地图点的操作(lock(mMutexFeatures)),包括通过地图点计算相连关键帧之间的权重。
1. 设置相机位姿参数:设置KeyFrame中成员变量mTcw,mTwc,Ow(左目相机中心坐标),和Cw(双目相机baseline中点坐标),相机坐标Z朝北,X朝东,Y朝地。
并给出了get函数获取姿态参数。
注意
- 这里的Ow等价于twc,表示当前相机光心在世界坐标系下的三维坐标
- Tcw直接求逆计算量比较大,一般矩阵求逆在实现时都会用等价的矩阵表达式去表示,这里Ow就对应Tcw-1中的平移向量-RTt.
void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
unique_lock<mutex> lock(mMutexPose);
Tcw_.copyTo(Tcw);
cv::Mat Rcw = Tcw.rowRange(,).colRange(,);
cv::Mat tcw = Tcw.rowRange(,).col();
cv::Mat Rwc = Rcw.t();
Ow = -Rwc*tcw; Twc = cv::Mat::eye(,,Tcw.type());
Rwc.copyTo(Twc.rowRange(,).colRange(,));
Ow.copyTo(Twc.rowRange(,).col());
cv::Mat center = (cv::Mat_<float>(,) << mHalfBaseline, , , );
Cw = Twc*center;
}
2. 为关键帧之间添加连接,通过关键帧之间的weight连接,weight指的是两个关键帧之间共同观测到的地图点:(注意这里都是接口函数,真实的建立连接关系使用的是void KeyFrame::UpdateConnections()函数)
使用的数据结构是:
std::map<KeyFrame*,int> mConnectedKeyFrameWeights
每一个关键帧都会维护一个自己的map,其中记录了与其他关键帧之间的weight。每次为当前关键帧添加新的连接关键帧后,都需要根据weight对map结构重新排序,
UpdateBestCovisibles();
并更新这两个向量:
mvpOrderedConnectedKeyFrames
mvOrderedWeights
由于map结构没有sort函数,需要将元素取出放入一个pair组成的vector中,排序后放入上面这两个向量中
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(mConnectedKeyFrameWeights.size());
for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
vPairs.push_back(make_pair(mit->second,mit->first)); sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs; // keyframe
list<int> lWs; // weight
for(size_t i=, iend=vPairs.size(); i<iend;i++)
{
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
} mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
还有几个相关的API:
set<KeyFrame*> KeyFrame::GetConnectedKeyFrames();
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames(); vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N);
vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w); int KeyFrame::GetWeight(KeyFrame *pKF);
都是返回连接的关键帧;
前两个返回所有连接的关键帧,区别在于一个未排序(set),一个排序(vector)。// set是关联容器 vector是顺序容器
中间两个返回满足一定阈值(前N个,权重大于等于w)的关键帧,最后一个返回指定帧与当前帧间的权重。
3. 当前帧对应的地图点的指针均存放在mvpMapPoints(mvp代表:member、vector、pointer)向量中,通过对mvpMapPoints操作封装,可以得到以下API:
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx);
void KeyFrame::EraseMapPointMatch(const size_t &idx);
void KeyFrame::EraseMapPointMatch(MapPoint* pMP);
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP); // 注意区别下面两个
set<MapPoint*> KeyFrame::GetMapPoints();
vector<MapPoint*> KeyFrame::GetMapPointMatches();
MapPoint* KeyFrame::GetMapPoint(const size_t &idx); // 返回高质量MapPoints(被至少minObs个关键帧观察到)的数量,其中会判断MapPoint的Observations()属性,对比给出的阈值
int KeyFrame::TrackedMapPoints(const int &minObs);
mvpMapPoints初始化在Frame.cpp中:
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
其中有N个空指针,因此有的位置上的MapPoint并没有指向实际的地图点(虽然对应有特征点,有索引idx,但是是外点),获取时需要注意。
4. UpdateConnections()函数:建立关键帧之间的连接关系
最新文章
- java 反编译
- goldengate 12c 12.2 新特性(updated)
- 【转】C#环形队列
- 01.C#数据类型、排序、过滤(一章1.1-1.2)
- CodeSmith连接Mysql配置
- [POJ] #1002# 487-3279 : 桶排序/字典树(Trie树)/快速排序
- [Unity3D+算法]一小时做个2048
- Linux&#160;下查看文件字符编码和转换编码
- 发送邮件(遵循smtp协议即简单的邮件发送协议)
- crawler_Docker_解决用 JavaScript 框架开发的 Web 站点抓取
- 家庭记账本小程序之java代码部分(java web基础版二)
- mysql+高可用MMM
- 二、xadmin----简单使用
- 项目中 2个或者多个EF模型 表名称相同会导致生成的实体类 覆盖的解决方法
- Win10+Ubuntu18.04 UEFI启动模式SSD+HDD
- Java学习笔记31(IO:Properties类)
- 【MySQL】 Cannot load from mysql.proc. The table is probably corrupted
- 使用promise方式来获取网络数据
- RenderScript多输入参数
- windows下使用GNU make命令报错的解决方法
热门文章
- Android开发经验01:31个Android开发实战经验
- HASH JION AND NESTED JION
- gluon实现softmax分类FashionMNIST
- [19/03/16-星期六] 常用类_Date时间类&;DateFormat类
- Zookeep启动异常:Error contacting service. It is probably not running.
- Oracle数据库用户密码设为无限期
- c#分析SQL语句
- Navicat for Mysql中错误提示索引过长1071-max key length is 767 byte
- JS apply 和 call 的实现
- oracle-sql优化-通过分组和缓存减少不必要的读