CGraphSlamEngine_MR_impl.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
                                 |                     Mobile Robot Programming Toolkit (MRPT) |
   | http://www.mrpt.org/                             | | | | Copyright (c)
   2005-2016, Individual contributors, see AUTHORS file        | | See:
   http://www.mrpt.org/Authors - All rights reserved.                   | |
   Released under BSD License. See details in http://www.mrpt.org/License    |
                                 +---------------------------------------------------------------------------+
 */

#pragma once

#include <chrono>
#include <mrpt/version.h>
#include <thread>

#if MRPT_VERSION >= 0x199
#include <mrpt/math/CMatrixFixed.h>
namespace mrpt::math {
// backwards compatible
template <typename T, std::size_t ROWS, std::size_t COLS>
using CMatrixFixedNumeric = CMatrixFixed<T, ROWS, COLS>;
} // namespace mrpt::math
#endif

namespace mrpt {
namespace graphslam {

template <class GRAPH_T>
CGraphSlamEngine_MR<GRAPH_T>::CGraphSlamEngine_MR(
    ros::NodeHandle *nh, const std::string &config_file,
    const std::string &rawlog_fname /* ="" */,
    const std::string &fname_GT /* ="" */,
    mrpt::graphslam::CWindowManager *win_manager /* = NULL */,
    mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>
        *node_reg /* = NULL */,
    mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>
        *edge_reg /* = NULL */,
    mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>
        *optimizer /* = NULL */)
    : parent_t::CGraphSlamEngine_ROS(nh, config_file, rawlog_fname, fname_GT,
                                     win_manager, node_reg, edge_reg,
                                     optimizer),
      m_conn_manager(dynamic_cast<mrpt::utils::COutputLogger *>(this), nh),
      m_nh(nh), m_graph_nodes_last_size(0), m_registered_multiple_nodes(false),
      cm_graph_async_spinner(/* threads_num: */ 1, &this->custom_service_queue),
      m_sec_alignment_params("AlignmentParameters"),
      m_sec_mr_slam_params("MultiRobotParameters"), m_opts(*this) {
  this->initClass();
}

template <class GRAPH_T> CGraphSlamEngine_MR<GRAPH_T>::~CGraphSlamEngine_MR() {
  MRPT_LOG_DEBUG_STREAM(
      "In Destructor: Deleting CGraphSlamEngine_MR instance...");
  cm_graph_async_spinner.stop();

  for (typename neighbors_t::iterator neighbors_it = m_neighbors.begin();
       neighbors_it != m_neighbors.end(); ++neighbors_it) {
        delete *neighbors_it;
  }
}

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::addNodeBatchesFromAllNeighbors() {
  MRPT_START;
  using namespace mrpt::graphslam::detail;

  bool ret_val = false;
  for (const auto &neighbor : m_neighbors) {
        if (!isEssentiallyZero(
                neighbor
                    ->tf_self_to_neighbor_first_integrated_pose) && // inter-graph
                                                                    // TF found
            neighbor->hasNewData() &&
            neighbor->hasNewNodesBatch(m_opts.nodes_integration_batch_size)) {
                bool loc_ret_val = this->addNodeBatchFromNeighbor(neighbor);
          if (loc_ret_val) {
                ret_val = true;
          }
        }
  }

  return ret_val;
  MRPT_END;
} // end of addNodeBatchesFromAllNeighbors

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::addNodeBatchFromNeighbor(
    TNeighborAgentProps *neighbor) {
  MRPT_START;
  ASSERT_(neighbor);
  using namespace mrpt::utils;
  using namespace mrpt::poses;
  using namespace mrpt::math;
  using namespace std;

  std::vector<TNodeID> nodeIDs;
  std::map<TNodeID, node_props_t> nodes_params;
  neighbor->getCachedNodes(&nodeIDs, &nodes_params, /*only unused=*/true);

  //
  // get the condensed measurements graph of the new nodeIDs
  //
  mrpt_msgs::GetCMGraph cm_graph_srv;
  // mrpt_msgs::GetCMGraphRequest::_nodeIDs_type& cm_graph_nodes =
  // cm_graph_srv.request.nodeIDs;
  for (const auto n : nodeIDs) {
        cm_graph_srv.request.nodeIDs.push_back(n);
  }

  MRPT_LOG_DEBUG_STREAM("Asking for the graph.");
  bool res = neighbor->cm_graph_srvclient.call(cm_graph_srv); // blocking
  if (!res) {
        MRPT_LOG_ERROR_STREAM("Service call for CM_Graph failed.");
        return false; // skip this if failed to fetch other's graph
  }
  MRPT_LOG_DEBUG_STREAM("Fetched graph successfully.");
  MRPT_LOG_DEBUG_STREAM(cm_graph_srv.response.cm_graph);

  GRAPH_T other_graph;
  mrpt_bridge::convert(cm_graph_srv.response.cm_graph, other_graph);

  //
  // merge batch of nodes in own graph
  //
  MRPT_LOG_WARN_STREAM(
      "Merging new batch from \""
      << neighbor->getAgentNs() << "\"..." << endl
      << "Batch: " << getSTLContainerAsString(cm_graph_srv.request.nodeIDs));
  hypots_t graph_conns;
  // build a hypothesis connecting the new batch with the last integrated
  // pose of the neighbor
  {
        pair<TNodeID, node_props_t> node_props_to_connect = *nodes_params.begin();

        hypot_t graph_conn;
        constraint_t c;
        c.mean = node_props_to_connect.second.pose -
                 neighbor->last_integrated_pair_neighbor_frame.second;
#if MRPT_VERSION >= 0x199
        c.cov_inv.setIdentity();
#else
        c.cov_inv.unit();
#endif
        graph_conn.setEdge(c);

        graph_conn.from = INVALID_NODEID;
        // get the nodeID of the last integrated neighbor node after remapping
        // in own graph numbering
        for (const auto n : this->m_graph.nodes) {
                if (n.second.agent_ID_str == neighbor->getAgentNs() &&
              n.second.nodeID_loc ==
                  neighbor->last_integrated_pair_neighbor_frame.first) {
                graph_conn.from = n.first;
                break;
          }
        }
        ASSERT_(graph_conn.from != INVALID_NODEID);
        graph_conn.to = node_props_to_connect.first; // other

        MRPT_LOG_DEBUG_STREAM(
            "Hypothesis for adding the batch of nodes: " << graph_conn);
        graph_conns.push_back(graph_conn);
  }

  std::map<TNodeID, TNodeID> old_to_new_mappings;
  this->m_graph.mergeGraph(other_graph, graph_conns,
                           /*hypots_from_other_to_self=*/false,
                           &old_to_new_mappings);

  //
  // Mark Nodes/LaserScans as integrated
  //
  MRPT_LOG_WARN_STREAM("Marking used nodes as integrated - Integrating LSs");
  nodes_to_scans2D_t new_nodeIDs_to_scans_pairs;
  for (typename std::vector<TNodeID>::const_iterator n_cit = nodeIDs.begin();
       n_cit != nodeIDs.end(); ++n_cit) {

        // Mark the neighbor's used nodes as integrated
        neighbor->nodeID_to_is_integrated.at(*n_cit) = true;

        // Integrate LaserScans at the newly integrated nodeIDs in own
        // nodes_to_laser_scans2D map
        new_nodeIDs_to_scans_pairs.insert(make_pair(old_to_new_mappings.at(*n_cit),
                                                    nodes_params.at(*n_cit).scan));
        MRPT_LOG_INFO_STREAM("Adding nodeID-LS of nodeID: "
                             << "\t[old:] " << *n_cit << endl
                             << "| [new:] " << old_to_new_mappings.at(*n_cit));
  }

  this->m_nodes_to_laser_scans2D.insert(new_nodeIDs_to_scans_pairs.begin(),
                                        new_nodeIDs_to_scans_pairs.end());
  edge_reg_mr_t *edge_reg_mr = dynamic_cast<edge_reg_mr_t *>(this->m_edge_reg);
  edge_reg_mr->addBatchOfNodeIDsAndScans(new_nodeIDs_to_scans_pairs);

  this->execDijkstraNodesEstimation();
  neighbor->resetFlags();

  // keep track of the last integrated nodeID
  {
        TNodeID last_node = *nodeIDs.rbegin();
        neighbor->last_integrated_pair_neighbor_frame =
            make_pair(last_node, nodes_params.at(last_node).pose);
  }

  return true;
  MRPT_END;
} // end of addNodeBatchFromNeighbor

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::findTFsWithAllNeighbors() {
  MRPT_START;
  using namespace mrpt::utils;
  using namespace mrpt::math;

  if (this->m_graph.nodeCount() < m_opts.inter_group_node_count_thresh) {
        return false;
  }

  // cache the gridmap so that it is computed only once during all the
  // findMatches*  calls
  this->computeMap();

  bool ret_val = false; // at *any* node registration, change to true

  // iterate over all neighbors - run map-matching proc.
  for (typename neighbors_t::iterator neighbors_it = m_neighbors.begin();
       neighbors_it != m_neighbors.end(); ++neighbors_it) {

        // run matching proc with neighbor only if I haven't found an initial
        // inter-graph TF
        if (!m_neighbor_to_found_initial_tf.at(*neighbors_it)) {

                bool loc_ret_val = findTFWithNeighbor(*neighbors_it);
          if (loc_ret_val) {
                MRPT_LOG_DEBUG_STREAM(
                    "Updating own cached map after successful node registration...");
                // update own map if new nodes have been added.
                this->computeMap();

                // mark current neighbor tf as found.
                m_neighbor_to_found_initial_tf.at(*neighbors_it) = true;

                ret_val = true;
          }
        }
  }

  return ret_val;
  MRPT_END;
} // end of findTFsWithAllNeighbors

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::findTFWithNeighbor(
    TNeighborAgentProps *neighbor) {
  MRPT_START;
  using namespace mrpt::slam;
  using namespace mrpt::math;
  using namespace mrpt::poses;
  using namespace mrpt::utils;
  using namespace mrpt::maps;
  using namespace mrpt::graphslam::detail;

  bool ret_val = false;

  std::vector<TNodeID> neighbor_nodes;
  std::map<TNodeID, node_props_t> nodes_params;
  neighbor->getCachedNodes(&neighbor_nodes, &nodes_params,
                           /*only unused=*/false);
  if (neighbor_nodes.size() < m_opts.inter_group_node_count_thresh ||
      !neighbor->hasNewData()) {
        return false;
  }

  //
  // run alignment procedure
  //
  COccupancyGridMap2D::Ptr neighbor_gridmap = neighbor->getGridMap();
  CGridMapAligner gridmap_aligner;
  gridmap_aligner.options = m_alignment_options;

  CGridMapAligner::TReturnInfo results;
  float run_time = 0;
  // TODO - make use of agents' rendez-vous in the initial estimation
  CPosePDFGaussian init_estim;
  init_estim.mean = neighbor->tf_self_to_neighbor_first_integrated_pose;
  this->logFmt(LVL_INFO, "Trying to align the maps, initial estimation: %s",
               init_estim.mean.asString().c_str());

  // Save them for debugging reasons
  // neighbor_gridmap->saveMetricMapRepresentationToFile(this->getLoggerName() +
  // "_other");
  // this->m_gridmap_cached->saveMetricMapRepresentationToFile(this->getLoggerName()
  // + "_self");

  const CPosePDF::Ptr pdf_tmp = gridmap_aligner.AlignPDF(
      this->m_gridmap_cached.get(), neighbor_gridmap.get(), init_estim,
      &run_time, &results);
  this->logFmt(LVL_INFO, "Elapsed Time: %f", run_time);
  CPosePDFSOG::Ptr pdf_out = CPosePDFSOG::Create();
  pdf_out->copyFrom(*pdf_tmp);

  CPose2D pose_out;
  CMatrixDouble33 cov_out;
  pdf_out->getMostLikelyCovarianceAndMean(cov_out, pose_out);

  this->logFmt(LVL_INFO, "%s\n",
               getGridMapAlignmentResultsAsString(*pdf_tmp, results).c_str());

  // dismiss this?
  if (results.goodness > 0.999 || isEssentiallyZero(pose_out)) {
        return false;
  }

  //
  //
  // Map-merging operation is successful. Integrate graph into own.

  //
  // ask for condensed measurements graph
  //
  mrpt_msgs::GetCMGraph cm_graph_srv;
  typedef mrpt_msgs::GetCMGraphRequest::_nodeIDs_type nodeID_type;
  nodeID_type &matched_nodeIDs = cm_graph_srv.request.nodeIDs;

  // which nodes to ask the condensed graph for
  // I assume that no nodes of the other graph have been integrated yet.
  for (typename std::vector<TNodeID>::const_iterator n_cit =
           neighbor_nodes.begin();
       n_cit != neighbor_nodes.end(); ++n_cit) {
        matched_nodeIDs.push_back(*n_cit); // all used nodes
  }

  MRPT_LOG_DEBUG_STREAM("Asking for the graph.");
  bool res = neighbor->cm_graph_srvclient.call(cm_graph_srv); // blocking
  if (!res) {
        MRPT_LOG_ERROR_STREAM("Service call for CM_Graph failed.");
        return false; // skip this if failed to fetch other's graph
  }
  MRPT_LOG_DEBUG_STREAM("Fetched graph successfully.");
  MRPT_LOG_DEBUG_STREAM(cm_graph_srv.response.cm_graph);

  GRAPH_T other_graph;
  mrpt_bridge::convert(cm_graph_srv.response.cm_graph, other_graph);

  //
  // merge graphs
  //
  MRPT_LOG_WARN_STREAM("Merging graph of \"" << neighbor->getAgentNs()
                                             << "\"...");
  hypots_t graph_conns;
  // build the only hypothesis connecting graph with neighbor subgraph
  // own origin -> first valid nodeID of neighbor
  {
        hypot_t graph_conn;
        constraint_t c;
        c.mean = pose_out;
        // assign the inverse of the found covariance to c
#if MRPT_VERSION >= 0x199
        cov_out = c.cov_inv.inverse();
#else
        cov_out.inv(c.cov_inv);
#endif
        graph_conn.setEdge(c);
        // TODO - change this.
        graph_conn.from = 0;                     // self
        graph_conn.to = *neighbor_nodes.begin(); // other

        graph_conn.goodness = results.goodness;
        graph_conns.push_back(graph_conn);
  }

  std::map<TNodeID, TNodeID> old_to_new_mappings;
  this->m_graph.mergeGraph(other_graph, graph_conns,
                           /*hypots_from_other_to_self=*/false,
                           &old_to_new_mappings);

  //
  // Mark Nodes/LaserScans as integrated
  //
  MRPT_LOG_WARN_STREAM("Marking used nodes as integrated - Integrating LSs");
  nodes_to_scans2D_t new_nodeIDs_to_scans_pairs;
  for (typename std::vector<TNodeID>::const_iterator n_cit =
           neighbor_nodes.begin();
       n_cit != neighbor_nodes.end(); ++n_cit) {

        // Mark the neighbor's used nodes as integrated
        neighbor->nodeID_to_is_integrated.at(*n_cit) = true;

        // Integrate LaserScans at the newly integrated nodeIDs in own
        // nodes_to_laser_scans2D map
        new_nodeIDs_to_scans_pairs.insert(make_pair(old_to_new_mappings.at(*n_cit),
                                                    nodes_params.at(*n_cit).scan));
        MRPT_LOG_INFO_STREAM("Adding nodeID-LS of nodeID: "
                             << "\t[old:] " << *n_cit << endl
                             << "| [new:] " << old_to_new_mappings.at(*n_cit));
  }

  this->m_nodes_to_laser_scans2D.insert(new_nodeIDs_to_scans_pairs.begin(),
                                        new_nodeIDs_to_scans_pairs.end());
  edge_reg_mr_t *edge_reg_mr = dynamic_cast<edge_reg_mr_t *>(this->m_edge_reg);
  edge_reg_mr->addBatchOfNodeIDsAndScans(new_nodeIDs_to_scans_pairs);

  // Call for a Full graph visualization update and Dijkstra update -
  // CGraphSlamOptimizer
  // MRPT_LOG_WARN_STREAM("Optimizing graph..." << endl);
  // this->m_optimizer->optimizeGraph();
  MRPT_LOG_WARN_STREAM("Executing Dijkstra..." << endl);
  this->execDijkstraNodesEstimation();

  neighbor->resetFlags();

  // update the initial tf estimation
  neighbor->tf_self_to_neighbor_first_integrated_pose = pose_out;

  // keep track of the last integrated nodeID
  {
        TNodeID last_node = *neighbor_nodes.rbegin();
        neighbor->last_integrated_pair_neighbor_frame =
            make_pair(last_node, nodes_params.at(last_node).pose);
  }

  MRPT_LOG_WARN_STREAM("Nodes of neighbor ["
                       << neighbor->getAgentNs()
                       << "] have been integrated successfully");

  ret_val = true;

  return ret_val;
  MRPT_END;
} // end if findTFWithNeighbor

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::_execGraphSlamStep(
    mrpt::obs::CActionCollection::Ptr &action,
    mrpt::obs::CSensoryFrame::Ptr &observations,
    mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry) {
  MRPT_START;
  using namespace mrpt::graphslam::deciders;
  using namespace mrpt;

  // call parent method
  bool continue_exec = parent_t::_execGraphSlamStep(action, observations,
                                                    observation, rawlog_entry);

  // find matches between own nodes and those of the neighbors
  bool did_register_from_map_merge;
  if (!m_opts.conservative_find_initial_tfs_to_neighbors) {
        did_register_from_map_merge = this->findTFsWithAllNeighbors();
  } else { // inter-graph TF is available - add new nodes
        THROW_EXCEPTION(
            "Conservative inter-graph TF computation is not yet implemented.");
  }

  bool did_register_from_batches = this->addNodeBatchesFromAllNeighbors();
  m_registered_multiple_nodes =
      (did_register_from_map_merge || did_register_from_batches);

  if (m_registered_multiple_nodes) {
        if (this->m_enable_visuals) {
                this->updateAllVisuals();
        }
  }

  return continue_exec;
  MRPT_END;
} // end of _execGraphSlamStep

template <class GRAPH_T> void CGraphSlamEngine_MR<GRAPH_T>::initClass() {
  using namespace mrpt::graphslam;
  using namespace mrpt::utils;
  using namespace std;

  // initialization of topic namespace names
  // TODO - put these into seperate method
  m_mr_ns = "mr_info";

  // initialization of topic names / services
  m_list_neighbors_topic = m_mr_ns + "/" + "neighbors";
  m_last_regd_id_scan_topic = m_mr_ns + "/" + "last_nodeID_laser_scan";
  m_last_regd_nodes_topic = m_mr_ns + "/" + "last_regd_nodes";
  m_cm_graph_service = m_mr_ns + "/" + "get_cm_graph";

  this->m_class_name = "CGraphSlamEngine_MR_" + m_conn_manager.getTrimmedNs();
  this->setLoggerName(this->m_class_name);
  this->setupComm();

  // Make sure that master_discovery and master_sync are up before trying to
  // connect to other agents
  std::vector<string> nodes_up;
  ros::master::getNodes(nodes_up);
  // If both master_dicovery and master_sync are running, then the
  // /master_sync/get_sync_info service should be available
  MRPT_LOG_INFO_STREAM(
      "Waiting for master_discovery, master_sync nodes to come up....");
  ros::service::waitForService(
      "/master_sync/get_sync_info"); // block until it is
  MRPT_LOG_INFO_STREAM("master_discovery, master_sync are available.");

  this->m_node_reg->setClassName(this->m_node_reg->getClassName() + "_" +
                                 m_conn_manager.getTrimmedNs());
  this->m_edge_reg->setClassName(this->m_edge_reg->getClassName() + "_" +
                                 m_conn_manager.getTrimmedNs());
  this->m_optimizer->setClassName(this->m_optimizer->getClassName() + "_" +
                                  m_conn_manager.getTrimmedNs());

  // in case of Multi-robot specific deciders/optimizers (they
  // inherit from the CDeciderOrOptimizer_ROS interface) set the
  // CConnectionManager*
  // NOTE: It's not certain, even though we are running multi-robot graphSLAM
  // that all these classes do inherit from the
  // CRegistrationDeciderOrOptimizer_MR base class. They might be more generic
  // and not care about the multi-robot nature of the algorithm (e.g.
  // optimization scheme)
  { // NRD
        CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *dec_opt_mr =
            dynamic_cast<CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *>(
                this->m_node_reg);

        if (dec_opt_mr) {
                dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager);
        }
  }
  { // ERD
        CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *dec_opt_mr =
            dynamic_cast<CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *>(
                this->m_edge_reg);
        ASSERT_(dec_opt_mr);
        dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager);
        dec_opt_mr->setCGraphSlamEnginePtr(this);
  }
  { // GSO
        CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *dec_opt_mr =
            dynamic_cast<CRegistrationDeciderOrOptimizer_MR<GRAPH_T> *>(
                this->m_optimizer);
        if (dec_opt_mr) {
                dec_opt_mr->setCConnectionManagerPtr(&m_conn_manager);
        }
  }
  // display messages with the names of the deciders, optimizer and agent string
  // ID
  if (this->m_enable_visuals) {
        // NRD
        this->m_win_manager->assignTextMessageParameters(
            /* offset_y* = */ &m_offset_y_nrd,
            /* text_index* = */ &m_text_index_nrd);
        this->m_win_manager->addTextMessage(
            this->m_offset_x_left, -m_offset_y_nrd,
            mrpt::format("NRD: %s", this->m_node_reg->getClassName().c_str()),
            TColorf(0, 0, 0),
            /* unique_index = */ m_text_index_nrd);

        // ERD
        this->m_win_manager->assignTextMessageParameters(
            /* offset_y* = */ &m_offset_y_erd,
            /* text_index* = */ &m_text_index_erd);
        this->m_win_manager->addTextMessage(
            this->m_offset_x_left, -m_offset_y_erd,
            mrpt::format("ERD: %s", this->m_edge_reg->getClassName().c_str()),
            TColorf(0, 0, 0),
            /* unique_index = */ m_text_index_erd);

        // GSO
        this->m_win_manager->assignTextMessageParameters(
            /* offset_y* = */ &m_offset_y_gso,
            /* text_index* = */ &m_text_index_gso);
        this->m_win_manager->addTextMessage(
            this->m_offset_x_left, -m_offset_y_gso,
            mrpt::format("GSO: %s", this->m_optimizer->getClassName().c_str()),
            TColorf(0, 0, 0),
            /* unique_index = */ m_text_index_gso);

        // Agent Namespace
        this->m_win_manager->assignTextMessageParameters(
            /* offset_y* = */ &m_offset_y_namespace,
            /* text_index* = */ &m_text_index_namespace);
        this->m_win_manager->addTextMessage(
            this->m_offset_x_left, -m_offset_y_namespace,
            mrpt::format("Agent ID: %s", m_conn_manager.getTrimmedNs().c_str()),
            TColorf(0, 0, 0),
            /* unique_index = */ m_text_index_namespace);
  }
  this->readParams();

