Changeset 63:f4f3640339c6 in rrlib_model_fitting


Ignore:
Timestamp:
05.05.2017 16:58:29 (4 years ago)
Author:
Tobias Föhst <tobias.foehst@…>
Branch:
17.03
Phase:
public
Message:

Improves performance of ICP by replacing sort by quickselect to find median

Files:
2 edited

Legend:

Unmodified
Added
Removed
  • tIterativeClosestPoint.h

    r60 r63  
    123123   * \param sufficient_improvement_threshold   If the errors of two successive iterations are lower the algorithm terminates 
    124124   * \param max_iterations                     Maximum number of iterations until the algorithm terminates without further convergence 
    125    */ 
    126   void DoICP(double sufficient_improvement_threshold = 1E-10, unsigned int max_iterations = 500); 
     125   * 
     126   * \returns false when input data was not suitable for running ICP, true otherwise indicating success 
     127   */ 
     128  bool DoICP(double sufficient_improvement_threshold = 1E-10, unsigned int max_iterations = 500); 
    127129 
    128130  /*! Access the originally stored model samples 
  • tIterativeClosestPoint.hpp

    r60 r63  
    373373  std::vector<std::pair<size_t, size_t>> filtered_correspondence_pairs; 
    374374  std::vector<double> filtered_distances; 
    375   auto sorted_distances = distances; 
    376   std::sort(sorted_distances.begin(), sorted_distances.end()); 
    377   const auto median = sorted_distances[sorted_distances.size() / 2]; 
     375  std::vector<size_t> index(distances.size()); 
    378376  for (size_t i = 0; i < distances.size(); ++i) 
    379377  { 
    380     if (distances[i] <= Tdimension * median) 
    381     { 
    382       filtered_correspondence_pairs.emplace_back(correspondence_pairs[i]); 
     378    index[i] = i; 
     379  } 
     380  auto median = index.begin() + index.size() / 2; 
     381  std::nth_element(index.begin(), median, index.end(), [&distances](size_t a, size_t b) 
     382  { 
     383    return distances[a] < distances[b]; 
     384  }); 
     385  for (size_t i = 0; i < distances.size(); ++i) 
     386  { 
     387    if (distances[i] <= Tdimension * distances[*median]) 
     388    { 
     389      filtered_correspondence_pairs.emplace_back(std::move(this->correspondence_pairs[i])); 
    383390      filtered_distances.emplace_back(distances[i]); 
    384391    } 
    385392  } 
    386   correspondence_pairs = std::move(filtered_correspondence_pairs); 
     393  this->correspondence_pairs = std::move(filtered_correspondence_pairs); 
    387394  distances = std::move(filtered_distances); 
    388395} 
     
    406413  using tMatrix = math::tMatrix<Tdimension, Tdimension>; 
    407414  tMatrix h; 
    408   for (const auto & c : correspondence_pairs) 
     415  for (const auto & c : this->correspondence_pairs) 
    409416  { 
    410417    h += tMatrix(model[c.first] + translation, this->data[c.second]); 
Note: See TracChangeset for help on using the changeset viewer.