| 702 | |
| 703 | |
| 704 | vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r, const bool bRight) const |
| 705 | { |
| 706 | vector<size_t> vIndices; |
| 707 | vIndices.reserve(N); |
| 708 | |
| 709 | float factorX = r; |
| 710 | float factorY = r; |
| 711 | |
| 712 | const int nMinCellX = max(0,(int)floor((x-mnMinX-factorX)*mfGridElementWidthInv)); |
| 713 | if(nMinCellX>=mnGridCols) |
| 714 | return vIndices; |
| 715 | |
| 716 | const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+factorX)*mfGridElementWidthInv)); |
| 717 | if(nMaxCellX<0) |
| 718 | return vIndices; |
| 719 | |
| 720 | const int nMinCellY = max(0,(int)floor((y-mnMinY-factorY)*mfGridElementHeightInv)); |
| 721 | if(nMinCellY>=mnGridRows) |
| 722 | return vIndices; |
| 723 | |
| 724 | const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+factorY)*mfGridElementHeightInv)); |
| 725 | if(nMaxCellY<0) |
| 726 | return vIndices; |
| 727 | |
| 728 | for(int ix = nMinCellX; ix<=nMaxCellX; ix++) |
| 729 | { |
| 730 | for(int iy = nMinCellY; iy<=nMaxCellY; iy++) |
| 731 | { |
| 732 | const vector<size_t> vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy]; |
| 733 | for(size_t j=0, jend=vCell.size(); j<jend; j++) |
| 734 | { |
| 735 | const cv::KeyPoint &kpUn = (NLeft == -1) ? mvKeysUn[vCell[j]] |
| 736 | : (!bRight) ? mvKeys[vCell[j]] |
| 737 | : mvKeysRight[vCell[j]]; |
| 738 | const float distx = kpUn.pt.x-x; |
| 739 | const float disty = kpUn.pt.y-y; |
| 740 | |
| 741 | if(fabs(distx)<r && fabs(disty)<r) |
| 742 | vIndices.push_back(vCell[j]); |
| 743 | } |
| 744 | } |
| 745 | } |
| 746 | |
| 747 | return vIndices; |
| 748 | } |
| 749 | |
| 750 | bool KeyFrame::IsInImage(const float &x, const float &y) const |
| 751 | { |