  this->m_optimized_map_color = m_neighbor_colors_manager.getNextTColor();

  // start the spinner for asynchronously servicing cm_graph requests
  cm_graph_async_spinner.start();
} // end of initClass

template <class GRAPH_T>
mrpt::poses::CPose3D
CGraphSlamEngine_MR<GRAPH_T>::getLSPoseForGridMapVisualization(
    const TNodeID nodeID) const {
  MRPT_START;
  using namespace mrpt::graphs::detail;

  // if this is my own node ID return the corresponding CPose2D, otherwise
  // return the pose connecting own graph with the neighbor's graph

  const TMRSlamNodeAnnotations *node_annots =
      dynamic_cast<const TMRSlamNodeAnnotations *>(
          &this->m_graph.nodes.at(nodeID));

  // Is the current nodeID registered by a neighboring agent or is it my own?
  TNeighborAgentProps *neighbor = NULL;
  this->getNeighborByAgentID(node_annots->agent_ID_str, neighbor);

  CPose3D laser_pose;
  if (!neighbor) { // own node
        laser_pose = parent_t::getLSPoseForGridMapVisualization(nodeID);
  } else { // not by me - return TF to neighbor graph
        laser_pose = CPose3D(neighbor->tf_self_to_neighbor_first_integrated_pose);
  }
  return laser_pose;

  MRPT_END;
}

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::getNeighborByAgentID(
    const std::string &agent_ID_str, TNeighborAgentProps *&neighbor) const {
  MRPT_START;

  bool ret = false;
  for (auto neighbors_it = m_neighbors.begin();
       neighbors_it != m_neighbors.end(); ++neighbors_it) {

        if ((*neighbors_it)->getAgentNs() == agent_ID_str) {
                ret = true;
          neighbor = *neighbors_it;
          break;
        }
  }

  return ret;
  MRPT_END;
}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::usePublishersBroadcasters() {
  MRPT_START;
  using namespace mrpt_bridge;
  using namespace mrpt_msgs;
  using namespace std;
  using namespace mrpt::math;
  using ::operator==;

  // call the parent class
  parent_t::usePublishersBroadcasters();

  // update list of neighbors that the current agent can communicate with.
  mrpt_msgs::GraphSlamAgents nearby_slam_agents;
  m_conn_manager.getNearbySlamAgents(&nearby_slam_agents,
                                     /*ignore_self = */ true);
  m_list_neighbors_pub.publish(nearby_slam_agents);

  // Initialize TNeighborAgentProps
  // for each *new* GraphSlamAgent we should add a TNeighborAgentProps instance,
  // and initialize its subscribers so that we fetch every new LaserScan and
  // list of modified nodes it publishes
  {
        for (GraphSlamAgents::_list_type::const_iterator it =
                 nearby_slam_agents.list.begin();
             it != nearby_slam_agents.list.end(); ++it) {

                const GraphSlamAgent &gsa = *it;

          // Is the current GraphSlamAgent already registered?
          const auto search = [gsa](const TNeighborAgentProps *neighbor) {
                return (neighbor->agent == gsa);
          };
          typename neighbors_t::const_iterator neighbor_it =
              find_if(m_neighbors.begin(), m_neighbors.end(), search);

          if (neighbor_it == m_neighbors.end()) { // current gsa not found, add it

                MRPT_LOG_DEBUG_STREAM("Initializing NeighborAgentProps instance for "
                                      << gsa.name.data);
                m_neighbors.push_back(new TNeighborAgentProps(*this, gsa));
                TNeighborAgentProps *latest_neighbor = m_neighbors.back();
                latest_neighbor->setTColor(m_neighbor_colors_manager.getNextTColor());
                m_neighbor_to_found_initial_tf.insert(
                    make_pair(latest_neighbor, false));
                latest_neighbor->setupComm();
          }
        }
  }

  this->pubUpdatedNodesList();
  this->pubLastRegdIDScan();

  MRPT_END;
} // end of usePublishersBroadcasters

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::pubUpdatedNodesList() {
  MRPT_START;
  using namespace mrpt_msgs;

  // update the last X NodeIDs + positions; Do it only when a new node is
  // inserted. notified of this change anyway after a new node addition
  if (this->m_graph_nodes_last_size == this->m_graph.nodes.size()) {
        return false;
  }
  MRPT_LOG_DEBUG_STREAM("Updating list of node poses");

  // send at most m_num_last_rgd_nodes
  typename GRAPH_T::global_poses_t poses_to_send;
  int poses_counter = 0;
  // fill the NodeIDWithPose_vec msg
  NodeIDWithPose_vec ros_nodes;

  // send up to num_last_regd_nodes Nodes - start from end.
  for (typename GRAPH_T::global_poses_t::const_reverse_iterator cit =
           this->m_graph.nodes.rbegin();
       (poses_counter <= m_opts.num_last_regd_nodes &&
        cit != this->m_graph.nodes.rend());
       ++cit) {

        // Do not resend nodes of others, registered in own graph.
        if (cit->second.agent_ID_str != m_conn_manager.getTrimmedNs()) {
                continue; // skip this.
        }

        NodeIDWithPose curr_node_w_pose;
        // send basics - NodeID, Pose
        curr_node_w_pose.nodeID = cit->first;
        mrpt_bridge::convert(cit->second, curr_node_w_pose.pose);

        // send mr-fields
        curr_node_w_pose.str_ID.data = cit->second.agent_ID_str;
        curr_node_w_pose.nodeID_loc = cit->second.nodeID_loc;

        ros_nodes.vec.push_back(curr_node_w_pose);

        poses_counter++;
  }

  m_last_regd_nodes_pub.publish(ros_nodes);

  // update the last known size
  m_graph_nodes_last_size = this->m_graph.nodeCount();

  bool res = false;
  if (poses_counter) {
        res = true;
  }

  return res;
  MRPT_END;
} // end of pubUpdatedNodesList

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::pubLastRegdIDScan() {
  MRPT_START;
  using namespace mrpt_msgs;
  using namespace mrpt_bridge;

  // Update the last registered scan + associated nodeID
  // Last registered scan always corresponds to the *last* element of the of the
  // published NodeIDWithPose_vec that is published above.
  //
  // - Check if map is empty
  // - Have I already published the last laser scan?
  if (!this->m_nodes_to_laser_scans2D.empty() &&
      m_nodes_to_laser_scans2D_last_size <
          this->m_nodes_to_laser_scans2D.size()) {
        // last registered scan
        MRPT_NodeIDWithLaserScan mrpt_last_regd_id_scan =
            *(this->m_nodes_to_laser_scans2D.rbegin());

        // if this is a mr-registered nodeID+LS, skip it.
        if (!this->isOwnNodeID(mrpt_last_regd_id_scan.first)) {
                return false;
        }

        // MRPT_LOG_DEBUG_STREAM
        //<< "Publishing LaserScan of nodeID \""
        //<< mrpt_last_regd_id_scan.first << "\"";
        ASSERT_(mrpt_last_regd_id_scan.second);

        // convert to ROS msg
        mrpt_msgs::NodeIDWithLaserScan ros_last_regd_id_scan;
        convert(*(mrpt_last_regd_id_scan.second), ros_last_regd_id_scan.scan);
        ros_last_regd_id_scan.nodeID = mrpt_last_regd_id_scan.first;

        m_last_regd_id_scan_pub.publish(ros_last_regd_id_scan);

        // update the last known size.
        m_nodes_to_laser_scans2D_last_size = this->m_nodes_to_laser_scans2D.size();

        return true;
  } else {
        return false;
  }

  MRPT_END;
} // end of pubLastRegdIDScan

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::isOwnNodeID(
    const TNodeID nodeID, const global_pose_t *pose_out /*=NULL*/) const {
  MRPT_START;
  using namespace mrpt::graphs::detail;

  // make sure give node is in the graph
  typename GRAPH_T::global_poses_t::const_iterator global_pose_search =
      this->m_graph.nodes.find(nodeID);
  ASSERTMSG_(global_pose_search != this->m_graph.nodes.end(),
             mrpt::format("isOwnNodeID called for nodeID \"%lu\" which is not "
                          "found in the graph",
                          static_cast<unsigned long>(nodeID)));

  // fill pointer to global_pose_t
  if (pose_out) {
        pose_out = &global_pose_search->second;
  }

  bool res = false;
  const TMRSlamNodeAnnotations *node_annots =
      dynamic_cast<const TMRSlamNodeAnnotations *>(&global_pose_search->second);
  if ((node_annots &&
       node_annots->agent_ID_str == m_conn_manager.getTrimmedNs()) ||
      (!node_annots)) {
        res = true;
  }

  return res;

  MRPT_END;
} // end of isOwnNodeID

