| 256 | |
| 257 | template <typename T> |
| 258 | std::vector<ObjectMatch<T>> sloam::matchFeatures(const SE3 tf, const std::vector<T> &currObjects, const std::vector<T> &mapObjects, const Scalar distThresh) |
| 259 | { |
| 260 | std::vector<ObjectMatch<T>> matches; |
| 261 | for (const auto &co : currObjects) |
| 262 | { |
| 263 | T proj_obj = co; |
| 264 | proj_obj.project(tf); |
| 265 | // find closest model in map |
| 266 | Scalar bestDist = distThresh + 100; |
| 267 | T bestObject = mapObjects[0]; |
| 268 | for (const auto &mo : mapObjects) |
| 269 | { |
| 270 | Scalar d = mo.distance(proj_obj.getModel()); |
| 271 | if (d < bestDist) |
| 272 | { |
| 273 | bestDist = d; |
| 274 | bestObject = mo; |
| 275 | } |
| 276 | } |
| 277 | // create feature matches according to model association |
| 278 | if (bestDist < distThresh) |
| 279 | { |
| 280 | addFeatureMatches(co.features, bestObject, bestDist, matches); |
| 281 | } |
| 282 | } |
| 283 | |
| 284 | return matches; |
| 285 | } |
| 286 | |
| 287 | template <typename T> |
| 288 | void sloam::addFeatureMatches(const VectorType &features, const T &object, const double dist, std::vector<ObjectMatch<T>> &matches) |