| 51 | } |
| 52 | |
| 53 | KeyPointIterator Led::nearest(KeyPointList &keypoints, |
| 54 | double threshold) const { |
| 55 | // If we have no elements in the vector, return the end(). |
| 56 | if (keypoints.empty()) { |
| 57 | return end(keypoints); |
| 58 | } |
| 59 | |
| 60 | auto location = m_location; |
| 61 | /// @todo can we use squared-norm instead of doing a square-root inside |
| 62 | /// of a tight loop like this? |
| 63 | auto computeDist = [location](KeyPointIterator it) { |
| 64 | return sqrt(norm(location - it->pt)); |
| 65 | }; |
| 66 | |
| 67 | // Find the distance to the first point and record it as the |
| 68 | // current minimum distance; |
| 69 | auto ret = begin(keypoints); |
| 70 | auto minDist = computeDist(ret); |
| 71 | |
| 72 | // Search the rest of the elements to see if we can find a |
| 73 | // better one. |
| 74 | for (auto it = begin(keypoints), e = end(keypoints); it != e; ++it) { |
| 75 | auto dist = computeDist(it); |
| 76 | if (dist < minDist) { |
| 77 | minDist = dist; |
| 78 | ret = it; |
| 79 | } |
| 80 | } |
| 81 | |
| 82 | // If the closest is within the threshold, return it. Otherwise, |
| 83 | // return the end. |
| 84 | if (minDist <= threshold) { |
| 85 | return ret; |
| 86 | } |
| 87 | return end(keypoints); |
| 88 | } |
| 89 | |
| 90 | } // End namespace vbtracker |
| 91 | } // End namespace osvr |