template <class GRAPH_T> void CGraphSlamEngine_MR<GRAPH_T>::readParams() {
  using namespace mrpt::utils;
  using namespace mrpt::slam;

  this->readROSParameters();

  // GridMap Alignment
  m_alignment_options.loadFromConfigFileName(this->m_config_fname,
                                             m_sec_alignment_params);

  // Thu Jun 8 17:02:17 EEST 2017, Nikos Koukis
  // other option ends up in segfault.
  m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC;

  m_opts.loadFromConfigFileName(this->m_config_fname, m_sec_mr_slam_params);
} // end of readParams

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::readROSParameters() {
  // call parent method first in case the parent wants to ask for any ROS
  // parameters
  parent_t::readROSParameters();
}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::printParams() const {
  parent_t::printParams();
  m_alignment_options.dumpToConsole();
  m_opts.dumpToConsole();
}

template <class GRAPH_T> void CGraphSlamEngine_MR<GRAPH_T>::setupSubs() {}

template <class GRAPH_T> void CGraphSlamEngine_MR<GRAPH_T>::setupPubs() {
  using namespace mrpt_msgs;
  using namespace sensor_msgs;

  m_list_neighbors_pub = m_nh->advertise<GraphSlamAgents>(
      m_list_neighbors_topic, this->m_queue_size,
      /*latch = */ true);

  // last registered laser scan - by default this corresponds to the last
  // nodeID of the vector of last registered nodes.
  m_last_regd_id_scan_pub = m_nh->advertise<NodeIDWithLaserScan>(
      m_last_regd_id_scan_topic, this->m_queue_size,
      /*latch = */ true);

  // last X nodeIDs + positions
  m_last_regd_nodes_pub = m_nh->advertise<NodeIDWithPose_vec>(
      m_last_regd_nodes_topic, this->m_queue_size,
      /*latch = */ true);
}

