MRPT  2.0.4
CMultiObjectiveMotionOptimizerBase.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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  classId->createObject());
237  }
238  catch (...)
239  {
241  }
242 }
243 
245 {
246  // Default scores:
247  formula_score["collision_free_distance"] = "collision_free_distance";
248  formula_score["path_index_near_target"] =
249  "var dif:=std::abs(target_k-move_k); if (dif>(num_paths/2)) { "
250  "dif:=num_paths-dif; }; exp(-std::abs(dif / (num_paths/10.0)));";
251  formula_score["euclidean_nearness"] =
252  "(ref_dist - dist_eucl_final) / ref_dist";
253  formula_score["hysteresis"] = "hysteresis";
254  formula_score["clearance"] = "clearance";
255 
256  // Default:
257  scores_to_normalize.emplace_back("clearance");
258 }
259 
261  const mrpt::config::CConfigFileBase& c, const std::string& s)
262 {
263  // Load: formula_score
264  {
265  formula_score.clear();
266  int idx = 1;
267  for (;; idx++)
268  {
269  const std::string sKeyName = mrpt::format("score%i_name", idx),
270  sKeyValue = mrpt::format("score%i_formula", idx);
271  const std::string sName = c.read_string(s, sKeyName, "");
272  const std::string sValue = c.read_string(s, sKeyValue, "");
273 
274  const bool none = (sName.empty() && sValue.empty());
275  const bool both = (!sName.empty() && !sValue.empty());
276 
277  if (none && idx == 1)
279  "Expect at least a first `%s` and `%s` pair defining one "
280  "score in section `[%s]`",
281  sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
282 
283  if (none) break;
284 
285  if (!both)
286  {
288  "Both `%s` and `%s` must be provided in section `[%s]`",
289  sKeyName.c_str(), sKeyValue.c_str(), s.c_str());
290  }
291 
292  formula_score[sName] = sValue;
293  }
294  }
295 
296  // Load: movement_assert
297  {
298  movement_assert.clear();
299  int idx = 1;
300  for (;; idx++)
301  {
302  const std::string sKey = mrpt::format("movement_assert%i", idx);
303  const std::string sValue = c.read_string(s, sKey, "");
304  if (sValue.empty()) break;
305  movement_assert.push_back(sValue);
306  }
307  }
308 
309  {
310  scores_to_normalize.clear();
311  std::string sLst = c.read_string(s, "scores_to_normalize", "");
312  if (!sLst.empty())
313  {
314  mrpt::system::tokenize(sLst, ", \t", scores_to_normalize);
315  }
316  }
317 }
318 
320  mrpt::config::CConfigFileBase& c, const std::string& s) const
321 {
322  // Save: formula_score
323  const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
325 
326  {
327  const std::string sComment =
328  "\n"
329  "# Next follows a list of `score%i_{name,formula}` pairs for "
330  "i=1,...,N\n"
331  "# Each one defines one of the scores that will be evaluated for "
332  "each candidate movement.\n"
333  "# Multiobjective optimizers will then use those scores to select "
334  "the best candidate, \n"
335  "# possibly using more parameters that follow below.\n";
336  c.write(s, "dummy", "", WN, WV, sComment);
337 
338  int idx = 0;
339  for (const auto& p : this->formula_score)
340  {
341  ++idx;
342  const std::string sKeyName = mrpt::format("score%i_name", idx),
343  sKeyValue = mrpt::format("score%i_formula", idx);
344  c.write(s, sKeyName, p.first, WN, WV);
345  c.write(s, sKeyValue, p.second, WN, WV);
346  }
347  }
348 
349  // Load: movement_assert
350  {
351  const std::string sComment =
352  "\n"
353  "# Next follows a list of `movement_assert%i` exprtk expressions "
354  "for i=1,...,N\n"
355  "# defining expressions for conditions that any candidate movement "
356  "must fulfill\n"
357  "# in order to get through the evaluation process. *All* assert "
358  "conditions must be satisfied.\n";
359  c.write(s, "dummy2", "", WN, WV, sComment);
360 
361  for (unsigned int idx = 0; idx < movement_assert.size(); idx++)
362  {
363  const std::string sKey = mrpt::format("movement_assert%i", idx + 1);
364  c.write(s, sKey, movement_assert[idx], WN, WV);
365  }
366  }
367 
368  {
369  std::string sLst;
370  for (const auto& sc : scores_to_normalize)
371  {
372  sLst += sc;
373  sLst += std::string(",");
374  }
375  c.write(s, "scores_to_normalize", sLst);
376  }
377 }
std::shared_ptr< CObject > createObject() const
Definition: CObject.cpp:79
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.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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:31
mrpt::vision::TStereoCalibParams params
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
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. ...
static CMultiObjectiveMotionOptimizerBase::Ptr Factory(const std::string &className) noexcept
Class factory from C++ class name.
std::vector< mrpt::expr::CRuntimeCompiledExpression > m_movement_assert_exprs
int val
Definition: mrpt_jpeglib.h:957
int MRPT_SAVE_VALUE_PADDING()
const TRuntimeClassId * findRegisteredClass(const std::string &className, const bool allow_ignore_namespace=true)
Return info about a given class by its name, or nullptr if the class is not registered.
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
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...
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 ...
static CAST_TO::Ptr from(const CAST_FROM_PTR &ptr)
Definition: CObject.h:356
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:69
virtual int impl_decide(const std::vector< mrpt::nav::TCandidateMovementPTG > &movs, TResultInfo &extra_info)=0



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020