resampler.cpp
Go to the documentation of this file.
1 
18 //Global includes
19 #include <list>
20 #include <map>
21 #include <utility>
22 #include <algorithm>
23 
24 //Pkg includes
25 #include <ros/ros.h>
26 
27 //Local includes
29 
30 using namespace std;
31 using namespace ISM;
32 
34 {
35 
36 Resampler::Resampler(bool equal_results_number, unsigned int importance_resampled_size, SharedRecognitionResultsManagerPtr shared_recognition_results_ptr):
37  EQUAL_RESULTS_NUMBER_(equal_results_number),
38  IMPORTANCE_RESAMPLED_SIZE_(importance_resampled_size),
39  shared_recognition_results_manager_ptr_(shared_recognition_results_ptr)
40 {
41  ROS_DEBUG_STREAM_NAMED("Resampler", "Resampler::Resampler() is called.");
42 }
43 
44 vector<RecognitionResult> Resampler::drawSamples()
45 {
46 
47  ROS_INFO("\n--------------------------------------------------\n");
48  ROS_INFO("Resampler::drawSamples() is called.");
49 
50  const unsigned int SCENE_RECOGNITION_RESULT_NUMBER = shared_recognition_results_manager_ptr_->getResultsNumber();
51  ROS_INFO_STREAM("Recognition results in shared memory before sampling: " << SCENE_RECOGNITION_RESULT_NUMBER);
52 
53  vector<RecognitionResultPtr> allResults = shared_recognition_results_manager_ptr_->getResults();
54 
55  map<string, unsigned int>::iterator resultIt;
56 
57  //TODO: Extract to own method
58 
59  //Count number of results in shared memory per scene
60  map<string, unsigned int> patternToResultsNumber;
61 
62  for(RecognitionResultPtr result : allResults) {
63 
64  resultIt = patternToResultsNumber.find(result->patternName);
65 
66  if(resultIt != patternToResultsNumber.end())
67  resultIt->second++;
68  else
69  patternToResultsNumber.insert(make_pair(result->patternName, 1));
70 
71  }
72 
73  for(pair<const string, unsigned int>& pair : patternToResultsNumber)
74  ROS_INFO_STREAM(pair.second << " recognition results in shared memory for scene: " << pair.first);
75 
76  //TODO - End: Extract to own method
77 
78  //TODO: Extract to own helper method (without sum_of_confidence)
79 
80  //We assume that recognition results are sorted by their confidence in shared memory.
81  list<RecognitionResult> results_for_sampling;
82  list<RecognitionResult> surplus_results;
83  double sum_of_confidence = 0.0;
84 
86  //Set mapping from present scenes to number of their results in shared memory to minimum number of results per scene.
87  auto minIt = min_element(patternToResultsNumber.begin(), patternToResultsNumber.end(),
88  [](decltype(patternToResultsNumber)::value_type& l, decltype(patternToResultsNumber)::value_type& r)
89  -> bool { return l.second < r.second; });
90 
91  ROS_INFO_STREAM(minIt->second << " results per scene are considered as input for importance sampling.");
92  ROS_INFO("The following results are considered as input for importance sampling: ");
93 
94  for(resultIt = patternToResultsNumber.begin(); resultIt != patternToResultsNumber.end(); resultIt++)
95  resultIt->second = minIt->second;
96 
97  unsigned int num_results_for_sampling = minIt->second * patternToResultsNumber.size();
98 
99  while(results_for_sampling.size() < num_results_for_sampling) {
100 
101  RecognitionResult recognition_result = shared_recognition_results_manager_ptr_->getLastResult();
102 
103  if(patternToResultsNumber.at(recognition_result.patternName)){
104  ROS_DEBUG_STREAM(recognition_result);
105  sum_of_confidence += recognition_result.confidence;
106  results_for_sampling.push_back(recognition_result);
107  patternToResultsNumber.at(recognition_result.patternName)--;
108  }
109  else{
110  surplus_results.push_back(recognition_result);
111  }
113 
114  }
115 
116  }
117  //Otherwise all results from shared mem are taken at once.
118  else {
119  for (unsigned int i = 0; i < SCENE_RECOGNITION_RESULT_NUMBER; ++i)
120  {
121  RecognitionResult recognition_result = shared_recognition_results_manager_ptr_->getLastResult();
123  sum_of_confidence += recognition_result.confidence;
124  results_for_sampling.push_back(recognition_result);
125  }
126  ROS_INFO("All results are considered as input for importance sampling.");
127  }
128 
129  const int ACTUAL_SAMPLE_SIZE = results_for_sampling.size();
130 
131  //TODO - End: Extract to own method
132 
133  ROS_ASSERT(SCENE_RECOGNITION_RESULT_NUMBER == results_for_sampling.size() + surplus_results.size() + shared_recognition_results_manager_ptr_->getResultsNumber());
134 
135  //TODO: Extract to own helper method
136  double accumulated_confidence_per_result[ACTUAL_SAMPLE_SIZE];
137  list<RecognitionResult>::iterator results_for_sampling_it = results_for_sampling.begin();
138  //Calculate accumulated confidences for following importance sampling.
139  accumulated_confidence_per_result[0] = results_for_sampling_it->confidence / sum_of_confidence;
140  for (int i = 1; i < ACTUAL_SAMPLE_SIZE; ++i)
141  {
142  results_for_sampling_it++;
143  RecognitionResult recognition_result = (*results_for_sampling_it);
144  accumulated_confidence_per_result[i] =
145  (recognition_result.confidence / sum_of_confidence) +
146  accumulated_confidence_per_result[i - 1];
147  }
148  if (accumulated_confidence_per_result[ACTUAL_SAMPLE_SIZE - 1] != 1.0)
149  accumulated_confidence_per_result[ACTUAL_SAMPLE_SIZE - 1] = 1.0;
150 
151  ROS_INFO_STREAM("These are the recognition results from importance sampling.");
152 
153  vector<RecognitionResult> resampled_results;
154  vector<list<RecognitionResult>::iterator> resultsToBeDeleted;
155 
156  //Perform the importance sampling
157  for (unsigned int i = 0; i < IMPORTANCE_RESAMPLED_SIZE_; ++i)
158  {
159  double slot = rand() / double(RAND_MAX);
160  int index = 0;
161  results_for_sampling_it = results_for_sampling.begin();
162  while (slot > accumulated_confidence_per_result[index])
163  {
164  index++;
165  results_for_sampling_it++;
166  }
167 
168  RecognitionResult recognition_result = *results_for_sampling_it;
169  ROS_DEBUG_STREAM(recognition_result);
170  resampled_results.push_back(recognition_result);
171 
172  if (std::find(resultsToBeDeleted.begin(), resultsToBeDeleted.end(), results_for_sampling_it) == resultsToBeDeleted.end()) {
173  resultsToBeDeleted.push_back(results_for_sampling_it);
174  }
175  }
176 
177  ROS_INFO_STREAM("We sampled " << resampled_results.size() << " results.");
178 
179  //TODO: End
180 
181  //Remove all sampled recognition results from list of extracted best results.
182  for (list<RecognitionResult>::iterator delIt : resultsToBeDeleted)
183  results_for_sampling.erase(delIt);
184 
185  results_for_sampling.insert(results_for_sampling.end(), surplus_results.begin(), surplus_results.end());
186  results_for_sampling.sort([](const RecognitionResult & a, const RecognitionResult & b) -> bool
187  {
188  return a.confidence > b.confidence;
189  });
190  vector<RecognitionResultPtr> remainingResults;
191 
192  //Append all remaining recognition results to shared memory.
193  list<RecognitionResult>::reverse_iterator reverseResultIt;
194 
195  for(reverseResultIt = results_for_sampling.rbegin(); reverseResultIt != results_for_sampling.rend();
196  reverseResultIt++)
197  remainingResults.push_back(RecognitionResultPtr(new RecognitionResult(*reverseResultIt)));
198 
199  shared_recognition_results_manager_ptr_->addResults(remainingResults);
200 
201  ROS_INFO_STREAM(shared_recognition_results_manager_ptr_->getResultsNumber() << " in shared memory after resampling.");
202 
203  ROS_INFO("\n--------------------------------------------------\n");
204  return resampled_results;
205 
206 }
207 
209 {
210  return shared_recognition_results_manager_ptr_->recognitionResultsAvailable();
211 }
212 
214 {
216 }
217 
218 }
const SharedRecognitionResultsManagerPtr shared_recognition_results_manager_ptr_
Definition: resampler.h:49
#define ROS_DEBUG_STREAM_NAMED(name, args)
const unsigned int IMPORTANCE_RESAMPLED_SIZE_
Definition: resampler.h:48
#define ROS_INFO(...)
#define ROS_DEBUG_STREAM(args)
std::vector< ISM::RecognitionResult > drawSamples()
Definition: resampler.cpp:44
#define ROS_INFO_STREAM(args)
#define ROS_ASSERT(cond)
unsigned int getImportanceResampledSize() const
Definition: resampler.cpp:213


asr_recognizer_prediction_ism
Author(s): Aumann Florian, Heller Florian, Hutmacher Robin, Meißner Pascal, Stöckle Patrick, Stroh Daniel
autogenerated on Wed Jan 8 2020 03:18:32