template <class GRAPH_T> void CGraphSlamEngine_MR<GRAPH_T>::setupSrvs() {
  // cm_graph service requests are handled by the custom custom_service_queue
  // CallbackQueue
  ros::CallbackQueueInterface *global_queue = m_nh->getCallbackQueue();
  m_nh->setCallbackQueue(&this->custom_service_queue);

  m_cm_graph_srvserver = m_nh->advertiseService(
      m_cm_graph_service, &CGraphSlamEngine_MR<GRAPH_T>::getCMGraph, this);

  m_nh->setCallbackQueue(global_queue);
}

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::getCMGraph(
    mrpt_msgs::GetCMGraph::Request &req, mrpt_msgs::GetCMGraph::Response &res) {
  using namespace std;
  using namespace mrpt::utils;
  using namespace mrpt::math;

  set<TNodeID> nodes_set(req.nodeIDs.begin(), req.nodeIDs.end());
  MRPT_LOG_INFO_STREAM("Called the GetCMGraph service for nodeIDs: "
                       << getSTLContainerAsString(nodes_set));

  if (nodes_set.size() < 2) {
        return false;
  }

  // fill the given Response with the ROS NetworkOfPoses
  GRAPH_T mrpt_subgraph;
  this->m_graph.extractSubGraph(nodes_set, &mrpt_subgraph,
                                /*root_node = */ INVALID_NODEID,
                                /*auto_expand_set=*/false);
  mrpt_bridge::convert(mrpt_subgraph, res.cm_graph);
  return true;
} // end of getCMGraph

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::setObjectPropsFromNodeID(
    const TNodeID nodeID, mrpt::opengl::CSetOfObjects::Ptr &viz_object) {
  using namespace mrpt::utils;
  MRPT_START;

  // who registered this node?
  TNeighborAgentProps *neighbor = NULL;
  std::string &agent_ID_str = this->m_graph.nodes.at(nodeID).agent_ID_str;
  bool is_not_own = getNeighborByAgentID(agent_ID_str, neighbor);

  TColor obj_color;
  if (!is_not_own) { // I registered this.
        // use the standard maps color
        obj_color = this->m_optimized_map_color;
  } else {
        ASSERT_(neighbor);
        obj_color = neighbor->color;
  }
  viz_object->setColor_u8(obj_color);

  MRPT_END;
} // end of setObjectPropsFromNodeID

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::monitorNodeRegistration(
    bool registered /*=false*/, std::string class_name /*="Class"*/) {
  MRPT_START;
  using namespace mrpt::utils;

  if (m_registered_multiple_nodes) {
        MRPT_LOG_ERROR_STREAM("m_registered_multiple_nodes = TRUE!");
        m_registered_multiple_nodes = !m_registered_multiple_nodes;
        this->m_nodeID_max = this->m_graph.nodeCount() - 1;
  } else {
        parent_t::monitorNodeRegistration(registered, class_name);
  }

  MRPT_END;
} // end of monitorNodeRegistration

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::getRobotEstimatedTrajectory(
    typename GRAPH_T::global_poses_t *graph_poses) const {
  MRPT_START;
  ASSERT_(graph_poses);

  for (const auto &n : this->m_graph.nodes) {
        if (n.second.agent_ID_str == m_conn_manager.getTrimmedNs()) {
                graph_poses->insert(n);
        }
  }

  MRPT_END;
} // end of getRobotEstimatedTrajectory

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::getAllOwnNodes(
    std::set<TNodeID> *nodes_set) const {
  ASSERT_(nodes_set);
  nodes_set->clear();

  for (const auto &n : this->m_graph.nodes) {
        if (n.second.agent_ID_str == m_conn_manager.getTrimmedNs()) {
                nodes_set->insert(n.first);
        }
  }
} // end of getAllOwnNodes

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::getNodeIDsOfEstimatedTrajectory(
    std::set<TNodeID> *nodes_set) const {
  ASSERT_(nodes_set);
  this->getAllOwnNodes(nodes_set);
} // end of getNodeIDsOfEstimatedTrajectory


