MRPT  2.0.1
CGraphSlamHandler_impl.h
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 #pragma once
11 
13 #include <mrpt/obs/CRawlog.h>
15 #include "CGraphSlamHandler.h"
16 
17 template <class GRAPH_T>
21  const bool enable_visuals)
22  : m_logger(logger),
23  m_options_checker(options_checker),
24  m_enable_visuals(enable_visuals)
25 {
26  using namespace mrpt::system;
29 
30  if (m_enable_visuals)
31  {
32  this->initVisualization();
33  }
34 }
35 
36 template <class GRAPH_T>
38 {
39  m_logger->logFmt(mrpt::system::LVL_WARN, "graphslam-engine has finished.");
40 
41  // keep the window open until user closes it.
42  if (m_win)
43  {
46  "Application will exit when the display window is closed.");
47  bool break_exec = false;
48  while (m_win->isOpen() && break_exec == false)
49  {
50  break_exec = !this->queryObserverForEvents();
51  std::this_thread::sleep_for(100ms);
53  }
54  }
55 
57  {
59  if (m_engine)
60  {
62  }
63  }
64 
65  if (m_engine)
66  {
67  delete m_engine;
68  }
69 
70  if (m_enable_visuals)
71  {
72  if (m_win)
73  {
76  "Releasing CDisplayWindow3D instance...");
77  delete m_win;
78  }
79 
80  if (m_win_observer)
81  {
84  "Releasing CWindowObserver instance...");
85  delete m_win_observer;
86  }
87 
88  if (m_win_manager)
89  {
92  "Releasing CWindowManager instance...");
93  delete m_win_manager;
94  }
95  }
97 }
98 
99 template <class GRAPH_T>
101  const std::string& output_dir_fname)
102 {
103  MRPT_START
104  using namespace std;
105  using namespace mrpt::system;
106  using namespace mrpt;
107 
108  m_logger->logFmt(
109  mrpt::system::LVL_INFO, "Setting up output directory: %s",
110  output_dir_fname.c_str());
111 
112  // current time vars - handy in the rest of the function.
114  string cur_date_str(timeToString(cur_date));
115  string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
116 
117  // Determine what to do with existing results if previous output directory
118  // exists
119  if (directoryExists(output_dir_fname))
120  {
121  int answer_int;
123  {
124  stringstream question;
125  string answer;
126 
127  question << "Directory exists. Choose between the "
128  << "following options" << std::endl;
129  question << "\t 1: Rename current folder and start new "
130  << "output directory (default)" << std::endl;
131  question << "\t 2: Remove existing contents and continue execution "
132  << std::endl;
133  question << "\t 3: Handle potential conflict manually "
134  "(Halts program execution)"
135  << std::endl;
136  question << "\t [ 1 | 2 | 3 ] --> ";
137  std::cout << question.str();
138 
139  getline(cin, answer);
140  answer = mrpt::system::trim(answer);
141  answer_int = atoi(&answer[0]);
142  }
143  else
144  {
145  answer_int = 2;
146  }
147 
148  switch (answer_int)
149  {
150  case 2:
151  {
152  m_logger->logFmt(
153  mrpt::system::LVL_INFO, "Deleting existing files...");
154  // purge directory
156  output_dir_fname,
157  /*deleteDirectoryAsWell = */ false);
158  break;
159  }
160  case 3:
161  {
162  // I don't need to exit gracefully here..
163  exit(0);
164  }
165  case 1:
166  default:
167  {
168  // rename the whole directory to DATE_TIME_${OUTPUT_DIR_NAME}
169  string dst_fname = output_dir_fname + cur_date_validstr;
170  m_logger->logFmt(
171  mrpt::system::LVL_INFO, "Renaming directory to: %s",
172  dst_fname.c_str());
173  string error_msg;
174 #if _DEBUG
175  bool did_rename =
176 #endif
177  renameFile(output_dir_fname, dst_fname, &error_msg);
179  did_rename, format(
180  "\nError while trying to rename the output "
181  "directory: %s",
182  error_msg.c_str()));
183  break;
184  }
185  } // end switch (answer_int)
186  } // end if directory exists..
187 
188  // Now rebuild the directory from scratch
189  m_logger->logFmt(
190  mrpt::system::LVL_INFO, "Creating the new directory structure...");
191  string cur_fname;
192 
193  // debug_fname
194  createDirectory(output_dir_fname);
195  m_logger->logFmt(
196  mrpt::system::LVL_INFO, "Finished initializing output directory.");
197 
198  MRPT_END
199 } // end of initOutputDir
200 
201 template <class GRAPH_T>
203  const std::string& ini_fname, const std::string& rawlog_fname,
204  const std::string& ground_truth_fname)
205 {
206  this->m_ini_fname = ini_fname;
207  this->m_rawlog_fname = rawlog_fname;
208  this->m_gt_fname = ground_truth_fname;
209 
211 
212  m_has_set_fnames = true;
213 }
214 
215 template <class GRAPH_T>
216 void CGraphSlamHandler<GRAPH_T>::readConfigFname(const std::string& fname)
217 {
220  mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));
221 
222  m_logger->logFmt(mrpt::system::LVL_INFO, "Reading the .ini file... ");
223 
224  mrpt::config::CConfigFile cfg_file(fname);
225 
227  "GeneralConfiguration", "user_decides_about_output_dir", false, false);
228  m_output_dir_fname = cfg_file.read_string(
229  "GeneralConfiguration", "output_dir_fname", "graphslam_results", false);
230  m_save_graph =
231  cfg_file.read_bool("GeneralConfiguration", "save_graph", true, false);
233  cfg_file.read_bool("GeneralConfiguration", "save_3DScene", true, false);
234  m_save_map =
235  cfg_file.read_bool("GeneralConfiguration", "save_map", true, false);
236  m_save_graph_fname = cfg_file.read_string(
237  "GeneralConfiguration", "save_graph_fname", "output_graph.graph",
238  false);
240  "GeneralConfiguration", "save_3DScene_fname", "scene.3DScene", false);
241  m_save_map_fname = cfg_file.read_string(
242  "GeneralConfiguration", "save_map_fname", "output_map", false);
243 }
244 
245 template <class GRAPH_T>
247  const std::string& node_reg_str, const std::string& edge_reg_str,
248  const std::string& optimizer_str)
249 {
250  using namespace mrpt;
251 
253 
255  m_options_checker->checkRegistrationDeciderExists(node_reg_str, "node"),
256  format(
257  "\nNode Registration Decider %s is not available.\n",
258  node_reg_str.c_str()));
260  m_options_checker->checkRegistrationDeciderExists(edge_reg_str, "edge"),
261  format(
262  "\nEdge Registration Decider %s is not available.\n",
263  edge_reg_str.c_str()));
265  m_options_checker->checkOptimizerExists(optimizer_str),
266  format("\nOptimizer %s is not available\n", optimizer_str.c_str()));
267 
270  m_options_checker->node_regs_map[node_reg_str](),
271  m_options_checker->edge_regs_map[edge_reg_str](),
272  m_options_checker->optimizers_map[optimizer_str]());
273 }
274 
275 template <class GRAPH_T>
277 {
278  std::cout << this->getParamsAsString() << std::endl;
279  m_engine->printParams();
280 }
281 
282 template <class GRAPH_T>
284 {
285  using namespace std;
286 
287  ASSERTDEB_(str);
288 
289  stringstream ss_out("");
290 
291  ss_out << "\n------------[ graphslam-engine_app Parameters ]------------"
292  << std::endl;
293 
294  // general configuration parameters
295  ss_out << "User decides about output dir? = "
296  << (m_user_decides_about_output_dir ? "TRUE" : "FALSE") << std::endl;
297  ss_out << "Output directory = " << m_output_dir_fname
298  << std::endl;
299  ss_out << "Generate .graph file? = "
300  << (m_save_graph ? "TRUE" : "FALSE") << std::endl;
301  ss_out << "Generate .3DScene file? = "
302  << (m_save_3DScene ? "TRUE" : "FALSE") << std::endl;
303  if (m_save_graph)
304  {
305  ss_out << "Generated .graph filename = " << m_save_graph_fname
306  << std::endl;
307  }
308  if (m_save_3DScene)
309  {
310  ss_out << "Generated .3DScene filename = " << m_save_3DScene_fname
311  << std::endl;
312  }
313  ss_out << "Rawlog filename = " << m_rawlog_fname
314  << std::endl;
315 
316  *str = ss_out.str();
317 }
318 template <class GRAPH_T>
320 {
321  std::string str;
322  this->getParamsAsString(&str);
323  return str;
324 }
325 
326 template <class GRAPH_T>
327 void CGraphSlamHandler<GRAPH_T>::setResultsDirName(const std::string& dirname)
328 {
330  m_logger->logFmt(
331  mrpt::system::LVL_WARN, "Overriding .ini Results directory -> %s...",
332  m_output_dir_fname.c_str());
333 }
334 
335 template <class GRAPH_T>
337  const std::string& output_dir_fname)
338 {
340 
341  m_logger->logFmt(mrpt::system::LVL_INFO, "Generating overall report...");
342  m_engine->generateReportFiles(output_dir_fname);
343  // save the graph and the 3DScene
344  if (m_save_graph)
345  {
346  std::string save_graph_fname =
347  output_dir_fname + "/" + m_save_graph_fname;
348  m_engine->saveGraph(&save_graph_fname);
349  }
351  {
352  std::string save_3DScene_fname =
353  output_dir_fname + "/" + m_save_3DScene_fname;
354  m_engine->save3DScene(&save_3DScene_fname);
355  }
356 
357  // get the occupancy map that was built
358  if (m_save_map)
359  {
360  this->saveMap(output_dir_fname + "/" + m_save_map_fname);
361  }
362 
363  m_logger->logFmt(mrpt::system::LVL_INFO, "Generated report.");
364 }
365 
366 template <class GRAPH_T>
367 void CGraphSlamHandler<GRAPH_T>::saveMap(const std::string& fname)
368 {
371  m_engine->getMap(map);
372  // map->saveAsBitmapFile(fname); // doesn't work.
373  map->saveMetricMapRepresentationToFile(fname);
374 }
375 
376 template <class GRAPH_T>
378 {
379  using namespace mrpt::obs;
381 
382  // Variables initialization
384  CActionCollection::Ptr action;
385  CSensoryFrame::Ptr observations;
386  CObservation::Ptr observation;
387  size_t curr_rawlog_entry;
388  auto arch = mrpt::serialization::archiveFrom(rawlog_stream);
389 
390  // Read the dataset and pass the measurements to CGraphSlamEngine
391  bool cont_exec = true;
392  while (CRawlog::getActionObservationPairOrObservation(
393  arch, action, observations, observation, curr_rawlog_entry) &&
394  cont_exec)
395  {
396  // actual call to the graphSLAM execution method
397  // Exit if user pressed C-c
398  cont_exec = m_engine->_execGraphSlamStep(
399  action, observations, observation, curr_rawlog_entry);
400  }
401  m_logger->logFmt(mrpt::system::LVL_WARN, "Finished graphslam execution.");
402 }
403 
404 template <class GRAPH_T>
406 {
407  using namespace mrpt::opengl;
408  using namespace mrpt::gui;
409  using namespace mrpt::graphslam;
410 
412  m_win = new CDisplayWindow3D("GraphSlam building procedure", 800, 600);
413  m_win->setPos(400, 200);
415  {
417  COpenGLViewport::Ptr main_view = scene->getViewport("main");
418  m_win_observer->observeBegin(*main_view);
420  }
421 
422  m_logger->logFmt(
423  mrpt::system::LVL_DEBUG, "Initialized CDisplayWindow3D...");
424  m_logger->logFmt(
425  mrpt::system::LVL_DEBUG, "Listening to CDisplayWindow3D events...");
426 
427  // pass the window and the observer pointers to the CWindowManager instance
431 }
432 
433 template <class GRAPH_T>
435 {
436  std::map<std::string, bool> events_occurred;
438  &events_occurred,
439  /* reset_keypresses = */ false);
440  bool request_to_exit = events_occurred.find("Ctrl+c")->second;
441 
442  return !request_to_exit;
443 }
void setResultsDirName(const std::string &dirname)
Override the results directory filename that was initially set in the .ini file.
void setCDisplayWindow3DPtr(mrpt::gui::CDisplayWindow3D *win_in)
Store the CDisplayWindow3D pointer in the CWindowManager instance.
void readConfigFname(const std::string &fname)
Read configuration variables for the current graphSLAM execution from a .ini file.
virtual bool checkRegistrationDeciderExists(std::string given_reg, std::string reg_type) const
Check if the given registrator decider exists in the vector of deciders.
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
void printParams() const
Print in a formatted manner the general configuraiton variables for the current graphSLAM execution...
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
std::string m_rawlog_fname
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::string m_save_graph_fname
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::opengl::COpenGLScene::Ptr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
void setWindowObserverPtr(mrpt::graphslam::CWindowObserver *obsever_in)
Store the CWindowObserver pointer in the CWindowManager instance.
void saveResults(const std::string &output_dir_fname)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
bool queryObserverForEvents()
Query the CWindowObserver instance for any pressed keys that might be of interest (e...
std::string m_output_dir_fname
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
bool renameFile(const std::string &oldFileName, const std::string &newFileName, std::string *error_msg=nullptr)
Renames a file - If the target path is different and the filesystem allows it, it will be moved to th...
Definition: filesystem.cpp:309
void returnEventsStruct(std::map< std::string, bool > *codes_to_pressed, bool reset_keypresses=true)
Return a map of key code to a boolean indicating whether it was pressed since the previous time the c...
std::string timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:244
STL namespace.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
CGraphSlamHandler(mrpt::system::COutputLogger *logger, mrpt::graphslam::apps::TUserOptionsChecker< GRAPH_T > *options_checker, const bool enable_visuals=true)
static Ptr Create(Args &&... args)
std::string m_save_map_fname
std::string getParamsAsString() const
void initVisualization()
Initialize visualization (e.g.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
SLAM methods related to graphs of pose constraints.
void setPos(int x, int y) override
Changes the position of the window on the screen.
Versatile class for consistent logging and management of output messages.
bool isOpen()
Returns false if the user has already closed the window.
Main file for the GraphSlamEngine.
This namespace contains representation of robot actions and observations.
void saveMap(const std::string &fname)
mrpt::graphslam::CWindowManager * m_win_manager
void forceRepaint()
Repaints the window.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
void execute()
Method to be called for parsing the rawlog file provided and for running graphSLAM using that informa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::gui::CDisplayWindow3D * m_win
Monitor events in the visualization window.
void setFNames(const std::string &ini_fname, const std::string &rawlog_fname, const std::string &ground_truth_fname=std::string())
Set the relevant filenames for instantiating CGraphSlamEngine instance.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
void observeBegin(CObservable &obj)
Starts the subscription of this observer to the given object.
Definition: CObserver.cpp:26
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists, or path is a file).
Definition: filesystem.cpp:218
std::string trim(const std::string &str)
Removes leading and trailing spaces.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
virtual bool checkOptimizerExists(std::string given_opt) const
Check if the given optimizer exists in the vector of optimizers.
mrpt::system::COutputLogger * m_logger
bool directoryExists(const std::string &fileName)
Test if a given directory exists (it fails if the given path refers to an existing file)...
Definition: filesystem.cpp:137
static uint64_t getCurrentTime() noexcept
Definition: Clock.cpp:46
void initEngine(const std::string &node_reg_str, const std::string &edge_reg_str, const std::string &optimizer_str)
std::string m_save_3DScene_fname
mrpt::graphslam::CGraphSlamEngine< GRAPH_T > * m_engine
void initOutputDir(const std::string &output_dir_fname="graphslam_results")
Initialize (clean up and create new files) the output directory.
mrpt::graphslam::CWindowObserver * m_win_observer
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
mrpt::graphslam::apps::TUserOptionsChecker< GRAPH_T > * m_options_checker
TUserOptionsChecker instance whose task is to evaluate the Registration Decider, Optimizer instances ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020