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];
}