template <class GRAPH_T>
CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::TNeighborAgentProps(
    CGraphSlamEngine_MR<GRAPH_T> &engine_in,
    const mrpt_msgs::GraphSlamAgent &agent_in)
    : engine(engine_in), agent(agent_in), has_setup_comm(false) {
  using namespace mrpt::utils;

  nh = engine.m_nh;
  m_queue_size = engine.m_queue_size;
  this->resetFlags();

  // fill the full paths of the topics to subscribe
  // ASSUMPTION: agents namespaces start at the root "/"
  this->last_regd_nodes_topic =
      "/" + agent.topic_namespace.data + "/" + engine.m_last_regd_nodes_topic;
  this->last_regd_id_scan_topic =
      "/" + agent.topic_namespace.data + "/" + engine.m_last_regd_id_scan_topic;
  this->cm_graph_service =
      "/" + agent.topic_namespace.data + "/" + engine.m_cm_graph_service;

  engine.logFmt(LVL_DEBUG,
                "In constructor of TNeighborAgentProps for topic namespace: %s",
                agent.topic_namespace.data.c_str());

  // initialize the occupancy map based on the engine's gridmap properties
  gridmap_cached = mrpt::maps::COccupancyGridMap2D::Create();
  COccupancyGridMap2D::Ptr eng_gridmap = engine.m_gridmap_cached;
  gridmap_cached->setSize(eng_gridmap->getXMin(), eng_gridmap->getXMax(),
                          eng_gridmap->getYMin(), eng_gridmap->getYMax(),
                          eng_gridmap->getResolution());

} // TNeighborAgentProps::TNeighborAgentProps

