Main MRPT website > C++ reference for MRPT 1.5.7
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-2016, 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"
15 
16 template<class GRAPH_T>
18  mrpt::utils::COutputLogger* logger,
20  const bool enable_visuals/*=true*/):
21  m_logger(logger),
22  m_options_checker(options_checker),
23  m_do_save_results(true),
24  m_has_set_fnames(false),
25  m_enable_visuals(enable_visuals)
26 {
27  using namespace mrpt::system;
30 
31  m_engine = NULL;
32  m_win_manager = NULL;
33  m_win_observer = NULL;
34  m_win = NULL;
35  if (m_enable_visuals) { this->initVisualization(); }
36 
37 }
38 
39 //////////////////////////////////////////////////////////////////////////////
40 
41 //////////////////////////////////////////////////////////////////////////////
42 template<class GRAPH_T>
44  using namespace mrpt::utils;
45 
46  m_logger->logFmt(LVL_WARN, "graphslam-engine has finished.");
47 
48  // keep the window open until user closes it.
49  if (m_win) {
50  m_logger->logFmt(LVL_WARN,
51  "Application will exit when the display window is closed.");
52  bool break_exec = false;
53  while (m_win->isOpen() && break_exec == false) {
54  break_exec = !this->queryObserverForEvents();
57  }
58  }
59 
60  if (m_do_save_results) {
62  if (m_engine) {
64  }
65  }
66 
67  if (m_engine) { delete m_engine; }
68 
69  if (m_enable_visuals) {
70  if (m_win) {
71  m_logger->logFmt(LVL_DEBUG, "Releasing CDisplayWindow3D instance...");
72  delete m_win;
73  }
74 
75  if (m_win_observer) {
76  m_logger->logFmt(LVL_DEBUG, "Releasing CWindowObserver instance...");
77  delete m_win_observer;
78  }
79 
80  if (m_win_manager) {
81  m_logger->logFmt(LVL_DEBUG, "Releasing CWindowManager instance...");
82  delete m_win_manager;
83  }
84  }
85  m_logger->logFmt(LVL_INFO, "Exited.");
86 }
87 
88 template<class GRAPH_T>
90  const std::string& output_dir_fname /* = graphslam_results */) {
91  MRPT_START;
92  using namespace std;
93  using namespace mrpt::utils;
94  using namespace mrpt::system;
95  using namespace mrpt;
96 
97  m_logger->logFmt(LVL_INFO,
98  "Setting up output directory: %s", output_dir_fname.c_str());
99 
100  // current time vars - handy in the rest of the function.
102  string cur_date_str(timeToString(cur_date));
103  string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
104 
105  // Determine what to do with existing results if previous output directory
106  // exists
107  if (directoryExists(output_dir_fname)) {
108  int answer_int;
110  stringstream question;
111  string answer;
112 
113  question << "Directory exists. Choose between the "
114  << "following options" << std::endl;
115  question << "\t 1: Rename current folder and start new "
116  << "output directory (default)" << std::endl;
117  question << "\t 2: Remove existing contents and continue execution "
118  << std::endl;
119  question << "\t 3: Handle potential conflict manually "
120  "(Halts program execution)" << std::endl;
121  question << "\t [ 1 | 2 | 3 ] --> ";
122  std::cout << question.str();
123 
124  getline(cin, answer);
125  answer = mrpt::system::trim(answer);
126  answer_int = atoi(&answer[0]);
127  }
128  else {
129  answer_int = 2;
130  }
131 
132  switch (answer_int)
133  {
134  case 2:
135  {
136  m_logger->logFmt(LVL_INFO, "Deleting existing files...");
137  // purge directory
138  deleteFilesInDirectory(output_dir_fname,
139  /*deleteDirectoryAsWell = */ false);
140  break;
141  }
142  case 3:
143  {
144  // I don't need to exit gracefully here..
145  exit(0);
146  }
147  case 1:
148  default:
149  {
150  // rename the whole directory to DATE_TIME_${OUTPUT_DIR_NAME}
151  string dst_fname = output_dir_fname + cur_date_validstr;
152  m_logger->logFmt(LVL_INFO,
153  "Renaming directory to: %s", dst_fname.c_str());
154  string error_msg;
155  bool did_rename = renameFile(output_dir_fname, dst_fname, &error_msg);
156  ASSERTMSG_(did_rename,
157  format("\nError while trying to rename the output directory: %s",
158  error_msg.c_str()) );
159  break;
160  }
161  } // SWITCH (ANSWER_INT)
162  } // IF DIRECTORY EXISTS..
163 
164  // Now rebuild the directory from scratch
165  m_logger->logFmt(LVL_INFO, "Creating the new directory structure...");
166  string cur_fname;
167 
168  // debug_fname
169  createDirectory(output_dir_fname);
170  m_logger->logFmt(LVL_INFO, "Finished initializing output directory.");
171 
172  MRPT_END;
173 } // end of initOutputDir
174 
175 //////////////////////////////////////////////////////////////////////////////
176 template<class GRAPH_T>
178  const std::string& ini_fname,
179  const std::string& rawlog_fname,
180  const std::string& ground_truth_fname/*=std::string()*/) {
181 
182  this->m_ini_fname = ini_fname;
183  this->m_rawlog_fname = rawlog_fname;
184  this->m_gt_fname = ground_truth_fname;
185 
187 
188  m_has_set_fnames = true;
189 }
190 
191 ////////////////////////////////////////////////////////////////////////////////
192 
193 template<class GRAPH_T>
195  using namespace mrpt::utils;
196 
198  mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));
199 
200  m_logger->logFmt(LVL_INFO, "Reading the .ini file... ");
201 
202  CConfigFile cfg_file(fname);
203 
205  "GeneralConfiguration",
206  "user_decides_about_output_dir",
207  false, false);
208  m_output_dir_fname = cfg_file.read_string(
209  "GeneralConfiguration",
210  "output_dir_fname",
211  "graphslam_results", false);
212  m_save_graph = cfg_file.read_bool(
213  "GeneralConfiguration",
214  "save_graph",
215  true, false);
216  m_save_3DScene = cfg_file.read_bool(
217  "GeneralConfiguration",
218  "save_3DScene",
219  true, false);
220  m_save_map = cfg_file.read_bool(
221  "GeneralConfiguration",
222  "save_map",
223  true, false);
224  m_save_graph_fname = cfg_file.read_string(
225  "GeneralConfiguration",
226  "save_graph_fname",
227  "output_graph.graph", false);
229  "GeneralConfiguration",
230  "save_3DScene_fname",
231  "scene.3DScene", false);
232  m_save_map_fname = cfg_file.read_string(
233  "GeneralConfiguration",
234  "save_map_fname",
235  "output_map", false);
236 
237 }
238 
239 template<class GRAPH_T>
241  const std::string& node_reg_str,
242  const std::string& edge_reg_str,
243  const std::string& optimizer_str) {
244  using namespace mrpt;
245 
247 
248  ASSERTMSG_(
249  m_options_checker->checkRegistrationDeciderExists(node_reg_str, "node"),
250  format("\nNode Registration Decider %s is not available.\n",
251  node_reg_str.c_str()) );
252  ASSERTMSG_(
253  m_options_checker->checkRegistrationDeciderExists(edge_reg_str, "edge"),
254  format("\nEdge Registration Decider %s is not available.\n",
255  edge_reg_str.c_str()) );
256  ASSERTMSG_(
257  m_options_checker->checkOptimizerExists(optimizer_str),
258  format("\nOptimizer %s is not available\n",
259  optimizer_str.c_str()) );
260 
262  m_ini_fname,
264  m_gt_fname,
266  m_options_checker->node_regs_map[node_reg_str](),
267  m_options_checker->edge_regs_map[edge_reg_str](),
268  m_options_checker->optimizers_map[optimizer_str]());
269 
270 }
271 
272 
273 
274 //////////////////////////////////////////////////////////////////////////////
275 
276 template<class GRAPH_T>
278  std::cout << this->getParamsAsString() << std::endl;
279  m_engine->printParams();
280 }
281 
282 //////////////////////////////////////////////////////////////////////////////
283 template<class GRAPH_T>
285  using namespace std;
286 
287  ASSERT_(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")
297  << std::endl;
298  ss_out << "Output directory = "
300  << std::endl;
301  ss_out << "Generate .graph file? = "
302  << ( m_save_graph? "TRUE" : "FALSE" )
303  << std::endl;
304  ss_out << "Generate .3DScene file? = "
305  << ( m_save_3DScene? "TRUE" : "FALSE" )
306  << std::endl;
307  if (m_save_graph) {
308  ss_out << "Generated .graph filename = "
310  << std::endl;
311  }
312  if (m_save_3DScene) {
313  ss_out << "Generated .3DScene filename = "
314  << m_save_3DScene_fname << std::endl;
315  }
316  ss_out << "Rawlog filename = "
317  << m_rawlog_fname
318  << std::endl;
319 
320  *str = ss_out.str();
321 }
322 //////////////////////////////////////////////////////////////////////////////
323 template<class GRAPH_T>
325  std::string str;
326  this->getParamsAsString(&str);
327  return str;
328 }
329 
330 //////////////////////////////////////////////////////////////////////////////
331 
332 template<class GRAPH_T>
334  const std::string& dirname) {
335  using namespace mrpt::utils;
336 
338  m_logger->logFmt(LVL_WARN,
339  "Overriding .ini Results directory -> %s...",
340  m_output_dir_fname.c_str());
341 }
342 
343 template<class GRAPH_T>
345  const std::string& output_dir_fname) {
346  using namespace mrpt::utils;
347  ASSERT_(m_engine);
348 
349  m_logger->logFmt(LVL_INFO, "Generating overall report...");
350  m_engine->generateReportFiles(output_dir_fname);
351  // save the graph and the 3DScene
352  if (m_save_graph) {
353  std::string save_graph_fname =
354  output_dir_fname + "/" + m_save_graph_fname;
355  m_engine->saveGraph(&save_graph_fname);
356  }
358  std::string save_3DScene_fname =
359  output_dir_fname + "/" + m_save_3DScene_fname;
360  m_engine->save3DScene(&save_3DScene_fname);
361  }
362 
363  // get the occupancy map that was built
364  if (m_save_map) {
365  this->saveMap(output_dir_fname + "/" + m_save_map_fname);
366  }
367 
368  m_logger->logFmt(LVL_INFO, "Generated report.");
369 }
370 
371 template<class GRAPH_T>
373  mrpt::maps::COccupancyGridMap2DPtr map =
375  m_engine->getMap(map);
376  //map->saveAsBitmapFile(fname); // doesn't work.
377  map->saveMetricMapRepresentationToFile(fname);
378 }
379 
380 template<class GRAPH_T>
382  using namespace mrpt::obs;
383  using namespace mrpt::utils;
384  ASSERT_(m_engine);
385 
386  // Variables initialization
387  CFileGZInputStream rawlog_stream(m_rawlog_fname);
388  CActionCollectionPtr action;
389  CSensoryFramePtr observations;
390  CObservationPtr observation;
391  size_t curr_rawlog_entry;
392 
393  // Read the dataset and pass the measurements to CGraphSlamEngine
394  bool cont_exec = true;
395  while (CRawlog::getActionObservationPairOrObservation(
396  rawlog_stream,
397  action,
398  observations,
399  observation,
400  curr_rawlog_entry) && cont_exec) {
401 
402  // actual call to the graphSLAM execution method
403  // Exit if user pressed C-c
404  cont_exec = m_engine->_execGraphSlamStep(
405  action,
406  observations,
407  observation,
408  curr_rawlog_entry);
409 
410  }
411  m_logger->logFmt(LVL_WARN, "Finished graphslam execution.");
412 
413 }
414 
415 //////////////////////////////////////////////////////////////////////////////
416 template<class GRAPH_T>
418 
419  using namespace mrpt::opengl;
420  using namespace mrpt::gui;
421  using namespace mrpt::utils;
422  using namespace mrpt::graphslam;
423 
425  m_win = new CDisplayWindow3D(
426  "GraphSlam building procedure", 800, 600);
427  m_win->setPos(400, 200);
429  {
430  COpenGLScenePtr &scene = m_win->get3DSceneAndLock();
431  COpenGLViewportPtr main_view = scene->getViewport("main");
432  m_win_observer->observeBegin( *main_view );
434  }
435 
436  m_logger->logFmt(LVL_DEBUG, "Initialized CDisplayWindow3D...");
437  m_logger->logFmt(LVL_DEBUG, "Listening to CDisplayWindow3D events...");
438 
439  // pass the window and the observer pointers to the CWindowManager instance
443 
444 }
445 
446 //////////////////////////////////////////////////////////////////////////////
447 template<class GRAPH_T>
449 
450  std::map<std::string, bool> events_occurred;
452  &events_occurred,
453  /* reset_keypresses = */ false);
454  bool request_to_exit = events_occurred.find("Ctrl+c")->second;
455 
456  return !request_to_exit;
457 }
458 
459 
460 #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.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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 BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
std::string m_rawlog_fname
std::string m_save_graph_fname
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
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.
Definition: CConfigFile.h:30
bool queryObserverForEvents()
Query the CWindowObserver instance for any pressed keys that might be of interest (e...
std::string m_output_dir_fname
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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 BASE_IMPEXP timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
Definition: datetime.cpp:347
STL namespace.
std::string BASE_IMPEXP 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:315
std::string m_save_map_fname
std::string getParamsAsString() const
void initVisualization()
Initialize visualization (e.g.
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
SLAM methods related to graphs of pose constraints.
CGraphSlamHandler(mrpt::utils::COutputLogger *logger, mrpt::graphslam::apps::TUserOptionsChecker< GRAPH_T > *options_checker, const bool enable_visuals)
Constructor.
#define MRPT_END
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
Transparently opens a compressed "gz" file and reads uncompressed data from it.
bool isOpen()
Returns false if the user has already closed the window.
void observeBegin(CObservable &obj)
Starts the subscription of this observer to the given object.
Definition: CObserver.cpp:30
Main file for the GraphSlamEngine.
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
void saveMap(const std::string &fname)
void setPos(int x, int y) MRPT_OVERRIDE
Changes the position of the window on the screen.
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::graphslam::CWindowManager * m_win_manager
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
static COccupancyGridMap2DPtr Create()
#define MRPT_START
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
mrpt::utils::COutputLogger * m_logger
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.
The namespace for 3D scene representation and rendering.
#define ASSERT_(f)
bool BASE_IMPEXP 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:209
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
GLenum GLsizei GLenum format
Definition: glext.h:3513
Classes for creating GUI windows for 2D and 3D visualization.
virtual bool checkOptimizerExists(std::string given_opt) const
Check if the given optimizer exists in the vector of optimizers.
bool BASE_IMPEXP 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:132
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.
#define ASSERTMSG_(f, __ERROR_MSG)
mrpt::graphslam::CWindowObserver * m_win_observer
~CGraphSlamHandler()
Destructor.
bool BASE_IMPEXP renameFile(const std::string &oldFileName, const std::string &newFileName, std::string *error_msg=NULL)
Renames a file - If the target path is different and the filesystem allows it, it will be moved to th...
Definition: filesystem.cpp:297
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.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019