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]; }
icoord TileGrid::virt2phys(rcoord virt) const noexcept { return icoord{static_cast<int>(virt.x) / tileDim.x, static_cast<int>(virt.y) / tileDim.y, depthIndex(virt.z)}; }
icoord TileGrid::virt2phys(vicoord virt) const noexcept { return icoord{static_cast<int>(virt.x), static_cast<int>(virt.y), depthIndex(virt.z)}; }