template <class GRAPH_T>
CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::~TNeighborAgentProps() {}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::setupComm() {
  using namespace mrpt::utils;

  this->setupSubs();
  this->setupSrvs();

  engine.logFmt(LVL_DEBUG,
                "TNeighborAgentProps: Successfully set up subscribers/services "
                "to agent topics for namespace [%s].",
                agent.topic_namespace.data.c_str());

  has_setup_comm = true;
}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::setupSrvs() {

  cm_graph_srvclient =
      nh->serviceClient<mrpt_msgs::GetCMGraph>(cm_graph_service);
}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::setupSubs() {
  using namespace mrpt_msgs;
  using namespace mrpt::utils;

  last_regd_nodes_sub = nh->subscribe<NodeIDWithPose_vec>(
      last_regd_nodes_topic, m_queue_size,
      &TNeighborAgentProps::fetchUpdatedNodesList, this);
  last_regd_id_scan_sub = nh->subscribe<NodeIDWithLaserScan>(
      last_regd_id_scan_topic, m_queue_size,
      &TNeighborAgentProps::fetchLastRegdIDScan, this);
}

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::fetchUpdatedNodesList(
    const mrpt_msgs::NodeIDWithPose_vec::ConstPtr &nodes) {
  MRPT_START;
  using namespace mrpt_msgs;
  using namespace mrpt_bridge;
  using namespace std;
  using namespace mrpt::utils;

  typedef typename GRAPH_T::constraint_t::type_value pose_t;
  engine.logFmt(LVL_DEBUG, "In fetchUpdatedNodesList method.");

  for (NodeIDWithPose_vec::_vec_type::const_iterator n_it = nodes->vec.begin();
       n_it != nodes->vec.end(); ++n_it) {

        // insert in the set if not already there.
        TNodeID nodeID = static_cast<TNodeID>(n_it->nodeID);
        std::pair<set<TNodeID>::iterator, bool> res = nodeIDs_set.insert(nodeID);
        // if I just inserted this node mark it as not used (in own graph)
        if (res.second) { // insertion took place.
                engine.logFmt(LVL_INFO, "Just fetched a new nodeID: %lu",
                        static_cast<unsigned long>(nodeID));
          nodeID_to_is_integrated.insert(make_pair(n_it->nodeID, false));
        }

        // update the poses
        pose_t curr_pose;
        // note: use "operator[]" instead of "insert" so that if the key already
        // exists, the corresponding value is changed rather than ignored.
        poses[static_cast<TNodeID>(n_it->nodeID)] = convert(n_it->pose, curr_pose);
  }
  has_new_nodes = true;

  // TODO When using 3 graphSLAM agents GDB Shows that it crashes on
  // engine.logFmt lines. Fix this.
  //
  // engine.logFmt(LVL_DEBUG, // THIS CRASHES - GDB WHERE
  //"NodeIDs for topic namespace: %s -> [%s]",
  // agent.topic_namespace.data.c_str(),
  // mrpt::math::getSTLContainerAsString(vector<TNodeID>(
  // nodeIDs_set.begin(), nodeIDs_set.end())).c_str());
  // print poses just for verification
  // engine.logFmt(LVL_DEBUG, "Poses for topic namespace: %s",
  // agent.topic_namespace.data.c_str());
  // for (typename GRAPH_T::global_poses_t::const_iterator
  // p_it = poses.begin();
  // p_it != poses.end();
  //++p_it) {
  // std::string p_str; p_it->second.asString(p_str);
  // engine.logFmt(LVL_DEBUG, "nodeID: %lu | pose: %s",
  // static_cast<unsigned long>(p_it->first),
  // p_str.c_str());
  //}
  // TODO - These also seem to crash sometimes
  // cout << "Agent information: " << agent << endl;
  // cout << "Nodes: " <<
  // mrpt::math::getSTLContainerAsString(vector<TNodeID>(nodeIDs_set.begin(),
  // nodeIDs_set.end()));
  // print nodeIDs just for verification

  MRPT_END;
} // end of fetchUpdatedNodesList

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::fetchLastRegdIDScan(
    const mrpt_msgs::NodeIDWithLaserScan::ConstPtr &last_regd_id_scan) {
  MRPT_START;
  using namespace std;
  using namespace mrpt::utils;
  ASSERT_(last_regd_id_scan);
  engine.logFmt(LVL_DEBUG, "In fetchLastRegdIDScan method.");

  // Pose may not be available due to timing in publishing of the corresponding
  // topics. Just keep it in ROS form and then convert them to MRPT form when
  // needed.
  ros_scans.push_back(*last_regd_id_scan);
  has_new_scans = true;

  MRPT_END;
} // end of fetchLastRegdIDScan

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::getCachedNodes(
    std::vector<TNodeID> *nodeIDs /*=NULL*/,
    std::map<TNodeID, node_props_t> *nodes_params /*=NULL*/,
    bool only_unused /*=true*/) const {
  MRPT_START;
  using namespace mrpt::obs;
  using namespace mrpt::utils;
  using namespace mrpt::math;
  using namespace std;
  using namespace mrpt::poses;

  // at least one of the two args should be given.
  ASSERT_(nodeIDs || nodes_params);

  // traverse all nodes
  for (std::set<TNodeID>::const_iterator n_it = nodeIDs_set.begin();
       n_it != nodeIDs_set.end(); ++n_it) {

        // Should I return only the unused ones?
        // If so, if the current nodeID is integrated, skip it.
        if (only_unused && nodeID_to_is_integrated.at(*n_it)) {
                continue;
        }

        // add the node properties
        if (nodes_params) {
                std::pair<TNodeID, node_props_t> params; // current pair to be inserted.
          const pose_t *p = &poses.at(*n_it);

          params.first = *n_it;
          params.second.pose = *p;
          CObservation2DRangeScan::Ptr mrpt_scan =
              CObservation2DRangeScan::Create();
          const sensor_msgs::LaserScan *ros_laser_scan =
              this->getLaserScanByNodeID(*n_it);

      // if LaserScan not found, skip nodeID altogether.
      //
      // Its natural to have more poses than LaserScans since on every node
      // registration I send a modified list of X node positions and just one
      // LaserScan
          if (!ros_laser_scan) {
                continue;
          }

          mrpt_bridge::convert(*ros_laser_scan, CPose3D(*p), *mrpt_scan);
          params.second.scan = mrpt_scan;

          // insert the pair
          nodes_params->insert(params);

          // debugging message
          // std::stringstream ss;
          // ss << "\ntID: " << nodes_params->rbegin()->first
          //<< " ==> Props: " << nodes_params->rbegin()->second;
          // engine.logFmt(LVL_DEBUG, "%s", ss.str().c_str());
        }

        // add the nodeID
        if (nodeIDs) {
                // do not return nodeIDs that don't have valid laserScans
                // this is also checked at the prior if
                if (!nodes_params && !this->getLaserScanByNodeID(*n_it)) {
                continue;
          }

          nodeIDs->push_back(*n_it);
        }
  }

  MRPT_END;
} // end of TNeighborAgentProps::getCachedNodes

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::resetFlags() const {
  has_new_nodes = false;
  has_new_scans = false;
}

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::hasNewNodesBatch(
    int new_batch_size) {
  MRPT_START;

  auto is_not_integrated = [this](const std::pair<TNodeID, bool> pair) {
        return (pair.second == false && getLaserScanByNodeID(pair.first));
  };
  int not_integrated_num =
      count_if(nodeID_to_is_integrated.begin(), nodeID_to_is_integrated.end(),
               is_not_integrated);

  return (not_integrated_num >= new_batch_size);

  MRPT_END;
}

