CMapMerger.cpp
Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)               |
00003    |                          http://www.mrpt.org/                             |
00004    |                                                                           |
00005    | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00007    | Released under BSD License. See details in http://www.mrpt.org/License    |
00008    +---------------------------------------------------------------------------+ */
00009 
00010 #include "mrpt_graphslam_2d/CMapMerger.h"
00011 
00012 using namespace mrpt::graphslam;
00013 using namespace mrpt::graphslam::detail;
00014 using namespace mrpt_msgs;
00015 using namespace mrpt::maps;
00016 using namespace mrpt::slam;
00017 using namespace mrpt::math;
00018 using namespace mrpt::system;
00019 using namespace mrpt::poses;
00020 using namespace mrpt::opengl;
00021 using namespace mrpt::utils;
00022 using namespace ros;
00023 using namespace nav_msgs;
00024 using namespace std;
00025 using namespace mrpt_bridge;
00026 
00027 // helper methods
00028 
00030 
00032 bool removeObjectFrom3DScene(
00033                 mrpt::gui::CDisplayWindow3D* win,
00034                 std::string obj_name="") {
00035         using namespace mrpt::opengl;
00036         ASSERT_(win);
00037         bool res = true;
00038         COpenGLScenePtr& scene = win->get3DSceneAndLock();
00039 
00040         if (obj_name.empty()) {
00041                 cout << "Clearing entire scene." << endl;
00042                 scene->clear();
00043         }
00044         else {
00045                 CRenderizablePtr obj = scene->getByName(obj_name);
00046                 if (obj) {
00047                         scene->removeObject(obj);
00048                 }
00049                 else {
00050                         res = false;
00051                 }
00052         }
00053 
00054         win->unlockAccess3DScene();
00055         win->forceRepaint();
00056         return res;
00057 }
00058 
00060 bool removeObjectFrom3DScene(
00061                 mrpt::graphslam::CWindowManager* win_manager,
00062                 std::string obj_name="") {
00063         ASSERT_(win_manager);
00064         return removeObjectFrom3DScene(win_manager->win, obj_name);
00065 }
00066 
00067 template<class RENDERIZABLE_OBJECT>
00068 void addToWindow(mrpt::gui::CDisplayWindow3D* win,
00069                 const RENDERIZABLE_OBJECT& o,
00070                 const std::string& obj_name="",
00071                 const mrpt::poses::CPose3D& obj_pose=EMPTY_POSE) {
00072         using namespace mrpt::opengl;
00073 
00074         COpenGLScenePtr &scene = win->get3DSceneAndLock();
00075         CSetOfObjectsPtr obj = CSetOfObjects::Create();
00076 
00077         o.getAs3DObject(obj);
00078         obj->setPose(obj_pose);
00079 
00080         if (!obj_name.empty()) {
00081                 obj->setName(obj_name);
00082         }
00083 
00084         scene->insert(obj);
00085 
00086         win->unlockAccess3DScene();
00087         win->forceRepaint();
00088 }
00089 
00090 void addAxis(mrpt::gui::CDisplayWindow3D* win) {
00091         using namespace mrpt;
00092         using namespace mrpt::opengl;
00093         ASSERT_(win);
00094 
00095         COpenGLScenePtr &scene = win->get3DSceneAndLock();
00096         opengl::CAxisPtr obj = opengl::CAxis::Create(-6, -6, -6, 6, 6, 6, 2, 2, true);
00097         obj->setLocation(0, 0, 0);
00098         scene->insert(obj);
00099         win->unlockAccess3DScene();
00100         win->forceRepaint();
00101 }
00102 
00103 void addGrid(mrpt::gui::CDisplayWindow3D* win) {
00104         using namespace mrpt;
00105         using namespace mrpt::opengl;
00106         ASSERT_(win);
00107 
00108         COpenGLScenePtr &scene = win->get3DSceneAndLock();
00109         opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1);
00110         obj->setColor(0.7, 0.7, 0.7);
00111         obj->setLocation(0, 0, 0);
00112         scene->insert(obj);
00113 
00114         win->unlockAccess3DScene();
00115         win->forceRepaint();
00116 }
00117 
00118 void addSupWidgets(mrpt::gui::CDisplayWindow3D* win) {
00119         ASSERT_(win);
00120 
00121         addAxis(win);
00122         addGrid(win);
00123 }
00124 
00126 
00127 
00128 CMapMerger::CMapMerger(
00129                 mrpt::utils::COutputLogger* logger_in,
00130                 ros::NodeHandle* nh_in):
00131         m_logger(logger_in),
00132         m_nh(nh_in),
00133         m_conn_manager(logger_in, nh_in),
00134         m_queue_size(1),
00135         quit_keypress1("q"),
00136         quit_keypress2("Ctrl+c"),
00137         map_merge_keypress("n"),
00138         save_map_merging_results(true)
00139 {
00140         ASSERT_(m_nh);
00141 
00142         m_global_ns = "/map_merger";
00143 
00144         // GridMap Alignment options to be used in merging.
00145         m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC;
00147         m_alignment_options.min_ICP_goodness = 0.60;
00148         m_alignment_options.maxKLd_for_merge = 0.90;
00149         m_alignment_options.ransac_minSetSizeRatio = 0.40;
00150         //m_alignment_options.loadFromConfigFileName(
00151                         //"/home/bergercookie/mrpt/share/mrpt/config_files/grid-matching/gridmatch_example.ini",
00152                         //"grid-match");
00153         m_alignment_options.dumpToConsole();
00154 
00155         // initialize CDisplayWindow for fused map
00156         m_fused_map_win_manager = this->initWindowVisuals();
00157         m_fused_map_win_manager->win->setWindowTitle("Fused map");
00158         this->monitorKeystrokes(m_fused_map_win_manager->observer);
00159 
00160 }
00161 
00162 void CMapMerger::monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer) {
00163         ASSERT_(win_observer);
00164 
00165         win_observer->registerKeystroke(quit_keypress1,
00166                         "Finish execution");
00167         win_observer->registerKeystroke(map_merge_keypress,
00168                         "Compute next available grid-merging");
00169 
00170 
00171 }
00172 
00173 CMapMerger::~CMapMerger() {
00174 
00175         // delete the generated neighbors intances
00176         for (neighbors_t::iterator n_it = m_neighbors.begin();
00177                         n_it != m_neighbors.end();
00178                         ++n_it) {
00179 
00180                 delete m_neighbors_to_windows.at(*n_it)->observer;
00181                 delete m_neighbors_to_windows.at(*n_it)->win;
00182                 delete m_neighbors_to_windows.at(*n_it);
00183                 delete *n_it;
00184         }
00185 
00186         // delete fused window manager
00187         delete m_fused_map_win_manager->win;
00188         delete m_fused_map_win_manager->observer;
00189         delete m_fused_map_win_manager;
00190 
00191         m_logger->logFmt(LVL_WARN, "Exiting...");
00192 }
00193 
00194 bool CMapMerger::updateState() {
00195         MRPT_START;
00196 
00197         // get the new GraphSlamAgents
00198         const   GraphSlamAgents& nearby_slam_agents =
00199                 m_conn_manager.getNearbySlamAgents();
00200         //m_logger->logFmt(LVL_DEBUG, "nearby_slam_agents size: %lu\n",
00201                         //static_cast<unsigned long>(nearby_slam_agents.list.size()));
00202 
00203         for (GraphSlamAgents::_list_type::const_iterator
00204                         it = nearby_slam_agents.list.begin();
00205                         it != nearby_slam_agents.list.end();
00206                         ++it) {
00207                 const GraphSlamAgent& gsa = *it;
00208 
00209                 // Is the current GraphSlamAgent already registered?
00210                 auto search = [gsa](const TNeighborAgentMapProps* neighbor) {
00211                         return (neighbor->agent == gsa);
00212                 };
00213                 typename neighbors_t::iterator n_it = find_if(
00214                                 m_neighbors.begin(),
00215                                 m_neighbors.end(), search);
00216 
00217                 if (n_it == m_neighbors.end()) { // current gsa not found, add it
00218 
00219                         m_neighbors.push_back(new TNeighborAgentMapProps(m_logger, gsa, m_nh));
00220                         TNeighborAgentMapProps* latest_neighbor = m_neighbors.back();
00221                         latest_neighbor->setupComm();
00222                         m_logger->logFmt(LVL_WARN,
00223                                         "Initialized NeighborAgentMapProps instance for agent %s...",
00224                                         latest_neighbor->agent.topic_namespace.data.c_str());
00225 
00226                         // initialize the window
00227                         mrpt::graphslam::CWindowManager* win_manager = initWindowVisuals();
00228                         win_manager->win->setWindowTitle(
00229                                         latest_neighbor->agent.topic_namespace.data);
00230                         m_neighbors_to_windows.insert(make_pair(latest_neighbor, win_manager));
00231                         this->monitorKeystrokes(win_manager->observer);
00232 
00233                 }
00234         } // end for all fetched GraphSlamAgents
00235 
00236         // run through the open windows - Exit if instructed
00237         bool continue_exec = true;
00238         for (map<TNeighborAgentMapProps*, CWindowManager*>::const_iterator
00239                         it = m_neighbors_to_windows.begin();
00240                         it != m_neighbors_to_windows.end();
00241                         ++it) {
00242 
00243                 CWindowManager* win_manager = it->second;
00244 
00245                 std::map<std::string, bool> events_map;
00246                 win_manager->observer->returnEventsStruct(&events_map);
00247 
00248                 if (events_map.at(map_merge_keypress)) {
00249                         mergeMaps();
00250                 }
00251                 win_manager->win->forceRepaint();
00252 
00253                 // continue or exit
00254                 if (!win_manager->isOpen() ||
00255                                 events_map.at(quit_keypress1) ||
00256                                 events_map.at(quit_keypress2)) {
00257                         continue_exec = false;
00258                         break;
00259                 }
00260         }
00261 
00262         // Fetch the events for the main (fused map) window
00263         if (continue_exec) {
00264                 std::map<std::string, bool> events_map;
00265                 m_fused_map_win_manager->observer->returnEventsStruct(&events_map);
00266                 if (events_map.at(map_merge_keypress)) {
00267                         mergeMaps();
00268                 }
00269                 m_fused_map_win_manager->win->forceRepaint();
00270                 continue_exec =
00271                         m_fused_map_win_manager->isOpen() &&
00272                         !events_map.at(quit_keypress1) &&
00273                         !events_map.at(quit_keypress2);
00274         }
00275 
00276         return continue_exec;
00277 
00278         MRPT_END;
00279 } // end of updateState
00280 
00281 void CMapMerger::mergeMaps() {
00282         MRPT_START;
00283         m_logger->logFmt(LVL_WARN, "In mergeMaps.");
00284 
00285         CGridMapAligner gridmap_aligner;
00286         gridmap_aligner.options = m_alignment_options;
00287 
00288         // List of maps that is to be filled.
00289         maps_t mrpt_gridmaps;
00290         trajectories_t mrpt_trajectories;
00291 
00292         // traverse Neighbor instances - get their nav_msgs::OccupancyGrid maps, trajectories
00293         for (neighbors_t::const_iterator n_it = m_neighbors.begin();
00294                         n_it != m_neighbors.end();
00295                         ++n_it) {
00296                 TNeighborAgentMapProps& neighbor = **n_it;
00297                 CWindowManager* neighbor_win_manager = m_neighbors_to_windows.at(*n_it);
00298 
00299                 // reset visuals for each neighbor's window
00300                 removeObjectFrom3DScene(neighbor_win_manager->win);
00301                 addSupWidgets(neighbor_win_manager->win);
00302 
00303                 if (neighbor.nav_map && neighbor.nav_robot_trajectory) {
00304                         //
00305                         // map
00306                         //
00307                         COccupancyGridMap2DPtr map = COccupancyGridMap2D::Create();
00308                         m_logger->logFmt(LVL_INFO, "Adding map of agent \"%s\" to the stack",
00309                                         neighbor.agent.topic_namespace.data.c_str());
00310                         convert(*neighbor.nav_map, *map);
00311 
00312                         // visualize map in corresponding window
00313                         addToWindow(neighbor_win_manager->win, *map);
00314                         mrpt_gridmaps.insert(make_pair(*n_it, map));
00315 
00316                         //
00317                         // trajectory
00318                         //
00319                         CSetOfLinesPtr curr_traj = CSetOfLines::Create();
00320                         curr_traj->setPose(CPose3D(0,0,0.3,0,0,0));
00321                         curr_traj->setLineWidth(1.5);
00322                         curr_traj->setColor(TColorf(0,0,1));
00323                         // append a dummy line so that you can later use append using
00324                         // CSetOfLines::appendLienStrip method.
00325                         curr_traj->appendLine(
00326                                         /* 1st */ 0, 0, 0,
00327                                         /* 2nd */ 0, 0, 0);
00328 
00329                         for (nav_msgs::Path::_poses_type::const_iterator
00330                                         pth_it = neighbor.nav_robot_trajectory->poses.begin();
00331                                         pth_it != neighbor.nav_robot_trajectory->poses.end();
00332                                         ++pth_it) {
00333                                 curr_traj->appendLineStrip(pth_it->pose.position.x, pth_it->pose.position.y, 0);
00334                         }
00335                         // visualize trajectory
00336                         {
00337                                 COpenGLScenePtr &scene = neighbor_win_manager->win->get3DSceneAndLock();
00338                                 scene->insert(curr_traj);
00339 
00340                                 neighbor_win_manager->win->unlockAccess3DScene();
00341                                 neighbor_win_manager->win->forceRepaint();
00342                         }
00343 
00344                         // cache trajectory so that I can later visualize it in the fused map
00345                         {
00346                                 // operate on copy of object - it is already inserted and used in
00347                                 // another window
00348                                 CObjectPtr tmp = curr_traj->duplicateGetSmartPtr();
00349                                 CSetOfLinesPtr curr_traj = static_cast<CSetOfLinesPtr>(tmp);
00350                                 ASSERT_(curr_traj);
00351                                 mrpt_trajectories.insert(make_pair(&neighbor, curr_traj));
00352                         }
00353 
00354                 } // end if both nav_map and nav_robot_trajectory exist
00355         } // end for all neighbors traversal
00356 
00357         // join all gathered MRPT gridmaps 
00358         if (mrpt_gridmaps.size() >= 2) {
00359 
00360                 m_logger->logFmt(LVL_INFO, "Executing map merging for [%lu] gridmaps",
00361                                 static_cast<unsigned long>(mrpt_gridmaps.size()));
00362                 int merge_counter = 0;
00363 
00364                 // save results
00365                 std::string output_dir_fname = "map_merger_results";
00366                 if (save_map_merging_results) {
00367                         m_logger->logFmt(LVL_INFO,
00368                                         "Saving map-merging results to \"%s\"",
00369                                         output_dir_fname.c_str());
00370 
00371                         mrpt::system::TTimeStamp cur_date(getCurrentTime());
00372                         //string cur_date_str(dateTimeToString(cur_date));
00373                         //string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));
00374                         //std::string output_dir_fname = "map_merger_results_" + cur_date_validstr;
00375                         bool does_exist = directoryExists(output_dir_fname);
00376                         if (does_exist) {
00377                                 deleteFilesInDirectory(output_dir_fname);
00378                         }
00379                         else {
00380                                 m_logger->logFmt(LVL_INFO, "Initializing gridmaps output directory.\n");
00381                                 createDirectory(output_dir_fname);
00382                         }
00383                 }
00384 
00385                 // initialize final fused map
00386                 COccupancyGridMap2DPtr fused_map = COccupancyGridMap2D::Create();
00387                 fused_map->copyMapContentFrom(*mrpt_gridmaps.begin()->second);
00388                 ASSERT_(fused_map.present());
00389 
00390                 {
00391                         // clear the fused map visuals
00392                         COpenGLScenePtr& fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
00393                         fused_scene->clear();
00394                         m_logger->logFmt(LVL_INFO, "Clearing the fused map visuals");
00395 
00396                         addSupWidgets(m_fused_map_win_manager->win);
00397 
00398                         // add first map
00399                         CSetOfObjectsPtr first_map_obj = CSetOfObjects::Create();
00400                         fused_map->getAs3DObject(first_map_obj);
00401                         // each map in the fused display will have a different name - based on
00402                         // the topic namespace
00403                         first_map_obj->setName(format("map_%s",
00404                                                 mrpt_gridmaps.begin()->first->agent.topic_namespace.data.c_str()));
00405                         fused_scene->insert(first_map_obj);
00406 
00407                         m_fused_map_win_manager->win->unlockAccess3DScene();
00408                         m_fused_map_win_manager->win->forceRepaint();
00409                 }
00410 
00411                 // save the first gridmap
00412                 if (save_map_merging_results) {
00413                         stringstream ss;
00414                         ss << output_dir_fname  << "/" << "map" << merge_counter;
00415                         (mrpt_gridmaps.begin()->second)->saveMetricMapRepresentationToFile(ss.str());
00416                 }
00417 
00418                 // value at which to display each new gridmap in the display window
00419                 double off_z = 1;
00420                 double off_z_step = 1;
00421 
00422                 // map of TNeighborAgentMapProps* to a corresponding flag specifying
00423                 // whether the COccupancyGridMap is correctly aligned can thus can be used.
00424                 neighbor_to_is_used_t neighbor_to_is_used;
00425                 // first gridmap frame coincedes with the global frame - used anyway
00426 
00427                 // map from TNeighborAgentMapProps* to a corresponding  relative pose with
00428                 // regards to the global fused map
00429                 neighbor_to_rel_pose_t neighbor_to_rel_pose;
00430                 for (neighbors_t::const_iterator
00431                                 n_it = m_neighbors.begin();
00432                                 n_it != m_neighbors.end();
00433                                 ++n_it) {
00434                         neighbor_to_is_used[*n_it] = false;
00435                         neighbor_to_rel_pose[*n_it] = CPose2D();
00436                 }
00437 
00438                 // mark first as used - one trajectory should be there anyway
00439                 neighbor_to_is_used[*m_neighbors.begin()] = true;
00440 
00441                 // for all captured gridmaps - except the first
00442                 for (maps_t::iterator
00443                                 m_it = std::next(mrpt_gridmaps.begin());
00444                                 m_it != mrpt_gridmaps.end();
00445                                 ++m_it, ++merge_counter) {
00446 
00447                         TNeighborAgentMapProps* curr_neighbor = m_it->first;
00448                         COccupancyGridMap2D* curr_gridmap = m_it->second.pointer();
00449 
00450                         // run alignment procedure
00451                         CGridMapAligner::TReturnInfo results;
00452                         float run_time = 0;
00453                         CPosePDFGaussian init_estim;
00454                         m_logger->logFmt(LVL_INFO,
00455                                         "Trying to align the maps, initial estimation: %s",
00456                                         init_estim.mean.asString().c_str());
00457                         const CPosePDFPtr pdf_tmp = gridmap_aligner.AlignPDF(
00458                                         fused_map.pointer(), curr_gridmap,
00459                                         init_estim,
00460                                         &run_time, &results);
00461                         m_logger->logFmt(LVL_INFO, "Elapsed Time: %f", run_time);
00462 
00463 
00464                         CPosePDFSOGPtr pdf_out = CPosePDFSOG::Create();
00465                         pdf_out->copyFrom(*pdf_tmp);
00466 
00467                         CPose2D pose_out; CMatrixDouble33 cov_out;
00468                         pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out);
00469 
00470                         // dismiss this?
00471                         if (results.goodness > 0.999 || isEssentiallyZero(pose_out)) {
00472                                 continue;
00473                         }
00474 
00475                         neighbor_to_rel_pose.at(curr_neighbor) = pose_out;
00476                         neighbor_to_is_used.at(curr_neighbor) = true;
00477 
00478                         m_logger->logFmt(LVL_INFO, "%s\n",
00479                                         getGridMapAlignmentResultsAsString(*pdf_tmp, results).c_str());
00480 
00481                         // save current gridmap
00482                         if (save_map_merging_results) {
00483                                 stringstream ss;
00484                                 ss << output_dir_fname  << "/" << "map" << merge_counter;
00485                                 curr_gridmap->saveMetricMapRepresentationToFile(ss.str());
00486                         }
00487 
00488                         // save correspondences image
00489                         if (save_map_merging_results) {
00490                                 stringstream ss;
00491                                 ss << output_dir_fname << "/" << "fusing_proc_with"
00492                                         << "_" << merge_counter;
00493                                 COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
00494                                                 ss.str(), fused_map.pointer(), curr_gridmap, results.correspondences);
00495                         }
00496 
00497                         // Add current map to the fused map visualization
00498                         {
00499                                 COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
00500 
00501                                 // insert current map
00502                                 m_logger->logFmt(LVL_INFO,
00503                                                 "Adding map to the fused map visualiztion using transformation %s",
00504                                                 pose_out.asString().c_str());
00505                                 CSetOfObjectsPtr curr_map_obj = CSetOfObjects::Create();
00506                                 curr_gridmap->getAs3DObject(curr_map_obj);
00507                                 curr_map_obj->setName(format("map_%s",
00508                                                 curr_neighbor->agent.topic_namespace.data.c_str()));
00509                                 curr_map_obj->setPose(pose_out + CPose3D(0,0,off_z,0,0,0));
00510                                 off_z += off_z_step;
00511                                 fused_scene->insert(curr_map_obj);
00512 
00513                                 m_fused_map_win_manager->win->unlockAccess3DScene();
00514                                 m_fused_map_win_manager->win->forceRepaint();
00515                         }
00516 
00517                         // TODO - merge current gridmap into the fused map by using the found
00518                         // transformation
00519 
00520                 } // end for all gridmaps
00521 
00522                 TColorManager traj_color_mngr; // different color to each trajectory
00523                 // Traverse and add all the trajectories to the fused map visualization
00524                 for (trajectories_t::const_iterator
00525                                 it = mrpt_trajectories.begin();
00526                                 it != mrpt_trajectories.end();
00527                                 ++it) {
00528 
00529                         TNeighborAgentMapProps* curr_neighbor = it->first;
00530 
00531                         if (!neighbor_to_is_used.at(curr_neighbor)) {
00532                                 m_logger->logFmt(LVL_WARN, "Skipping visualizing trajectory of agent %s in the fused map",
00533                                                 curr_neighbor->agent.topic_namespace.data.c_str());
00534                                 continue;
00535                         }
00536 
00537                         CSetOfLinesPtr curr_traj = it->second;
00538                         m_logger->logFmt(LVL_WARN, "Adding #%lu lines...",
00539                                         static_cast<unsigned long>(curr_traj->getLineCount()));
00540 
00541                         // set the pose of the trajectory
00542                         CPose3D rel_pose(neighbor_to_rel_pose.at(curr_neighbor));
00543                         // elevate by the last used offset
00544                         rel_pose += CPose3D(0,0,off_z,0,0,0);
00545 
00546                         curr_traj->setColor(traj_color_mngr.getNextTColorf());
00547                         curr_traj->setPose(rel_pose);
00548                         curr_traj->setName(format("traj_%s", curr_neighbor->agent.topic_namespace.data.c_str()));
00549                         { // save 3D Scene
00550                                 COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
00551                                 fused_scene->insert(curr_traj);
00552 
00553                                 m_fused_map_win_manager->win->unlockAccess3DScene();
00554                                 m_fused_map_win_manager->win->forceRepaint();
00555                         }
00556 
00557                 }
00558 
00559 
00560                 { // save the COpenGLScene
00561 
00562                         COpenGLScenePtr fused_scene = m_fused_map_win_manager->win->get3DSceneAndLock();
00563                         std::string fname = output_dir_fname + "/" + "output_scene.3DScene";
00564                         fused_scene->saveToFile(fname);
00565 
00566                         m_fused_map_win_manager->win->unlockAccess3DScene();
00567                         m_fused_map_win_manager->win->forceRepaint();
00568                 }
00569 
00570 
00571                 // save final fused map
00572                 // TODO
00573 
00574 
00575         } // end if gridmap.size() >= 2
00576         MRPT_END;
00577 }
00578 
00579 CWindowManager* CMapMerger::initWindowVisuals() {
00580         mrpt::graphslam::CWindowManager* win_manager = new CWindowManager();
00581         this->initWindowVisuals(win_manager);
00582         return win_manager;
00583 }
00584 
00585 void CMapMerger::initWindowVisuals(
00586                 mrpt::graphslam::CWindowManager* win_manager) {
00587         using namespace mrpt::opengl;
00588         using namespace mrpt::gui;
00589         using namespace mrpt::utils;
00590         using namespace mrpt::graphslam;
00591         ASSERT_(win_manager);
00592 
00593         CWindowObserver* win_observer = new CWindowObserver();
00594         CDisplayWindow3D* win = new CDisplayWindow3D(
00595                         "GraphSlam building procedure", 800, 600);
00596         win->setPos(400, 200);
00597         win_observer->observeBegin(*win);
00598         {
00599                 COpenGLScenePtr &scene = win->get3DSceneAndLock();
00600                 COpenGLViewportPtr main_view = scene->getViewport("main");
00601                 win_observer->observeBegin(*main_view);
00602                 win->unlockAccess3DScene();
00603         }
00604 
00605         // pass the window and the observer pointers to the CWindowManager instance
00606         win_manager->setCDisplayWindow3DPtr(win);
00607         win_manager->setWindowObserverPtr(win_observer);
00608 
00609         addSupWidgets(win_manager->win);
00610         m_logger->logFmt(LVL_DEBUG, "Initialized CDisplayWindow3D...");
00611 }


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sun Sep 17 2017 03:02:04