Main MRPT website > C++ reference for MRPT 1.5.6
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.
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
std::string m_rawlog_fname
std::string m_save_graph_fname
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
void printParams() const
Print in a formatted manner the general configuraiton variables for the current graphSLAM execution...
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
std::string m_save_map_fname
void initVisualization()
Initialize visualization (e.g.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
virtual bool checkRegistrationDeciderExists(std::string given_reg, std::string reg_type) const
Check if the given registrator decider exists in the vector of deciders.
std::string getParamsAsString() const
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
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
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.
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.
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()
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars= '_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
Definition: filesystem.cpp:315
#define MRPT_START
void execute()
Method to be called for parsing the rawlog file provided and for running graphSLAM using that informa...
mrpt::gui::CDisplayWindow3D * m_win
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
#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.
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
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
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
virtual bool checkOptimizerExists(std::string given_opt) const
Check if the given optimizer exists in the vector of optimizers.
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018