template <class GRAPH_T>
const sensor_msgs::LaserScan *
CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::getLaserScanByNodeID(
    const TNodeID nodeID) const {
  MRPT_START;

  // assert that the current nodeID exists in the nodeIDs_set
  ASSERT_(nodeIDs_set.find(nodeID) != nodeIDs_set.end());

  for (std::vector<mrpt_msgs::NodeIDWithLaserScan>::const_iterator it =
           ros_scans.begin();
       it != ros_scans.end(); ++it) {
        if (it->nodeID == nodeID) {
                return &it->scan;
        }
  }

  // if out here, LaserScan doesn't exist.
  return 0;
  MRPT_END;
} // end of TNeighborAgentProps::getLaserScanByNodeID

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::fillOptPaths(
    const std::set<TNodeID> &nodeIDs, paths_t *opt_paths) const {
  MRPT_START;
  using namespace std;
  using namespace mrpt::utils;
  typedef std::set<TNodeID>::const_iterator set_cit;

  // make sure that all given nodes belong in the nodeIDs_set.
  ASSERTMSG_(includes(nodeIDs_set.begin(), nodeIDs_set.end(), nodeIDs.begin(),
                      nodeIDs.end()),
             "nodeIDs is not a strict subset of the current neighbor's nodes");
  ASSERT_(opt_paths);

  for (set_cit cit = nodeIDs.begin(); cit != std::prev(nodeIDs.end()); ++cit) {

        for (set_cit cit2 = std::next(cit); cit2 != nodeIDs.end(); ++cit2) {

                constraint_t c = constraint_t(poses.at(*cit2) - poses.at(*cit));
          c.cov_inv =
              mrpt::math::CMatrixFixedNumeric<double, constraint_t::state_length,
                                              constraint_t::state_length>();
          c.cov_inv.unit();
          c.cov_inv *= 500;

          opt_paths->push_back(path_t(
              /*start=*/*cit,
              /*end=*/*cit2,
              /*constraint=*/c));

          engine.logFmt(
              LVL_DEBUG, "Filling optimal path for nodes: %lu => %lu: %s",
              static_cast<unsigned long>(*cit), static_cast<unsigned long>(*cit2),
              opt_paths->back().curr_pose_pdf.getMeanVal().asString().c_str());
        }
  }
  MRPT_END;
} // end of TNeighborAgentProps::fillOptPaths

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::computeGridMap() const {
  MRPT_START;
  using namespace mrpt::poses;
  using namespace mrpt::utils;
  using namespace mrpt;
  using namespace mrpt::math;
  using namespace std;

  gridmap_cached->clear();

  std::vector<TNodeID> nodeIDs;
  std::map<TNodeID, node_props_t> nodes_params;
  // get list of nodes, laser scans
  this->getCachedNodes(&nodeIDs, &nodes_params, false);

  // iterate over poses, scans. Add them to the gridmap.
  for (typename map<TNodeID, node_props_t>::const_iterator it =
           nodes_params.begin();
       it != nodes_params.end(); ++it) {
        const CPose3D curr_pose_3d; // do not add the actual pose!
        auto obs = it->second.scan.get();
#if MRPT_VERSION >= 0x199
        gridmap_cached->insertObservation(*obs, &curr_pose_3d);
#else
        gridmap_cached->insertObservation(obs, &curr_pose_3d);
#endif
  }

  MRPT_END;
} // end of TNeighborAgentProps::computeGridMap

