Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #include <limits>
15 
16 using namespace mrpt::nav;
17 
20 
22  TParamsBase& params)
23  : m_params_base(params)
24 {
25 }
26 
27 int CMultiObjectiveMotionOptimizerBase::decide(
28  const std::vector<mrpt::nav::TCandidateMovementPTG>& movs,
29  TResultInfo& extra_info)
30 {
31  auto& score_values = extra_info.score_values;
32  score_values.resize(movs.size());
33 
34  // For each movement:
35  for (unsigned int mov_idx = 0; mov_idx < movs.size(); ++mov_idx)
36  {
37  const auto& m = movs[mov_idx];
38 
39  // Mark all vars as NaN so we detect uninitialized values:
40  for (auto& p : m_expr_vars)
41  {
42  p.second = std::numeric_limits<double>::quiet_NaN();
43  }
44 
45  for (const auto& prop : m.props)
46  {
47  double& var = m_expr_vars[prop.first];
48  var = prop.second;
49  }
50 
51  // Upon first iteration: compile expressions
52  if (m_score_exprs.size() != m_params_base.formula_score.size())
53  {
54  m_score_exprs.clear();
55 
56  for (const auto& f : m_params_base.formula_score)
57  {
58  auto& se = m_score_exprs[f.first];
59  try
60  {
61  se.compile(
62  f.second, m_expr_vars,
63  std::string("score: ") + f.first);
64  }
65  catch (std::exception&)
66  {
67  m_score_exprs.clear();
68  throw; // rethrow
69  }
70 
71  // Register formulas also as variables, usable by the assert()
72  // expressions:
73  {
74  auto it = m_expr_vars.find(f.first);
75  if (it != m_expr_vars.end())
76  {
78  "Error: Expression name `%s` already exists as an "
79  "input variable.",
80  f.first.c_str());
81  }
82  // Add it:
83  m_expr_vars[f.first] =
84  std::numeric_limits<double>::quiet_NaN();
85  }
86  }
87  } // end for each score expr
88 
89  // Upon first iteration: compile expressions
90  if (m_movement_assert_exprs.size() !=
92  {
93  const size_t N = m_params_base.movement_assert.size();
95  m_movement_assert_exprs.resize(N);
96  for (size_t i = 0; i < N; i++)
97  {
98  const auto& str = m_params_base.movement_assert[i];
99  auto& ce = m_movement_assert_exprs[i];
100 
101  try
102  {
103  ce.compile(str, m_expr_vars, "assert");
104  }
105  catch (std::exception&)
106  {
107  m_movement_assert_exprs.clear();
108  throw; // rethrow
109  }
110  }
111  }
112 
113  // For each score: evaluate it
114  for (auto& sc : m_score_exprs)
115  {
116  // Evaluate:
117  double val;
118  if (m.speed <= 0) // Invalid candidate
119  {
120  val = .0;
121  }
122  else
123  {
124  val = sc.second.eval();
125  }
126 
127  if (val != val /* NaN */)
128  {
130  "Undefined value evaluating score `%s` for mov_idx=%u!",
131  sc.first.c_str(), mov_idx);
132  }
133 
134  // Store:
135  score_values[mov_idx][sc.first] = val;
136  }
137  } // end for mov_idx
138 
139  // Optional score post-processing: normalize highest value to 1.0
140  for (const auto& sScoreName : m_params_base.scores_to_normalize)
141  {
142  // Find max:
143  double maxScore = .0;
144  for (const auto& s : score_values)
145  {
146  const auto it = s.find(sScoreName);
147  if (it != s.cend()) mrpt::keep_max(maxScore, it->second);
148  }
149 
150  // Normalize:
151  if (maxScore <= 0) // all scores=0... let's decide that all are equal,
152  // so normalized to "1"
153  {
154  for (auto& s : score_values)
155  {
156  auto it = s.find(sScoreName);
157  if (it != s.end())
158  {
159  it->second = 1.0;
160  }
161  }
162  }
163  else if (maxScore > 0 && maxScore != 1.0 /* already normalized! */)
164  {
165  double K = 1.0 / maxScore;
166  for (auto& s : score_values)
167  {
168  auto it = s.find(sScoreName);
169  if (it != s.end())
170  {
171  it->second *= K;
172  }
173  }
174  }
175  }
176 
177  // For each assert, evaluate it (*after* score normalization)
178  for (unsigned int mov_idx = 0; mov_idx < movs.size(); ++mov_idx)
179  {
180  const auto& m = movs[mov_idx];
181  // Mark all vars as NaN so we detect uninitialized values:
182  for (auto& p : m_expr_vars)
183  {
184  p.second = std::numeric_limits<double>::quiet_NaN();
185  }
186  for (const auto& prop : m.props)
187  {
188  double& var = m_expr_vars[prop.first];
189  var = prop.second;
190  }
191 
192  bool assert_failed = false;
193  {
194  for (auto& ma : m_movement_assert_exprs)
195  {
196  const double val = ma.eval();
197  if (val == 0)
198  {
199  assert_failed = true;
200  extra_info.log_entries.emplace_back(mrpt::format(
201  "[CMultiObjectiveMotionOptimizerBase] "
202  "mov_idx=%u ASSERT failed: `%s`",
203  mov_idx, ma.get_original_expression().c_str()));
204  break;
205  }
206  }
207  }
208  if (assert_failed)
209  {
210  for (auto& e : score_values[mov_idx])
211  {
212  e.second = .0;
213  }
214  }
215  } // end mov_idx
216 
217  // Run algorithm:
218  return impl_decide(movs, extra_info);
219 }
220 
224  const std::string& className) noexcept
225 {
226  try
227  {
229 
230  // Factory:
231  const mrpt::rtti::TRuntimeClassId* classId =
233  if (!classId) return nullptr;
234 
236  dynamic_cast<CMultiObjectiveMotionOptimizerBase*>(
237  classId->createObject()));
238  }
239  catch (...)
240  {
241  return nullptr;
242  }
243 }
244 
246 {
247  // Default scores:
248  formula_score["collision_free_distance"] = "collision_free_distance";
249  formula_score["path_index_near_target"] =
250  "var dif:=abs(target_k-move_k); if (dif>(num_paths/2)) { "
251  "dif:=num_paths-dif; }; exp(-abs(dif / (num_paths/10.0)));";
252  formula_score["euclidean_nearness"] =
253  "(ref_dist - dist_eucl_final) / ref_dist";
254  formula_score["hysteresis"] = "hysteresis";
255  formula_score["clearance"] = "clearance";
256 
257  // Default:
258  scores_to_normalize.push_back("clearance");
259 }
260 
263 {
264  // Load: formula_score
265  {
266  formula_score.clear();
267  int idx = 1;
268  for (;; idx++)
269  {
270  const std::string sKeyName = mrpt::format("score%i_name", idx),
271  sKeyValue = mrpt::format("score%i_formula", idx);
272  const std::string sName = c.read_string(s, sKeyName, "");
273  const std::string sValue = c.read_string(s, sKeyValue, "");
274 
275  const bool none = (sName.empty() && sValue.empty());
276  const bool both = (!sName.empty() && !sValue.empty());
277 
278  if (none && idx == 1)
280  "Expect at least a first `%s` and `%s` pair defining one "
281  "score in section `[%s]`",
282  sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
283 
284  if (none) break;
285 
286  if (!both)
287  {
289  "Both `%s` and `%s` must be provided in section `[%s]`",
290  sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
291  }
292 
293  formula_score[sName] = sValue;
294  }
295  }
296 
297  // Load: movement_assert
298  {
299  movement_assert.clear();
300  int idx = 1;
301  for (;; idx++)
302  {
303  const std::string sKey = mrpt::format("movement_assert%i", idx);
304  const std::string sValue = c.read_string(s, sKey, "");
305  if (sValue.empty()) break;
306  movement_assert.push_back(sValue);
307  }
308  }
309 
310  {
311  scores_to_normalize.clear();
312  std::string sLst = c.read_string(s, "scores_to_normalize", "");
313  if (!sLst.empty())
314  {
315  mrpt::system::tokenize(sLst, ", \t", scores_to_normalize);
316  }
317  }
318 }
319 
322 {
323  // Save: formula_score
324  const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
326 
327  {
328  const std::string sComment =
329  "\n"
330  "# Next follows a list of `score%i_{name,formula}` pairs for "
331  "i=1,...,N\n"
332  "# Each one defines one of the scores that will be evaluated for "
333  "each candidate movement.\n"
334  "# Multiobjective optimizers will then use those scores to select "
335  "the best candidate, \n"
336  "# possibly using more parameters that follow below.\n";
337  c.write(s, "dummy", "", WN, WV, sComment);
338 
339  int idx = 0;
340  for (const auto& p : this->formula_score)
341  {
342  ++idx;
343  const std::string sKeyName = mrpt::format("score%i_name", idx),
344  sKeyValue = mrpt::format("score%i_formula", idx);
345  c.write(s, sKeyName, p.first, WN, WV);
346  c.write(s, sKeyValue, p.second, WN, WV);
347  }
348  }
349 
350  // Load: movement_assert
351  {
352  const std::string sComment =
353  "\n"
354  "# Next follows a list of `movement_assert%i` exprtk expressions "
355  "for i=1,...,N\n"
356  "# defining expressions for conditions that any candidate movement "
357  "must fulfill\n"
358  "# in order to get through the evaluation process. *All* assert "
359  "conditions must be satisfied.\n";
360  c.write(s, "dummy2", "", WN, WV, sComment);
361 
362  for (unsigned int idx = 0; idx < movement_assert.size(); idx++)
363  {
364  const std::string sKey = mrpt::format("movement_assert%i", idx + 1);
365  c.write(s, sKey, movement_assert[idx], WN, WV);
366  }
367  }
368 
369  {
370  std::string sLst;
371  for (const auto& sc : scores_to_normalize)
372  {
373  sLst += sc;
374  sLst += std::string(",");
375  }
376  c.write(s, "scores_to_normalize", sLst);
377  }
378 }
void registerAllPendingClasses()
Register all pending classes - to be called just before de-serializing an object, for example...
std::vector< std::string > log_entries
Optionally, debug logging info will be stored here by the implementor classes.
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
virtual void clear()
Resets the object state; use if the parameters change, so they are re-read and applied.
A structure that holds runtime class type information.
Definition: CObject.h:30
GLdouble s
Definition: glext.h:3676
std::shared_ptr< CMultiObjectiveMotionOptimizerBase > Ptr
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
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...
IMPLEMENTS_VIRTUAL_MRPT_OBJECT(CMultiObjectiveMotionOptimizerBase, CObject, mrpt::nav) CMultiObjectiveMotionOptimizerBase
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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...
static CMultiObjectiveMotionOptimizerBase::Ptr Factory(const std::string &className) noexcept
Class factory from C++ class name.
const GLubyte * c
Definition: glext.h:6313
std::vector< mrpt::expr::CRuntimeCompiledExpression > m_movement_assert_exprs
int val
Definition: mrpt_jpeglib.h:955
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
int MRPT_SAVE_VALUE_PADDING()
GLsizei const GLchar ** string
Definition: glext.h:4101
const TRuntimeClassId * findRegisteredClass(const std::string &className)
Return info about a given class by its name, or nullptr if the class is not registered.
std::map< std::string, std::string > formula_score
A list of name -> mathematical expression (in the format of the exprtk library) for the list of "scor...
Virtual base class for multi-objective motion choosers, as used for reactive navigation engines...
std::vector< std::string > scores_to_normalize
List of score names (as defined in the key of formula_score) that must be normalized across all candi...
std::map< std::string, mrpt::expr::CRuntimeCompiledExpression > m_score_exprs
score names -> score compiled expressions
std::vector< std::string > movement_assert
A list of exprtk expressions for conditions that any candidate movement must fulfill in order to get ...
mrpt::rtti::CObject * createObject() const
Definition: CObject.cpp:79
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
CMultiObjectiveMotionOptimizerBase(TParamsBase &params)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
virtual int impl_decide(const std::vector< mrpt::nav::TCandidateMovementPTG > &movs, TResultInfo &extra_info)=0



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019