| 297 | |
| 298 | template <typename T> |
| 299 | void sloam::matchModels(const std::vector<T> &currObjects, const std::vector<T> &mapObjects, std::vector<int> &matchIndices) |
| 300 | { |
| 301 | |
| 302 | size_t treeCounter = 0; |
| 303 | for (const auto &co : currObjects) |
| 304 | { |
| 305 | // find closest model in map |
| 306 | Scalar bestDist = fmParams_.treeMatchThresh + 100; |
| 307 | T bestObject = mapObjects[0]; |
| 308 | size_t bestKey = 0; |
| 309 | size_t key = 0; |
| 310 | for (const auto &mo : mapObjects) |
| 311 | { |
| 312 | Scalar d = mo.distance(co.getModel()); |
| 313 | if (d < bestDist) |
| 314 | { |
| 315 | bestDist = d; |
| 316 | bestObject = mo; |
| 317 | bestKey = key; |
| 318 | } |
| 319 | ++key; |
| 320 | } |
| 321 | // create feature matches according to model association |
| 322 | if (bestDist < fmParams_.AddNewTreeThreshDist) |
| 323 | { |
| 324 | matchIndices[treeCounter] = bestKey; |
| 325 | } |
| 326 | treeCounter++; |
| 327 | } |
| 328 | } |
| 329 | |
| 330 | void sloam::binGroundPoints(const SE3 pose, const VectorType &points, boost::multi_array<VectorType, 2> &scgf) |
| 331 | { |