CMapMerger.cpp
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 
11 
12 using namespace mrpt::graphslam;
13 using namespace mrpt::graphslam::detail;
14 using namespace mrpt_msgs;
15 using namespace mrpt::maps;
16 using namespace mrpt::slam;
17 using namespace mrpt::math;
18 using namespace mrpt::system;
19 using namespace mrpt::poses;
20 using namespace mrpt::opengl;
21 using namespace mrpt::utils;
22 using namespace ros;
23 using namespace nav_msgs;
24 using namespace std;
25 using namespace mrpt_bridge;
26 
27 // helper methods
28 
30 
34  std::string obj_name="") {
35  using namespace mrpt::opengl;
36  ASSERT_(win);
37  bool res = true;
38  COpenGLScene::Ptr& scene = win->get3DSceneAndLock();
39 
40  if (obj_name.empty()) {
41  cout << "Clearing entire scene." << endl;
42  scene.reset();
43  }
44  else {
45  CRenderizable::Ptr obj = scene->getByName(obj_name);
46  if (obj) {
47  scene->removeObject(obj);
48  }
49  else {
50  res = false;
51  }
52  }
53 
54  win->unlockAccess3DScene();
55  win->forceRepaint();
56  return res;
57 }
58 
62  std::string obj_name="") {
63  ASSERT_(win_manager);
64  return removeObjectFrom3DScene(win_manager->win, obj_name);
65 }
66 
67 template<class RENDERIZABLE_OBJECT>
69  const RENDERIZABLE_OBJECT& o,
70  const std::string& obj_name="",
71  const mrpt::poses::CPose3D& obj_pose=EMPTY_POSE) {
72  using namespace mrpt::opengl;
73 
74  COpenGLScene::Ptr &scene = win->get3DSceneAndLock();
75  CSetOfObjects::Ptr obj = CSetOfObjects::Create();
76 
77  o.getAs3DObject(obj);
78  obj->setPose(obj_pose);
79 
80  if (!obj_name.empty()) {
81  obj->setName(obj_name);
82  }
83 
84  scene->insert(obj);
85 
86  win->unlockAccess3DScene();
87  win->forceRepaint();
88 }
89 
91  using namespace mrpt;
92  using namespace mrpt::opengl;
93  ASSERT_(win);
94 
95  COpenGLScene::Ptr &scene = win->get3DSceneAndLock();
96  opengl::CAxis::Ptr obj = opengl::CAxis::Create(-6, -6, -6, 6, 6, 6, 2, 2, true);
97  obj->setLocation(0, 0, 0);
98  scene->insert(obj);
99  win->unlockAccess3DScene();
100  win->forceRepaint();
101 }
102 
104  using namespace mrpt;
105  using namespace mrpt::opengl;
106  ASSERT_(win);
107 
108  COpenGLScene::Ptr &scene = win->get3DSceneAndLock();
109  opengl::CGridPlaneXY::Ptr obj = opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1);
110  obj->setColor(0.7, 0.7, 0.7);
111  obj->setLocation(0, 0, 0);
112  scene->insert(obj);
113 
114  win->unlockAccess3DScene();
115  win->forceRepaint();
116 }
117 
119  ASSERT_(win);
120 
121  addAxis(win);
122  addGrid(win);
123 }
124 
126 
127 
129  mrpt::utils::COutputLogger* logger_in,
130  ros::NodeHandle* nh_in):
131  m_logger(logger_in),
132  m_nh(nh_in),
133  m_conn_manager(logger_in, nh_in),
134  m_queue_size(1),
135  quit_keypress1("q"),
136  quit_keypress2("Ctrl+c"),
137  map_merge_keypress("n"),
138  save_map_merging_results(true)
139 {
140  ASSERT_(m_nh);
141 
142  m_global_ns = "/map_merger";
143 
144  // GridMap Alignment options to be used in merging.
145  m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC;
150  //m_alignment_options.loadFromConfigFileName(
151  //"/home/bergercookie/mrpt/share/mrpt/config_files/grid-matching/gridmatch_example.ini",
152  //"grid-match");
154 
155  // initialize CDisplayWindow for fused map
159 
160 }
161 
163  ASSERT_(win_observer);
164 
165  win_observer->registerKeystroke(quit_keypress1,
166  "Finish execution");
168  "Compute next available grid-merging");
169 
170 
171 }
172 
174 
175  // delete the generated neighbors intances
176  for (neighbors_t::iterator n_it = m_neighbors.begin();
177  n_it != m_neighbors.end();
178  ++n_it) {
179 
180  delete m_neighbors_to_windows.at(*n_it)->observer;
181  delete m_neighbors_to_windows.at(*n_it)->win;
182  delete m_neighbors_to_windows.at(*n_it);
183  delete *n_it;
184  }
185 
186  // delete fused window manager
190 
191  m_logger->logFmt(LVL_WARN, "Exiting...");
192 }
193 
195  MRPT_START;
196 
197  // get the new GraphSlamAgents
198  const GraphSlamAgents& nearby_slam_agents =
200  //m_logger->logFmt(LVL_DEBUG, "nearby_slam_agents size: %lu\n",
201  //static_cast<unsigned long>(nearby_slam_agents.list.size()));
202 
203  for (GraphSlamAgents::_list_type::const_iterator
204  it = nearby_slam_agents.list.begin();
205  it != nearby_slam_agents.list.end();
206  ++it) {
207  const GraphSlamAgent& gsa = *it;
208 
209  // Is the current GraphSlamAgent already registered?
210  auto search = [gsa](const TNeighborAgentMapProps* neighbor) {
211  return (neighbor->agent == gsa);
212  };
213  typename neighbors_t::iterator n_it = find_if(
214  m_neighbors.begin(),
215  m_neighbors.end(), search);
216 
217  if (n_it == m_neighbors.end()) { // current gsa not found, add it
218 
219  m_neighbors.push_back(new TNeighborAgentMapProps(m_logger, gsa, m_nh));
220  TNeighborAgentMapProps* latest_neighbor = m_neighbors.back();
221  latest_neighbor->setupComm();
222  m_logger->logFmt(LVL_WARN,
223  "Initialized NeighborAgentMapProps instance for agent %s...",
224  latest_neighbor->agent.topic_namespace.data.c_str());
225 
226  // initialize the window
228  win_manager->win->setWindowTitle(
229  latest_neighbor->agent.topic_namespace.data);
230  m_neighbors_to_windows.insert(make_pair(latest_neighbor, win_manager));
231  this->monitorKeystrokes(win_manager->observer);
232 
233  }
234  } // end for all fetched GraphSlamAgents
235 
236  // run through the open windows - Exit if instructed
237  bool continue_exec = true;
238  for (map<TNeighborAgentMapProps*, CWindowManager*>::const_iterator
239  it = m_neighbors_to_windows.begin();
240  it != m_neighbors_to_windows.end();
241  ++it) {
242 
243  CWindowManager* win_manager = it->second;
244 
245  std::map<std::string, bool> events_map;
246  win_manager->observer->returnEventsStruct(&events_map);
247 
248  if (events_map.at(map_merge_keypress)) {
249  mergeMaps();
250  }
251  win_manager->win->forceRepaint();
252 
253  // continue or exit
254  if (!win_manager->isOpen() ||
255  events_map.at(quit_keypress1) ||
256  events_map.at(quit_keypress2)) {
257  continue_exec = false;
258  break;
259  }
260  }
261 
262  // Fetch the events for the main (fused map) window
263  if (continue_exec) {
264  std::map<std::string, bool> events_map;
266  if (events_map.at(map_merge_keypress)) {
267  mergeMaps();
268  }
270  continue_exec =
272  !events_map.at(quit_keypress1) &&
273  !events_map.at(quit_keypress2);
274  }
275 
276  return continue_exec;
277 
278  MRPT_END;
279 } // end of updateState
280 
282  MRPT_START;
283  m_logger->logFmt(LVL_WARN, "In mergeMaps.");
284 
285  CGridMapAligner gridmap_aligner;
286  gridmap_aligner.options = m_alignment_options;
287 
288  // List of maps that is to be filled.
289  maps_t mrpt_gridmaps;
290  trajectories_t mrpt_trajectories;
291 
292  // traverse Neighbor instances - get their nav_msgs::OccupancyGrid maps, trajectories
293  for (neighbors_t::const_iterator n_it = m_neighbors.begin();
294  n_it != m_neighbors.end();
295  ++n_it) {
296  TNeighborAgentMapProps& neighbor = **n_it;
297  CWindowManager* neighbor_win_manager = m_neighbors_to_windows.at(*n_it);
298 
299  // reset visuals for each neighbor's window
300  removeObjectFrom3DScene(neighbor_win_manager->win);
301  addSupWidgets(neighbor_win_manager->win);
302 
303  if (neighbor.nav_map && neighbor.nav_robot_trajectory) {
304  //
305  // map
306  //
307  COccupancyGridMap2D::Ptr map = COccupancyGridMap2D::Create();
308  m_logger->logFmt(LVL_INFO, "Adding map of agent \"%s\" to the stack",
309  neighbor.agent.topic_namespace.data.c_str());
310  convert(*neighbor.nav_map, *map);
311 
312  // visualize map in corresponding window
313  addToWindow(neighbor_win_manager->win, *map);
314  mrpt_gridmaps.insert(make_pair(*n_it, map));
315 
316  //
317  // trajectory
318  //
319  CSetOfLines::Ptr curr_traj = CSetOfLines::Create();
320  curr_traj->setPose(CPose3D(0,0,0.3,0,0,0));
321  curr_traj->setLineWidth(1.5);
322  curr_traj->setColor(TColorf(0,0,1));
323  // append a dummy line so that you can later use append using
324  // CSetOfLines::appendLienStrip method.
325  curr_traj->appendLine(
326  /* 1st */ 0, 0, 0,
327  /* 2nd */ 0, 0, 0);
328 
329  for (nav_msgs::Path::_poses_type::const_iterator
330  pth_it = neighbor.nav_robot_trajectory->poses.begin();
331  pth_it != neighbor.nav_robot_trajectory->poses.end();
332  ++pth_it) {
333  curr_traj->appendLineStrip(pth_it->pose.position.x, pth_it->pose.position.y, 0);
334  }
335  // visualize trajectory
336  {
337  COpenGLScene::Ptr &scene = neighbor_win_manager->win->get3DSceneAndLock();
338  scene->insert(curr_traj);
339 
340  neighbor_win_manager->win->unlockAccess3DScene();
341  neighbor_win_manager->win->forceRepaint();
342  }
343 
344  // cache trajectory so that I can later visualize it in the fused map
345  {
346  // operate on copy of object - it is already inserted and used in
347  // another window
348  auto tmp = CSetOfLines::Ptr(dynamic_cast<CSetOfLines*>(curr_traj->clone()));
349  auto curr_traj = mrpt::ptr_cast<CSetOfLines>::from(tmp);
350  ASSERT_(curr_traj);
351  mrpt_trajectories.insert(make_pair(&neighbor, curr_traj));
352  }
353 
354  } // end if both nav_map and nav_robot_trajectory exist
355  } // end for all neighbors traversal
356 
357  // join all gathered MRPT gridmaps
358  if (mrpt_gridmaps.size() >= 2) {
359 
360  m_logger->logFmt(LVL_INFO, "Executing map merging for [%lu] gridmaps",
361  static_cast<unsigned long>(mrpt_gridmaps.size()));
362  int merge_counter = 0;
363 
364  // save results
365  std::string output_dir_fname = "map_merger_results";
367  m_logger->logFmt(LVL_INFO,
368  "Saving map-merging results to \"%s\"",
369  output_dir_fname.c_str());
370 
371  //mrpt::system::TTimeStamp cur_date(getCurrentTime());
372  //string cur_date_str(dateTimeToString(cur_date));
373  //string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
374  //std::string output_dir_fname = "map_merger_results_" + cur_date_validstr;
375  bool does_exist = directoryExists(output_dir_fname);
376  if (does_exist) {
377  deleteFilesInDirectory(output_dir_fname);
378  }
379  else {
380  m_logger->logFmt(LVL_INFO, "Initializing gridmaps output directory.\n");
381  createDirectory(output_dir_fname);
382  }
383  }
384 
385  // initialize final fused map
386  COccupancyGridMap2D::Ptr fused_map = COccupancyGridMap2D::Create();
387  fused_map->copyMapContentFrom(*mrpt_gridmaps.begin()->second);
388 
389  ASSERT_(fused_map);
390 
391  {
392  // clear the fused map visuals
393  COpenGLScene::Ptr& fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
394  fused_scene.reset();
395  m_logger->logFmt(LVL_INFO, "Clearing the fused map visuals");
396 
398 
399  // add first map
400  CSetOfObjects::Ptr first_map_obj = CSetOfObjects::Create();
401  fused_map->getAs3DObject(first_map_obj);
402  // each map in the fused display will have a different name - based on
403  // the topic namespace
404  first_map_obj->setName(format("map_%s",
405  mrpt_gridmaps.begin()->first->agent.topic_namespace.data.c_str()));
406  fused_scene->insert(first_map_obj);
407 
410  }
411 
412  // save the first gridmap
414  stringstream ss;
415  ss << output_dir_fname << "/" << "map" << merge_counter;
416  (mrpt_gridmaps.begin()->second)->saveMetricMapRepresentationToFile(ss.str());
417  }
418 
419  // value at which to display each new gridmap in the display window
420  double off_z = 1;
421  double off_z_step = 1;
422 
423  // map of TNeighborAgentMapProps* to a corresponding flag specifying
424  // whether the COccupancyGridMap is correctly aligned can thus can be used.
425  neighbor_to_is_used_t neighbor_to_is_used;
426  // first gridmap frame coincedes with the global frame - used anyway
427 
428  // map from TNeighborAgentMapProps* to a corresponding relative pose with
429  // regards to the global fused map
430  neighbor_to_rel_pose_t neighbor_to_rel_pose;
431  for (neighbors_t::const_iterator
432  n_it = m_neighbors.begin();
433  n_it != m_neighbors.end();
434  ++n_it) {
435  neighbor_to_is_used[*n_it] = false;
436  neighbor_to_rel_pose[*n_it] = CPose2D();
437  }
438 
439  // mark first as used - one trajectory should be there anyway
440  neighbor_to_is_used[*m_neighbors.begin()] = true;
441 
442  // for all captured gridmaps - except the first
443  for (maps_t::iterator
444  m_it = std::next(mrpt_gridmaps.begin());
445  m_it != mrpt_gridmaps.end();
446  ++m_it, ++merge_counter) {
447 
448  TNeighborAgentMapProps* curr_neighbor = m_it->first;
449  COccupancyGridMap2D* curr_gridmap = m_it->second.get();
450 
451  // run alignment procedure
453  float run_time = 0;
454  CPosePDFGaussian init_estim;
455  m_logger->logFmt(LVL_INFO,
456  "Trying to align the maps, initial estimation: %s",
457  init_estim.mean.asString().c_str());
458  const CPosePDF::Ptr pdf_tmp = gridmap_aligner.AlignPDF(
459  fused_map.get(), curr_gridmap,
460  init_estim,
461  &run_time, &results);
462  m_logger->logFmt(LVL_INFO, "Elapsed Time: %f", run_time);
463 
464 
465  CPosePDFSOG::Ptr pdf_out = CPosePDFSOG::Create();
466  pdf_out->copyFrom(*pdf_tmp);
467 
468  CPose2D pose_out; CMatrixDouble33 cov_out;
469  pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out);
470 
471  // dismiss this?
472  if (results.goodness > 0.999 || isEssentiallyZero(pose_out)) {
473  continue;
474  }
475 
476  neighbor_to_rel_pose.at(curr_neighbor) = pose_out;
477  neighbor_to_is_used.at(curr_neighbor) = true;
478 
479  m_logger->logFmt(LVL_INFO, "%s\n",
480  getGridMapAlignmentResultsAsString(*pdf_tmp, results).c_str());
481 
482  // save current gridmap
484  stringstream ss;
485  ss << output_dir_fname << "/" << "map" << merge_counter;
486  curr_gridmap->saveMetricMapRepresentationToFile(ss.str());
487  }
488 
489  // save correspondences image
491  stringstream ss;
492  ss << output_dir_fname << "/" << "fusing_proc_with"
493  << "_" << merge_counter;
494  COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
495  ss.str(), fused_map.get(), curr_gridmap, results.correspondences);
496  }
497 
498  // Add current map to the fused map visualization
499  {
500  COpenGLScene::Ptr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
501 
502  // insert current map
503  m_logger->logFmt(LVL_INFO,
504  "Adding map to the fused map visualiztion using transformation %s",
505  pose_out.asString().c_str());
506  CSetOfObjects::Ptr curr_map_obj = CSetOfObjects::Create();
507  curr_gridmap->getAs3DObject(curr_map_obj);
508  curr_map_obj->setName(format("map_%s",
509  curr_neighbor->agent.topic_namespace.data.c_str()));
510  curr_map_obj->setPose(pose_out + CPose3D(0,0,off_z,0,0,0));
511  off_z += off_z_step;
512  fused_scene->insert(curr_map_obj);
513 
516  }
517 
518  // TODO - merge current gridmap into the fused map by using the found
519  // transformation
520 
521  } // end for all gridmaps
522 
523  TColorManager traj_color_mngr; // different color to each trajectory
524  // Traverse and add all the trajectories to the fused map visualization
525  for (trajectories_t::const_iterator
526  it = mrpt_trajectories.begin();
527  it != mrpt_trajectories.end();
528  ++it) {
529 
530  TNeighborAgentMapProps* curr_neighbor = it->first;
531 
532  if (!neighbor_to_is_used.at(curr_neighbor)) {
533  m_logger->logFmt(LVL_WARN, "Skipping visualizing trajectory of agent %s in the fused map",
534  curr_neighbor->agent.topic_namespace.data.c_str());
535  continue;
536  }
537 
538  CSetOfLines::Ptr curr_traj = it->second;
539  m_logger->logFmt(LVL_WARN, "Adding #%lu lines...",
540  static_cast<unsigned long>(curr_traj->getLineCount()));
541 
542  // set the pose of the trajectory
543  CPose3D rel_pose(neighbor_to_rel_pose.at(curr_neighbor));
544  // elevate by the last used offset
545  rel_pose += CPose3D(0,0,off_z,0,0,0);
546 
547  curr_traj->setColor(traj_color_mngr.getNextTColorf());
548  curr_traj->setPose(rel_pose);
549  curr_traj->setName(format("traj_%s", curr_neighbor->agent.topic_namespace.data.c_str()));
550  { // save 3D Scene
551  COpenGLScene::Ptr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
552  fused_scene->insert(curr_traj);
553 
556  }
557 
558  }
559 
560 
561  { // save the COpenGLScene
562 
563  COpenGLScene::Ptr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
564  std::string fname = output_dir_fname + "/" + "output_scene.3DScene";
565  fused_scene->saveToFile(fname);
566 
569  }
570 
571 
572  // save final fused map
573  // TODO
574 
575 
576  } // end if gridmap.size() >= 2
577  MRPT_END;
578 }
579 
581  mrpt::graphslam::CWindowManager* win_manager = new CWindowManager();
582  this->initWindowVisuals(win_manager);
583  return win_manager;
584 }
585 
587  mrpt::graphslam::CWindowManager* win_manager) {
588  using namespace mrpt::opengl;
589  using namespace mrpt::gui;
590  using namespace mrpt::utils;
591  using namespace mrpt::graphslam;
592  ASSERT_(win_manager);
593 
594  CWindowObserver* win_observer = new CWindowObserver();
596  "GraphSlam building procedure", 800, 600);
597  win->setPos(400, 200);
598  win_observer->observeBegin(*win);
599  {
600  COpenGLScene::Ptr &scene = win->get3DSceneAndLock();
601  COpenGLViewport::Ptr main_view = scene->getViewport("main");
602  win_observer->observeBegin(*main_view);
603  win->unlockAccess3DScene();
604  }
605 
606  // pass the window and the observer pointers to the CWindowManager instance
607  win_manager->setCDisplayWindow3DPtr(win);
608  win_manager->setWindowObserverPtr(win_observer);
609 
610  addSupWidgets(win_manager->win);
611  m_logger->logFmt(LVL_DEBUG, "Initialized CDisplayWindow3D...");
612 }
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
bool BASE_IMPEXP createDirectory(const std::string &dirName)
std::map< TNeighborAgentMapProps *, CWindowManager * > m_neighbors_to_windows
Definition: CMapMerger.h:92
void monitorKeystrokes(mrpt::graphslam::CWindowObserver *win_observer)
Compact method for monitoring the given keystrokes for the given observer.
Definition: CMapMerger.cpp:162
const mrpt::poses::CPose3D EMPTY_POSE
Definition: CMapMerger.h:48
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
std::string map_merge_keypress
Definition: CMapMerger.h:110
mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options
Definition: CMapMerger.h:106
mrpt::graphslam::detail::CConnectionManager m_conn_manager
Definition: CMapMerger.h:95
scene
void setWindowObserverPtr(mrpt::graphslam::CWindowObserver *obsever_in)
void setWindowTitle(const std::string &str) MRPT_OVERRIDE
void addToWindow(mrpt::gui::CDisplayWindow3D *win, const RENDERIZABLE_OBJECT &o, const std::string &obj_name="", const mrpt::poses::CPose3D &obj_pose=EMPTY_POSE)
Definition: CMapMerger.cpp:68
CMapMerger(mrpt::utils::COutputLogger *logger_in, ros::NodeHandle *nh_in)
Definition: CMapMerger.cpp:128
void observeBegin(CObservable &obj)
mrpt::poses::CPosePDFPtr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
std::string getGridMapAlignmentResultsAsString(const mrpt::poses::CPosePDF &pdf, const mrpt::slam::CGridMapAligner::TReturnInfo &ret_info)
Definition: common.cpp:26
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
nav_msgs::Path::ConstPtr nav_robot_trajectory
Trajectory of the correspondign agent - in ROS form.
#define MRPT_END
void addAxis(mrpt::gui::CDisplayWindow3D *win)
Definition: CMapMerger.cpp:90
std::map< TNeighborAgentMapProps *, COccupancyGridMap2D::Ptr > maps_t
Definition: CMapMerger.h:61
std::string m_global_ns
Topic namespace under which current node is going to be publishing.
Definition: CMapMerger.h:99
mrpt::graphslam::CWindowManager * m_fused_map_win_manager
Definition: CMapMerger.h:114
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
GLhandleARB obj
const mrpt_msgs::GraphSlamAgent & agent
Pointer to the GraphSlamAgent instance of the neighbor.
neighbors_t m_neighbors
CConnectionManager instance for fetching the running graphSLAM agents.
Definition: CMapMerger.h:91
static CGridPlaneXYPtr Create(float xMin, float xMax, float yMin, float yMax, float z=0, float frequency=1, float lineWidth=1.3f, bool antiAliasing=true)
bool isEssentiallyZero(const mrpt::poses::CPose2D &p)
Definition: common.cpp:50
std::map< TNeighborAgentMapProps *, mrpt::opengl::CSetOfLines::Ptr > trajectories_t
Robot trajectory visual object type.
Definition: CMapMerger.h:68
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
Struct holding a COccupancyGridMap2D (as well as subscribers for updating it) for a specific running ...
std::vector< CMatrixD > results
mrpt::gui::CDisplayWindow3D * win
GLuint res
nav_msgs::OccupancyGrid::ConstPtr nav_map
Map generated by the corresponding agent - in ROS form.
void returnEventsStruct(std::map< std::string, bool > *codes_to_pressed, bool reset_keypresses=true)
#define MRPT_START
mrpt::utils::TColorf getNextTColorf()
mrpt::graphslam::CWindowObserver * observer
mrpt::slam::CGridMapAligner::TConfigParams options
ros::NodeHandle * m_nh
Definition: CMapMerger.h:94
void getNearbySlamAgents(mrpt_msgs::GraphSlamAgents *agents_vec, bool ignore_self=true)
Fill the given vector with the SLAM Agents that the current manager can see and communicate with...
#define ASSERT_(f)
void addSupWidgets(mrpt::gui::CDisplayWindow3D *win)
Definition: CMapMerger.cpp:118
static CAST_TO::Ptr from(const CAST_FROM_PTR &ptr)
std::map< TNeighborAgentMapProps *, mrpt::poses::CPose2D > neighbor_to_rel_pose_t
Definition: CMapMerger.h:63
mrpt::gui::CDisplayWindow3DPtr win
bool BASE_IMPEXP deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
void registerKeystroke(const std::string key_str, const std::string key_desc)
void setCDisplayWindow3DPtr(mrpt::gui::CDisplayWindow3D *win_in)
static CAxisPtr Create(float xmin, float ymin, float zmin, float xmax, float ymax, float zmax, float frecuency=1, float lineWidth=3, bool marks=false)
string ss
mrpt::utils::COutputLogger * m_logger
Definition: CMapMerger.h:93
bool updateState()
Query and fetch the list of new graphSLAM agents.
Definition: CMapMerger.cpp:194
bool BASE_IMPEXP directoryExists(const std::string &fileName)
void setPos(int x, int y) MRPT_OVERRIDE
void addGrid(mrpt::gui::CDisplayWindow3D *win)
Definition: CMapMerger.cpp:103
bool removeObjectFrom3DScene(mrpt::gui::CDisplayWindow3D *win, std::string obj_name="")
Definition: CMapMerger.cpp:32
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
mrpt::utils::TMatchingPairList correspondences
mrpt::graphslam::CWindowManager * initWindowVisuals()
Definition: CMapMerger.cpp:580
std::map< TNeighborAgentMapProps *, bool > neighbor_to_is_used_t
Definition: CMapMerger.h:62
void asString(std::string &s) const


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sat May 2 2020 03:44:17