Main MRPT website > C++ reference for MRPT 1.5.7
CMultiObjectiveMotionOptimizerBase.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
14 
15 using namespace mrpt::nav;
16 using namespace mrpt::utils;
17 
19 
21  m_params_base(params)
22 {
23 }
24 
25 int CMultiObjectiveMotionOptimizerBase::decide(const std::vector<mrpt::nav::TCandidateMovementPTG>& movs, TResultInfo & extra_info)
26 {
27  auto & score_values = extra_info.score_values;
28  score_values.resize(movs.size());
29 
30  // For each movement:
31  for (unsigned int mov_idx = 0; mov_idx< movs.size();++mov_idx)
32  {
33  const auto &m = movs[mov_idx];
34 
35  // Mark all vars as NaN so we detect uninitialized values:
36  for (auto &p : m_expr_vars) {
37  p.second = std::numeric_limits<double>::quiet_NaN();
38  }
39 
40  for (const auto &prop : m.props) {
41  double & var = m_expr_vars[prop.first];
42  var = prop.second;
43  }
44 
45  // Upon first iteration: compile expressions
46  if (m_score_exprs.size() != m_params_base.formula_score.size())
47  {
48  m_score_exprs.clear();
49 
50  for (const auto &f : m_params_base.formula_score)
51  {
52  auto &se = m_score_exprs[f.first];
53  try
54  {
55  se.compile(f.second, m_expr_vars, std::string("score: ") + f.first);
56  }
57  catch (std::exception &)
58  {
59  m_score_exprs.clear();
60  throw; // rethrow
61  }
62 
63  // Register formulas also as variables, usable by the assert() expressions:
64  {
65  auto it = m_expr_vars.find(f.first);
66  if (it != m_expr_vars.end()) {
67  THROW_EXCEPTION_FMT("Error: Expression name `%s` already exists as an input variable.", f.first.c_str());
68  }
69  // Add it:
70  m_expr_vars[f.first] = std::numeric_limits<double>::quiet_NaN();
71  }
72  }
73  } // end for each score expr
74 
75  // Upon first iteration: compile expressions
76  if (m_movement_assert_exprs.size() != m_params_base.movement_assert.size())
77  {
78  const size_t N = m_params_base.movement_assert.size();
79  m_movement_assert_exprs.clear();
80  m_movement_assert_exprs.resize(N);
81  for (size_t i=0;i<N;i++)
82  {
83  const auto &str = m_params_base.movement_assert[i];
84  auto &ce = m_movement_assert_exprs[i];
85 
86  try
87  {
88  ce.compile(str, m_expr_vars, "assert");
89  }
90  catch (std::exception &)
91  {
92  m_movement_assert_exprs.clear();
93  throw; // rethrow
94  }
95  }
96  }
97 
98  // For each score: evaluate it
99  for (auto &sc : m_score_exprs)
100  {
101  // Evaluate:
102  double val;
103  if (m.speed <= 0) // Invalid candidate
104  {
105  val = .0;
106  }
107  else
108  {
109  val = sc.second.eval();
110  }
111 
112  if (val != val /* NaN */)
113  {
114  THROW_EXCEPTION_FMT("Undefined value evaluating score `%s` for mov_idx=%u!", sc.first.c_str(), mov_idx);
115  }
116 
117  // Store:
118  score_values[mov_idx][sc.first] = val;
119  }
120  } // end for mov_idx
121 
122  // Optional score post-processing: normalize highest value to 1.0
123  for (const auto& sScoreName : m_params_base.scores_to_normalize)
124  {
125  // Find max:
126  double maxScore = .0;
127  for (const auto &s : score_values)
128  {
129  const auto it = s.find(sScoreName);
130  if (it != s.cend())
131  mrpt::utils::keep_max(maxScore, it->second);
132  }
133 
134  // Normalize:
135  if (maxScore <= 0) // all scores=0... let's decide that all are equal, so normalized to "1"
136  {
137  for (auto &s : score_values)
138  {
139  auto it = s.find(sScoreName);
140  if (it != s.end()) {
141  it->second = 1.0;
142  }
143  }
144  }
145  else
146  if (maxScore > 0 && maxScore!=1.0 /* already normalized! */)
147  {
148  double K = 1.0 / maxScore;
149  for (auto &s : score_values)
150  {
151  auto it = s.find(sScoreName);
152  if (it != s.end()) {
153  it->second *= K;
154  }
155  }
156  }
157  }
158 
159  // For each assert, evaluate it (*after* score normalization)
160  for (unsigned int mov_idx = 0; mov_idx < movs.size(); ++mov_idx)
161  {
162  const auto &m = movs[mov_idx];
163  // Mark all vars as NaN so we detect uninitialized values:
164  for (auto &p : m_expr_vars) {
165  p.second = std::numeric_limits<double>::quiet_NaN();
166  }
167  for (const auto &prop : m.props) {
168  double & var = m_expr_vars[prop.first];
169  var = prop.second;
170  }
171 
172  bool assert_failed = false;
173  {
174  for (auto &ma : m_movement_assert_exprs)
175  {
176  const double val = ma.eval();
177  if (val == 0) {
178  assert_failed = true;
179  extra_info.log_entries.emplace_back(std::move(mrpt::format("[CMultiObjectiveMotionOptimizerBase] mov_idx=%u ASSERT failed: `%s`", mov_idx, ma.get_original_expression().c_str())));
180  break;
181  }
182  }
183  }
184  if (assert_failed)
185  {
186  for (auto &e : score_values[mov_idx])
187  {
188  e.second = .0;
189  }
190  }
191  } // end mov_idx
192 
193  // Run algorithm:
194  return impl_decide(movs, extra_info);
195 }
196 
198 {
199  m_score_exprs.clear();
200 }
201 
203 {
204  try
205  {
207 
208  // Factory:
210  if (!classId) return nullptr;
211 
213  return holo;
214  }
215  catch (...)
216  {
217  return nullptr;
218  }
219 }
220 
222 {
223  // Default scores:
224  formula_score["collision_free_distance"] = "collision_free_distance";
225  formula_score["path_index_near_target"] = "var dif:=abs(target_k-move_k); if (dif>(num_paths/2)) { dif:=num_paths-dif; }; exp(-abs(dif / (num_paths/10.0)));";
226  formula_score["euclidean_nearness"] = "(ref_dist - dist_eucl_final) / ref_dist";
227  formula_score["hysteresis"] = "hysteresis";
228  formula_score["clearance"] = "clearance";
229 
230  // Default:
231  scores_to_normalize.push_back("clearance");
232 }
233 
235 {
236  // Load: formula_score
237  {
238  formula_score.clear();
239  int idx = 1;
240  for (;;idx++)
241  {
242  const std::string sKeyName = mrpt::format("score%i_name",idx), sKeyValue = mrpt::format("score%i_formula", idx);
243  const std::string sName = c.read_string(s, sKeyName, "");
244  const std::string sValue = c.read_string(s, sKeyValue, "");
245 
246  const bool none = (sName.empty() && sValue.empty());
247  const bool both = (!sName.empty() && !sValue.empty());
248 
249  if (none && idx == 1)
250  THROW_EXCEPTION_FMT("Expect at least a first `%s` and `%s` pair defining one score in section `[%s]`", sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
251 
252  if (none)
253  break;
254 
255  if (!both) {
256  THROW_EXCEPTION_FMT("Both `%s` and `%s` must be provided in section `[%s]`", sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
257  }
258 
259  formula_score[sName] = sValue;
260  }
261  }
262 
263  // Load: movement_assert
264  {
265  movement_assert.clear();
266  int idx = 1;
267  for (;; idx++)
268  {
269  const std::string sKey = mrpt::format("movement_assert%i", idx);
270  const std::string sValue = c.read_string(s, sKey, "");
271  if (sValue.empty())
272  break;
273  movement_assert.push_back(sValue);
274  }
275  }
276 
277  {
278  scores_to_normalize.clear();
279  std::string sLst = c.read_string(s, "scores_to_normalize", "");
280  if (!sLst.empty()) {
281  mrpt::system::tokenize(sLst, ", \t", scores_to_normalize);
282  }
283  }
284 
285 }
286 
288 {
289  // Save: formula_score
290 
291  {
292  const std::string sComment = "\n"
293  "# Next follows a list of `score%i_{name,formula}` pairs for i=1,...,N\n"
294  "# Each one defines one of the scores that will be evaluated for each candidate movement.\n"
295  "# Multiobjective optimizers will then use those scores to select the best candidate, \n"
296  "# possibly using more parameters that follow below.\n"
297  ;
299 
300  int idx = 0;
301  for (const auto &p : this->formula_score)
302  {
303  ++idx;
304  const std::string sKeyName = mrpt::format("score%i_name", idx), sKeyValue = mrpt::format("score%i_formula", idx);
307  }
308  }
309 
310  // Load: movement_assert
311  {
312  const std::string sComment = "\n"
313  "# Next follows a list of `movement_assert%i` exprtk expressions for i=1,...,N\n"
314  "# defining expressions for conditions that any candidate movement must fulfill\n"
315  "# in order to get through the evaluation process. *All* assert conditions must be satisfied.\n"
316  ;
318 
319  for (unsigned int idx=0;idx<movement_assert.size();idx++)
320  {
321  const std::string sKey = mrpt::format("movement_assert%i", idx+1);
323  }
324  }
325 
326  {
327  std::string sLst;
328  for (const auto& s : scores_to_normalize) {
329  sLst += s;
330  sLst += std::string(",");
331  }
332  c.write(s, "scores_to_normalize", sLst);
333  }
334 
335 }
int BASE_IMPEXP MRPT_SAVE_VALUE_PADDING
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define IMPLEMENTS_VIRTUAL_MRPT_OBJECT(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
Definition: CObject.h:303
std::vector< std::string > log_entries
Optionally, debug logging info will be stored here by the implementor classes.
int decide(const std::vector< mrpt::nav::TCandidateMovementPTG > &movs, TResultInfo &extra_info)
The main entry point for the class: returns the 0-based index of the best of the N motion candidates ...
void BASE_IMPEXP registerAllPendingClasses()
Register all pending classes - to be called just before de-serializing an object, for example...
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &section) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
virtual void clear()
Resets the object state; use if the parameters change, so they are re-read and applied.
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
GLdouble s
Definition: glext.h:3602
std::vector< std::map< std::string, double > > score_values
For each candidate (vector indices), the numerical evaluation of all scores defined in TParamsBase::f...
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::utils::CObject * createObject() const
Definition: CObject.cpp:88
const GLubyte * c
Definition: glext.h:5590
const TRuntimeClassId BASE_IMPEXP * findRegisteredClass(const std::string &className)
Return info about a given class by its name, or NULL if the class is not registered.
int val
Definition: mrpt_jpeglib.h:953
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
Virtual base class for multi-objective motion choosers, as used for reactive navigation engines...
static CMultiObjectiveMotionOptimizerBase * Create(const std::string &className) MRPT_NO_THROWS
Class factory from C++ class name.
A structure that holds runtime class type information.
Definition: CObject.h:36
The virtual base class of all MRPT classes with a unified RTTI system.
Definition: CObject.h:113
GLfloat GLfloat p
Definition: glext.h:5587
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
GLenum const GLfloat * params
Definition: glext.h:3514
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
int BASE_IMPEXP MRPT_SAVE_NAME_PADDING



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019