本文整理汇总了C++中MapPoint::GetIndexInKeyFrame方法的典型用法代码示例。如果您正苦于以下问题：C++ MapPoint::GetIndexInKeyFrame方法的具体用法？C++ MapPoint::GetIndexInKeyFrame怎么用？C++ MapPoint::GetIndexInKeyFrame使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MapPoint的用法示例。
在下文中一共展示了MapPoint::GetIndexInKeyFrame方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: ComputeSceneMedianDepthWithCoord
float KeyFrame::ComputeSceneMedianDepthWithCoord(int &x, int &y, int q) {
//vector<MapPoint*> vpMapPoints;
cv::Mat Tcw_;
{
boost::mutex::scoped_lock lock(mMutexFeatures);
boost::mutex::scoped_lock lock2(mMutexPose);
//vpMapPoints = mvpMapPoints;
Tcw_ = Tcw.clone();
}
vector<float> vDepths;
vDepths.reserve(mvpMapPoints.size());
cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
Rcw2 = Rcw2.t();
float zcw = Tcw_.at<float>(2,3);
for(size_t i=0; i<mvpMapPoints.size(); i++)
{
if(mvpMapPoints[i])
{
MapPoint* pMP = mvpMapPoints[i];
cv::Mat x3Dw = pMP->GetWorldPos();
float z = Rcw2.dot(x3Dw)+zcw;
vDepths.push_back(z);
}
}
vector<size_t> depthIndex(vDepths.size());
for (size_t i = 0; i != depthIndex.size(); ++i) depthIndex[i] = i;
sort(vDepths.begin(),vDepths.end());
sort(depthIndex.begin(), depthIndex.end(),
[&vDepths](size_t i1, size_t i2) {return vDepths[i1] < vDepths[i2];});
size_t medianIndex = depthIndex[(vDepths.size()-1)/q];
size_t ptCounter = 0;
MapPoint* pointWithMedianDepth;
for (size_t i = 0; i<mvpMapPoints.size(); i++) {
if (mvpMapPoints[i]) {
if (ptCounter == medianIndex) {
pointWithMedianDepth = mvpMapPoints[i];
break;
}
ptCounter++;
}
}
if (pointWithMedianDepth) {
size_t keysUnIdx = pointWithMedianDepth->GetIndexInKeyFrame(this);
x = mvKeysUn[keysUnIdx].pt.x;
y = mvKeysUn[keysUnIdx].pt.y;
}
return vDepths[(vDepths.size()-1)/q];
}