template <class GRAPH_T>
const mrpt::maps::COccupancyGridMap2D::Ptr &
CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::getGridMap() const {
  MRPT_START;
  if (hasNewData()) {
        this->computeGridMap();
  }

  return gridmap_cached;
  MRPT_END;
} // end of TNeighborAgentProps::getGridMap

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::getGridMap(
    mrpt::maps::COccupancyGridMap2D::Ptr &map) const {
  MRPT_START;
  ASSERT_(map);

  if (hasNewData()) {
        this->computeGridMap();
  }

  map->copyMapContentFrom(*gridmap_cached);
  MRPT_END;
} // end of TNeighborAgentProps::getGridMap

template <class GRAPH_T>
bool CGraphSlamEngine_MR<GRAPH_T>::TNeighborAgentProps::hasNewData() const {
  return has_new_nodes && has_new_scans;
} // end of hasNewData

template <class GRAPH_T>
CGraphSlamEngine_MR<GRAPH_T>::TOptions::TOptions(const engine_mr_t &engine_in)
    : conservative_find_initial_tfs_to_neighbors(false),
      nodes_integration_batch_size(4), num_last_regd_nodes(10),
      inter_group_node_count_thresh(40),
      inter_group_node_count_thresh_minadv(25), engine(engine_in) {
} // end of TOptions::TOptions

template <class GRAPH_T>
CGraphSlamEngine_MR<GRAPH_T>::TOptions::~TOptions() {
} // end of TOptions::~TOptions

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TOptions::loadFromConfigFile(
    const CConfigFileBase &source, const std::string &section) {

  MRPT_LOAD_CONFIG_VAR(nodes_integration_batch_size, int, source, section);
  MRPT_LOAD_CONFIG_VAR(num_last_regd_nodes, int, source, section);

  MRPT_LOAD_CONFIG_VAR(conservative_find_initial_tfs_to_neighbors, bool, source,
                       section);
  ASSERTMSG_(!conservative_find_initial_tfs_to_neighbors,
             "Conservative behavior is not yet implemented.");

  MRPT_LOAD_CONFIG_VAR(inter_group_node_count_thresh, int, source, section);
  // warn user if they choose smaller threshold.
  if (inter_group_node_count_thresh < inter_group_node_count_thresh_minadv) {
        engine.logFmt(LVL_ERROR,
                      "inter_group_node_count_thresh [%d]"
                      "is set lower than the advised minimum [%d]",
                      inter_group_node_count_thresh,
                      inter_group_node_count_thresh_minadv);

        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
  }

} // end of TOptions::loadFromConfigFile

template <class GRAPH_T>
void CGraphSlamEngine_MR<GRAPH_T>::TOptions::dumpToTextStream(
#if MRPT_VERSION >= 0x199
    std::ostream &out) const {
#else
    mrpt::utils::CStream &out) const {
#endif

  LOADABLEOPTS_DUMP_VAR(nodes_integration_batch_size, int);
  LOADABLEOPTS_DUMP_VAR(num_last_regd_nodes, int);
  LOADABLEOPTS_DUMP_VAR(conservative_find_initial_tfs_to_neighbors, bool);
  LOADABLEOPTS_DUMP_VAR(static_cast<int>(inter_group_node_count_thresh), int)
  LOADABLEOPTS_DUMP_VAR(inter_group_node_count_thresh_minadv, int)

} // end of TOptions::dumpToTextStream

} // namespace graphslam
} // namespace mrpt