MRPT  1.9.9
CGraphSlamHandler_impl.h
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 #ifndef CGRAPHSLAMHANDLER_IMPL_H
11 #define CGRAPHSLAMHANDLER_IMPL_H
12 
13 // Implementation file for CGraphSlamHandler class
14 #include "CGraphSlamHandler.h"
17 #include <mrpt/obs/CRawlog.h>
18 
19 template <class GRAPH_T>
23  const bool enable_visuals /*=true*/)
24  : m_logger(logger),
25  m_options_checker(options_checker),
26  m_do_save_results(true),
27  m_has_set_fnames(false),
28  m_enable_visuals(enable_visuals)
29 {
30  using namespace mrpt::system;
33 
34  m_engine = NULL;
35  m_win_manager = NULL;
36  m_win_observer = NULL;
37  m_win = NULL;
38  if (m_enable_visuals)
39  {
40  this->initVisualization();
41  }
42 }
43 
44 //////////////////////////////////////////////////////////////////////////////
45 
46 //////////////////////////////////////////////////////////////////////////////
47 template <class GRAPH_T>
49 {
50  m_logger->logFmt(mrpt::system::LVL_WARN, "graphslam-engine has finished.");
51 
52  // keep the window open until user closes it.
53  if (m_win)
54  {
57  "Application will exit when the display window is closed.");
58  bool break_exec = false;
59  while (m_win->isOpen() && break_exec == false)
60  {
61  break_exec = !this->queryObserverForEvents();
62  std::this_thread::sleep_for(100ms);
64  }
65  }
66 
68  {
70  if (m_engine)
71  {
73  }
74  }
75 
76  if (m_engine)
77  {
78  delete m_engine;
79  }
80 
81  if (m_enable_visuals)
82  {
83  if (m_win)
84  {
87  "Releasing CDisplayWindow3D instance...");
88  delete m_win;
89  }
90 
91  if (m_win_observer)
92  {
95  "Releasing CWindowObserver instance...");
96  delete m_win_observer;
97  }
98 
99  if (m_win_manager)
100  {
101  m_logger->logFmt(
103  "Releasing CWindowManager instance...");
104  delete m_win_manager;
105  }
106  }
108 }
109 
110 template <class GRAPH_T>
112  const std::string& output_dir_fname /* = graphslam_results */)
113 {
114  MRPT_START;
115  using namespace std;
116  using namespace mrpt::system;
117  using namespace mrpt;
118 
119  m_logger->logFmt(
120  mrpt::system::LVL_INFO, "Setting up output directory: %s",
121  output_dir_fname.c_str());
122 
123  // current time vars - handy in the rest of the function.
125  string cur_date_str(timeToString(cur_date));
126  string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
127 
128  // Determine what to do with existing results if previous output directory
129  // exists
130  if (directoryExists(output_dir_fname))
131  {
132  int answer_int;
134  {
135  stringstream question;
136  string answer;
137 
138  question << "Directory exists. Choose between the "
139  << "following options" << std::endl;
140  question << "\t 1: Rename current folder and start new "
141  << "output directory (default)" << std::endl;
142  question << "\t 2: Remove existing contents and continue execution "
143  << std::endl;
144  question << "\t 3: Handle potential conflict manually "
145  "(Halts program execution)"
146  << std::endl;
147  question << "\t [ 1 | 2 | 3 ] --> ";
148  std::cout << question.str();
149 
150  getline(cin, answer);
151  answer = mrpt::system::trim(answer);
152  answer_int = atoi(&answer[0]);
153  }
154  else
155  {
156  answer_int = 2;
157  }
158 
159  switch (answer_int)
160  {
161  case 2:
162  {
163  m_logger->logFmt(
164  mrpt::system::LVL_INFO, "Deleting existing files...");
165  // purge directory
167  output_dir_fname,
168  /*deleteDirectoryAsWell = */ false);
169  break;
170  }
171  case 3:
172  {
173  // I don't need to exit gracefully here..
174  exit(0);
175  }
176  case 1:
177  default:
178  {
179  // rename the whole directory to DATE_TIME_${OUTPUT_DIR_NAME}
180  string dst_fname = output_dir_fname + cur_date_validstr;
181  m_logger->logFmt(
182  mrpt::system::LVL_INFO, "Renaming directory to: %s",
183  dst_fname.c_str());
184  string error_msg;
185  bool did_rename =
186  renameFile(output_dir_fname, dst_fname, &error_msg);
188  did_rename, format(
189  "\nError while trying to rename the output "
190  "directory: %s",
191  error_msg.c_str()));
192  break;
193  }
194  } // SWITCH (ANSWER_INT)
195  } // IF DIRECTORY EXISTS..
196 
197  // Now rebuild the directory from scratch
198  m_logger->logFmt(
199  mrpt::system::LVL_INFO, "Creating the new directory structure...");
200  string cur_fname;
201 
202  // debug_fname
203  createDirectory(output_dir_fname);
204  m_logger->logFmt(
205  mrpt::system::LVL_INFO, "Finished initializing output directory.");
206 
207  MRPT_END;
208 } // end of initOutputDir
209 
210 //////////////////////////////////////////////////////////////////////////////
211 template <class GRAPH_T>
213  const std::string& ini_fname, const std::string& rawlog_fname,
214  const std::string& ground_truth_fname /*=std::string()*/)
215 {
216  this->m_ini_fname = ini_fname;
217  this->m_rawlog_fname = rawlog_fname;
218  this->m_gt_fname = ground_truth_fname;
219 
221 
222  m_has_set_fnames = true;
223 }
224 
225 ////////////////////////////////////////////////////////////////////////////////
226 
227 template <class GRAPH_T>
229 {
232  mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));
233 
234  m_logger->logFmt(mrpt::system::LVL_INFO, "Reading the .ini file... ");
235 
236  mrpt::config::CConfigFile cfg_file(fname);
237 
239  "GeneralConfiguration", "user_decides_about_output_dir", false, false);
240  m_output_dir_fname = cfg_file.read_string(
241  "GeneralConfiguration", "output_dir_fname", "graphslam_results", false);
242  m_save_graph =
243  cfg_file.read_bool("GeneralConfiguration", "save_graph", true, false);
245  cfg_file.read_bool("GeneralConfiguration", "save_3DScene", true, false);
246  m_save_map =
247  cfg_file.read_bool("GeneralConfiguration", "save_map", true, false);
248  m_save_graph_fname = cfg_file.read_string(
249  "GeneralConfiguration", "save_graph_fname", "output_graph.graph",
250  false);
252  "GeneralConfiguration", "save_3DScene_fname", "scene.3DScene", false);
253  m_save_map_fname = cfg_file.read_string(
254  "GeneralConfiguration", "save_map_fname", "output_map", false);
255 }
256 
257 template <class GRAPH_T>
259  const std::string& node_reg_str, const std::string& edge_reg_str,
260  const std::string& optimizer_str)
261 {
262  using namespace mrpt;
263 
265 
267  m_options_checker->checkRegistrationDeciderExists(node_reg_str, "node"),
268  format(
269  "\nNode Registration Decider %s is not available.\n",
270  node_reg_str.c_str()));
272  m_options_checker->checkRegistrationDeciderExists(edge_reg_str, "edge"),
273  format(
274  "\nEdge Registration Decider %s is not available.\n",
275  edge_reg_str.c_str()));
277  m_options_checker->checkOptimizerExists(optimizer_str),
278  format("\nOptimizer %s is not available\n", optimizer_str.c_str()));
279 
282  m_options_checker->node_regs_map[node_reg_str](),
283  m_options_checker->edge_regs_map[edge_reg_str](),
284  m_options_checker->optimizers_map[optimizer_str]());
285 }
286 
287 //////////////////////////////////////////////////////////////////////////////
288 
289 template <class GRAPH_T>
291 {
292  std::cout << this->getParamsAsString() << std::endl;
293  m_engine->printParams();
294 }
295 
296 //////////////////////////////////////////////////////////////////////////////
297 template <class GRAPH_T>
299 {
300  using namespace std;
301 
302  ASSERTDEB_(str);
303 
304  stringstream ss_out("");
305 
306  ss_out << "\n------------[ graphslam-engine_app Parameters ]------------"
307  << std::endl;
308 
309  // general configuration parameters
310  ss_out << "User decides about output dir? = "
311  << (m_user_decides_about_output_dir ? "TRUE" : "FALSE") << std::endl;
312  ss_out << "Output directory = " << m_output_dir_fname
313  << std::endl;
314  ss_out << "Generate .graph file? = "
315  << (m_save_graph ? "TRUE" : "FALSE") << std::endl;
316  ss_out << "Generate .3DScene file? = "
317  << (m_save_3DScene ? "TRUE" : "FALSE") << std::endl;
318  if (m_save_graph)
319  {
320  ss_out << "Generated .graph filename = " << m_save_graph_fname
321  << std::endl;
322  }
323  if (m_save_3DScene)
324  {
325  ss_out << "Generated .3DScene filename = " << m_save_3DScene_fname
326  << std::endl;
327  }
328  ss_out << "Rawlog filename = " << m_rawlog_fname
329  << std::endl;
330 
331  *str = ss_out.str();
332 }
333 //////////////////////////////////////////////////////////////////////////////
334 template <class GRAPH_T>
336 {
337  std::string str;
338  this->getParamsAsString(&str);
339  return str;
340 }
341 
342 //////////////////////////////////////////////////////////////////////////////
343 
344 template <class GRAPH_T>
346 {
348  m_logger->logFmt(
349  mrpt::system::LVL_WARN, "Overriding .ini Results directory -> %s...",
350  m_output_dir_fname.c_str());
351 }
352 
353 template <class GRAPH_T>
355  const std::string& output_dir_fname)
356 {
358 
359  m_logger->logFmt(mrpt::system::LVL_INFO, "Generating overall report...");
360  m_engine->generateReportFiles(output_dir_fname);
361  // save the graph and the 3DScene
362  if (m_save_graph)
363  {
364  std::string save_graph_fname =
365  output_dir_fname + "/" + m_save_graph_fname;
366  m_engine->saveGraph(&save_graph_fname);
367  }
369  {
370  std::string save_3DScene_fname =
371  output_dir_fname + "/" + m_save_3DScene_fname;
372  m_engine->save3DScene(&save_3DScene_fname);
373  }
374 
375  // get the occupancy map that was built
376  if (m_save_map)
377  {
378  this->saveMap(output_dir_fname + "/" + m_save_map_fname);
379  }
380 
381  m_logger->logFmt(mrpt::system::LVL_INFO, "Generated report.");
382 }
383 
384 template <class GRAPH_T>
386 {
388  mrpt::make_aligned_shared<mrpt::maps::COccupancyGridMap2D>();
389  m_engine->getMap(map);
390  // map->saveAsBitmapFile(fname); // doesn't work.
391  map->saveMetricMapRepresentationToFile(fname);
392 }
393 
394 template <class GRAPH_T>
396 {
397  using namespace mrpt::obs;
399 
400  // Variables initialization
402  CActionCollection::Ptr action;
403  CSensoryFrame::Ptr observations;
404  CObservation::Ptr observation;
405  size_t curr_rawlog_entry;
406  auto arch = mrpt::serialization::archiveFrom(rawlog_stream);
407 
408  // Read the dataset and pass the measurements to CGraphSlamEngine
409  bool cont_exec = true;
410  while (CRawlog::getActionObservationPairOrObservation(
411  arch, action, observations, observation, curr_rawlog_entry) &&
412  cont_exec)
413  {
414  // actual call to the graphSLAM execution method
415  // Exit if user pressed C-c
416  cont_exec = m_engine->_execGraphSlamStep(
417  action, observations, observation, curr_rawlog_entry);
418  }
419  m_logger->logFmt(mrpt::system::LVL_WARN, "Finished graphslam execution.");
420 }
421 
422 //////////////////////////////////////////////////////////////////////////////
423 template <class GRAPH_T>
425 {
426  using namespace mrpt::opengl;
427  using namespace mrpt::gui;
428  using namespace mrpt::graphslam;
429 
431  m_win = new CDisplayWindow3D("GraphSlam building procedure", 800, 600);
432  m_win->setPos(400, 200);
434  {
436  COpenGLViewport::Ptr main_view = scene->getViewport("main");
437  m_win_observer->observeBegin(*main_view);
439  }
440 
441  m_logger->logFmt(
442  mrpt::system::LVL_DEBUG, "Initialized CDisplayWindow3D...");
443  m_logger->logFmt(
444  mrpt::system::LVL_DEBUG, "Listening to CDisplayWindow3D events...");
445 
446  // pass the window and the observer pointers to the CWindowManager instance
450 }
451 
452 //////////////////////////////////////////////////////////////////////////////
453 template <class GRAPH_T>
455 {
456  std::map<std::string, bool> events_occurred;
458  &events_occurred,
459  /* reset_keypresses = */ false);
460  bool request_to_exit = events_occurred.find("Ctrl+c")->second;
461 
462  return !request_to_exit;
463 }
464 
465 #endif /* end of include guard: CGRAPHSLAMHANDLER_IMPL_H */
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:158
#define MRPT_START
Definition: exceptions.h:262
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.
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:127
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:308
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:241
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:328
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:555
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.
CGraphSlamHandler(mrpt::system::COutputLogger *logger, mrpt::graphslam::apps::TUserOptionsChecker< GRAPH_T > *options_checker, const bool enable_visuals)
Constructor.
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.
static uint64_t getCurrentTime()
Definition: Clock.cpp:22
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)
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::graphslam::CWindowManager * m_win_manager
void forceRepaint()
Repaints the window.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
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:205
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:266
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
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:215
std::string trim(const std::string &str)
Removes leading and trailing spaces.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
GLenum GLsizei GLenum format
Definition: glext.h:3531
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:136
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
Pointer to the engine instance.
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
~CGraphSlamHandler()
Destructor.
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 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020