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

#pragma once
#include <mrpt/containers/stl_containers_utils.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/obs_utils.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CSphere.h>

namespace mrpt::graphslam::deciders
{
template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::CLoopCloserERD()
{
  this->initializeLoggers("CLoopCloserERD");
  m_edge_types_to_nums["ICP2D"] = 0;
  m_edge_types_to_nums["LC"] = 0;
  MRPT_LOG_DEBUG_STREAM("Initialized class object");
}

template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::~CLoopCloserERD()
{
  for (auto it = m_node_optimal_paths.begin(); it != m_node_optimal_paths.end(); ++it)
  {
    delete it->second;
  }
}

template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::updateState(
    [[maybe_unused]] mrpt::obs::CActionCollection::Ptr action,
    mrpt::obs::CSensoryFrame::Ptr observations,
    mrpt::obs::CObservation::Ptr observation)
{
  MRPT_START
  this->m_time_logger.enter("updateState");
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::obs;
  using namespace mrpt::obs::utils;
  using namespace mrpt::opengl;
  using namespace mrpt::poses;
  using namespace mrpt::math;

  // Track the last recorded laser scan
  {
    CObservation2DRangeScan::Ptr scan =
        getObservation<CObservation2DRangeScan>(observations, observation);
    if (scan)
    {
      m_last_laser_scan2D = scan;
    }
  }

  if (!m_first_laser_scan)
  {
    m_first_laser_scan = m_last_laser_scan2D;
  }

  // check possible prior node registration
  size_t num_registered = absDiff(this->m_last_total_num_nodes, this->m_graph->nodeCount());
  bool registered_new_node = num_registered > 0;

  if (registered_new_node)
  {
    MRPT_LOG_DEBUG_STREAM("New node has been registered in the graph!");
    registered_new_node = true;

    // either single node registration, or double node registration for the
    // first time only, unless we override this check.
    if (!this->m_override_registered_nodes_check)
    {
      if (!((num_registered == 1) ^ (num_registered == 2 && m_is_first_time_node_reg)))
      {
        MRPT_LOG_ERROR_STREAM(
            "Invalid number of registered nodes since last call to "
            "updateStates| "
            "Found \""
            << num_registered << "\" new nodes.");
        THROW_EXCEPTION("Invalid number of registered nodes.");
      }
    }

    // first time call:
    // NRD should have registered *2* nodes; one for the root node and one
    // for
    // the first added constraint. Add the first measurement taken to the
    // root
    // and the second as usual
    if (m_is_first_time_node_reg)
    {
      MRPT_LOG_WARN_STREAM("Assigning first laserScan to the graph root node.");
      this->m_nodes_to_laser_scans2D[this->m_graph->root] = m_first_laser_scan;
      m_is_first_time_node_reg = false;
    }

    // register the new node-laserScan pair
    this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] = m_last_laser_scan2D;

    if (m_laser_params.use_scan_matching)
    {
      // scan match with previous X nodes
      this->addScanMatchingEdges(this->m_graph->nodeCount() - 1);
    }

    // update partitioning scheme with the latest pose/measurement
    m_partitions_full_update =
        ((this->m_graph->nodeCount() % m_lc_params.full_partition_per_nodes) == 0 ||
         this->m_just_inserted_lc)
            ? true
            : false;
    this->updateMapPartitions(m_partitions_full_update, num_registered == 2);

    // check for loop closures
    partitions_t partitions_for_LC;
    this->checkPartitionsForLC(&partitions_for_LC);
    this->evaluatePartitionsForLC(partitions_for_LC);

    if (m_visualize_curr_node_covariance)
    {
      this->execDijkstraProjection();
    }

    this->m_last_total_num_nodes = this->m_graph->nodeCount();
  }

  this->m_time_logger.leave("updateState");
  return true;
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::fetchNodeIDsForScanMatching(
    const mrpt::graphs::TNodeID& curr_nodeID, std::set<mrpt::graphs::TNodeID>* nodes_set)
{
  ASSERTDEB_(nodes_set);

  // deal with the case that less than `prev_nodes_for_ICP` nodes have been
  // registered
  int fetched_nodeIDs = 0;
  for (int nodeID_i = static_cast<int>(curr_nodeID) - 1;
       ((fetched_nodeIDs <= this->m_laser_params.prev_nodes_for_ICP) &&
        (nodeID_i >= 0));  // <----- I *have* to use int (instead of unsigned)
                           // if I use this condition
       --nodeID_i)
  {
    nodes_set->insert(nodeID_i);
    fetched_nodeIDs++;
  }
}  // end of fetchNodeIDsForScanMatching

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::addScanMatchingEdges(const mrpt::graphs::TNodeID& curr_nodeID)
{
  MRPT_START
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::obs;
  using namespace mrpt::math;
  using mrpt::graphs::TNodeID;

  // get a list of nodes to check ICP against
  MRPT_LOG_DEBUG_STREAM("Adding ICP Constraints for nodeID: " << curr_nodeID);

  std::set<TNodeID> nodes_set;
  this->fetchNodeIDsForScanMatching(curr_nodeID, &nodes_set);

  // try adding ICP constraints with each node in the previous set
  for (unsigned long node_it : nodes_set)
  {
    constraint_t rel_edge;
    mrpt::slam::CICP::TReturnInfo icp_info;

    MRPT_LOG_DEBUG_STREAM("Fetching laser scan for nodes: " << node_it << "==> " << curr_nodeID);

    bool found_edge = this->getICPEdge(node_it, curr_nodeID, &rel_edge, &icp_info);
    if (!found_edge) continue;

    // keep track of the recorded goodness values
    // TODO - rethink on these condition.
    if (!isnan(icp_info.goodness) ||
        !approximatelyEqual(static_cast<double>(icp_info.goodness), 0.0))
    {
      m_laser_params.goodness_threshold_win.addNewMeasurement(icp_info.goodness);
    }
    double goodness_thresh =
        m_laser_params.goodness_threshold_win.getMedian() * m_consec_icp_constraint_factor;
    bool accept_goodness = icp_info.goodness > goodness_thresh;
    MRPT_LOG_DEBUG_STREAM(
        "Curr. Goodness: " << icp_info.goodness << "|\t Threshold: " << goodness_thresh << " => "
                           << (accept_goodness ? "ACCEPT" : "REJECT"));

    // make sure that the suggested edge makes sense with regards to current
    // graph config - check against the current position difference
    bool accept_mahal_distance =
        this->mahalanobisDistanceOdometryToICPEdge(node_it, curr_nodeID, rel_edge);

    // criterion for registering a new node
    if (accept_goodness && accept_mahal_distance)
    {
      this->registerNewEdge(node_it, curr_nodeID, rel_edge);
    }
  }

  MRPT_END
}  // end of addScanMatchingEdges
template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::getICPEdge(
    const mrpt::graphs::TNodeID& from,
    const mrpt::graphs::TNodeID& to,
    constraint_t* rel_edge,
    mrpt::slam::CICP::TReturnInfo* icp_info,
    const TGetICPEdgeAdParams* ad_params)
{
  MRPT_START
  ASSERTDEB_(rel_edge);
  this->m_time_logger.enter("getICPEdge");

  using namespace mrpt::obs;
  using namespace std;
  using namespace mrpt::graphslam::detail;

  // fetch the relevant laser scans and poses of the nodeIDs
  // If given in the additional params struct, use those values instead of
  // searching in the class std::map(s)
  CObservation2DRangeScan::Ptr from_scan, to_scan;
  global_pose_t from_pose;
  global_pose_t to_pose;

  MRPT_LOG_DEBUG_STREAM("****In getICPEdge method: ");
  if (ad_params)
  {
    MRPT_LOG_DEBUG_STREAM("TGetICPEdgeAdParams:\n" << ad_params->getAsString() << endl);
  }

  // from-node parameters
  const node_props_t* from_params = ad_params ? &ad_params->from_params : nullptr;
  bool from_success = this->getPropsOfNodeID(from, &from_pose, from_scan, from_params);  // TODO
  // to-node parameters
  const node_props_t* to_params = ad_params ? &ad_params->to_params : nullptr;
  bool to_success = this->getPropsOfNodeID(to, &to_pose, to_scan, to_params);

  if (!from_success || !to_success)
  {
    MRPT_LOG_DEBUG_STREAM(
        "Either node #" << from << " or node #" << to
                        << " doesn't contain a valid LaserScan. Ignoring this...");
    return false;
  }

  // make use of initial node position difference for the ICP edge
  // from_node pose
  pose_t initial_estim;
  if (ad_params)
  {
    initial_estim = ad_params->init_estim;
  }
  else
  {
    initial_estim = to_pose - from_pose;
  }

  MRPT_LOG_DEBUG_STREAM(
      "from_pose: " << from_pose << "| to_pose: " << to_pose << "| init_estim: " << initial_estim);

  range_ops_t::getICPEdge(*from_scan, *to_scan, rel_edge, &initial_estim, icp_info);
  MRPT_LOG_DEBUG_STREAM("*************");

  this->m_time_logger.leave("getICPEdge");
  return true;
  MRPT_END
}  // end of getICPEdge

template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::fillNodePropsFromGroupParams(
    const mrpt::graphs::TNodeID& nodeID,
    const std::map<mrpt::graphs::TNodeID, node_props_t>& group_params,
    node_props_t* node_props)
{
  ASSERTDEB_(node_props);

  // MRPT_LOG_DEBUG_STREAM(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
  // MRPT_LOG_DEBUG_STREAM("Running fillNodePropsFromGroupParams for nodeID
  // \""
  //<< nodeID << "\"");

  // Make sure that the given nodeID exists in the group_params
  auto search = group_params.find(nodeID);
  bool res = false;
  if (search == group_params.end())
  {
    // MRPT_LOG_DEBUG_STREAM("Operation failed.");
  }
  else
  {
    *node_props = search->second;
    res = true;
    // MRPT_LOG_DEBUG_STREAM("Properties filled: " <<
    // node_props->getAsString());
  }

  // MRPT_LOG_DEBUG_STREAM("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
  return res;
}

template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::getPropsOfNodeID(
    const mrpt::graphs::TNodeID& nodeID,
    global_pose_t* pose,
    mrpt::obs::CObservation2DRangeScan::Ptr& scan,
    const node_props_t* node_props) const
{
  // make sure output instances are valid
  ASSERTDEB_(pose);

  bool filled_pose = false;
  bool filled_scan = false;

  if (node_props)
  {
    // Pose
    if (node_props->pose != global_pose_t())
    {
      *pose = node_props->pose;
      filled_pose = true;
    }
    // LaserScan
    if (node_props->scan)
    {
      scan = node_props->scan;
      filled_scan = true;
    }
  }

  // TODO - What if the node_props->pose is indeed 0?
  ASSERTDEBMSG_(
      !(filled_pose ^ filled_scan),
      format(
          "Either BOTH or NONE of the filled_pose, filled_scan should be set."
          "NodeID:  [%lu]",
          static_cast<unsigned long>(nodeID)));

  //
  // fill with class data if not yet available
  //
  if (!filled_pose)
  {
    // fill with class data if not yet available
    auto search = this->m_graph->nodes.find(nodeID);
    if (search != this->m_graph->nodes.end())
    {
      *pose = search->second;
      filled_pose = true;
    }
    else
    {
      MRPT_LOG_WARN_STREAM("pose not found for nodeID: " << nodeID);
    }
  }
  if (!filled_scan)
  {
    auto search = this->m_nodes_to_laser_scans2D.find(nodeID);
    if (search != this->m_nodes_to_laser_scans2D.end())
    {
      scan = search->second;
      filled_scan = true;
    }
  }

  return filled_pose && filled_scan;
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::checkPartitionsForLC(partitions_t* partitions_for_LC)
{
  MRPT_START
  this->m_time_logger.enter("LoopClosureEvaluation");

  using namespace std;
  using namespace mrpt;

  ASSERTDEB_(partitions_for_LC);
  partitions_for_LC->clear();

  // keep track of the previous nodes list of every partition. If this is not
  // changed - do not mark it as potential for loop closure
  map<int, std::vector<uint32_t>>::iterator finder;
  // reset the previous list if full partitioning was issued
  if (m_partitions_full_update)
  {
    m_partitionID_to_prev_nodes_list.clear();
  }

  int partitionID = 0;
  // for every partition...
  for (auto partitions_it = m_curr_partitions.begin(); partitions_it != m_curr_partitions.end();
       ++partitions_it, ++partitionID)
  {
    // check whether the last registered node is in the currently traversed
    // partition - if not, ignore it.
    if (m_lc_params.LC_check_curr_partition_only)
    {
      bool curr_node_in_curr_partition =
          ((find(partitions_it->begin(), partitions_it->end(), this->m_graph->nodeCount() - 1)) !=
           partitions_it->end());
      if (!curr_node_in_curr_partition)
      {
        continue;
      }
    }

    // keep track of the previous nodes list
    finder = m_partitionID_to_prev_nodes_list.find(partitionID);
    if (finder == m_partitionID_to_prev_nodes_list.end())
    {
      // nodes list is not registered yet
      m_partitionID_to_prev_nodes_list.insert(make_pair(partitionID, *partitions_it));
    }
    else
    {
      if (*partitions_it == finder->second)
      {
        // MRPT_LOG_DEBUG_STREAM("Partition " << partitionID
        //<< " remained unchanged. ");
        continue;  // same list as before.. no need to check this...
      }
      else
      {  // list was changed  - update the previous nodes list
        // MRPT_LOG_DEBUG_STREAM("Partition " << partitionID << "
        // CHANGED. ");
        finder->second = *partitions_it;
      }
    }

    // investigate each partition
    int curr_node_index = 1;
    size_t prev_nodeID = *(partitions_it->begin());
    for (auto it = partitions_it->begin() + 1; it != partitions_it->end(); ++it, ++curr_node_index)
    {
      size_t curr_nodeID = *it;

      // are there consecutive nodes with large difference inside this
      // partition? Are these nodes enough to consider LC?
      if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
      {
        // there is at least one divergent node..

        int num_after_nodes = partitions_it->size() - curr_node_index;
        int num_before_nodes = partitions_it->size() - num_after_nodes;
        if (num_after_nodes >= m_lc_params.LC_min_remote_nodes &&
            num_before_nodes >= m_lc_params.LC_min_remote_nodes)
        {  // at least X LC nodes
          MRPT_LOG_WARN_STREAM(
              "Found potential loop closures:"
              << endl
              << "\tPartitionID: " << partitionID << endl
              << "\tPartition: "
              << mrpt::containers::getSTLContainerAsString(*partitions_it).c_str() << endl
              << "\t" << prev_nodeID << " ==> " << curr_nodeID << endl
              << "\tNumber of LC nodes: " << num_after_nodes);
          partitions_for_LC->push_back(*partitions_it);
          break;  // no need to check the rest of the nodes in this
                  // partition
        }
      }

      // update the previous node
      prev_nodeID = curr_nodeID;
    }
    MRPT_LOG_DEBUG_STREAM("Successfully checked partition: " << partitionID);
  }

  this->m_time_logger.leave("LoopClosureEvaluation");
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::evaluatePartitionsForLC(const partitions_t& partitions)
{
  MRPT_START
  using namespace mrpt;
  using namespace mrpt::graphslam::detail;
  using namespace mrpt::math;
  using namespace std;
  this->m_time_logger.enter("LoopClosureEvaluation");

  if (partitions.size() == 0) return;

  const static std::string header_sep(80, '#');

  MRPT_LOG_DEBUG_FMT("Evaluating partitions for loop closures...\n%s\n", header_sep.c_str());

  // for each partition to be evaulated...
  for (auto partition : partitions)
  {
    // split the partition to groups
    std::vector<uint32_t> groupA, groupB;
    this->splitPartitionToGroups(partition, &groupA, &groupB, 5);

    // generate hypotheses pool
    hypotsp_t hypots_pool;
    this->generateHypotsPool(groupA, groupB, &hypots_pool);

    // compute the pair-wise consistency matrix
    CMatrixDouble consist_matrix(hypots_pool.size(), hypots_pool.size());
    this->generatePWConsistenciesMatrix(groupA, groupB, hypots_pool, &consist_matrix);

    // evaluate resulting matrix - fill valid hypotheses
    hypotsp_t valid_hypots;
    this->evalPWConsistenciesMatrix(consist_matrix, hypots_pool, &valid_hypots);

    // registering the indicated/valid hypotheses
    if (valid_hypots.size())
    {
      MRPT_LOG_WARN_STREAM("Registering Hypotheses...");
      for (auto it = valid_hypots.begin(); it != valid_hypots.end(); ++it)
      {
        this->registerHypothesis(**it);
      }
    }
    // delete all hypotheses - generated in the heap...
    MRPT_LOG_DEBUG_STREAM("Deleting the generated hypotheses pool...");
    for (auto it = hypots_pool.begin(); it != hypots_pool.end(); ++it)
    {
      delete *it;
    }
  }  // for each partition

  MRPT_LOG_DEBUG_STREAM("\n" << header_sep);
  this->m_time_logger.leave("LoopClosureEvaluation");

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::evalPWConsistenciesMatrix(
    const mrpt::math::CMatrixDouble& consist_matrix,
    const hypotsp_t& hypots_pool,
    hypotsp_t* valid_hypots)
{
  MRPT_START
  using namespace std;
  using namespace mrpt::math;

  ASSERTDEB_(valid_hypots);
  valid_hypots->clear();

  // evaluate the pair-wise consistency matrix
  // compute dominant eigenvector
  CVectorDouble u;
  bool valid_lambda_ratio = this->computeDominantEigenVector(consist_matrix, &u, false);
  if (!valid_lambda_ratio) return;

  // cout << "Dominant eigenvector: " << u.transpose() << endl;

  // discretize the indicator vector - maximize the dot product of
  // w_unit .* u
  ASSERTDEB_(u.size());
  CVectorDouble w(u.size(), 0);  // discretized  indicator vector
  double dot_product = 0;
  for (int i = 0; i != w.size(); ++i)
  {
    // stream for debugging reasons
    stringstream ss;

    // make the necessary change and see if the dot product increases
    w[i] = 1;
    double potential_dot_product =
        ((w.asEigen().transpose() * u.asEigen()) / mrpt::square(w.norm())).value();
    ss << mrpt::format(
        "current: %f | potential_dot_product: %f", dot_product, potential_dot_product);
    if (potential_dot_product > dot_product)
    {
      ss << " ==>  ACCEPT";
      dot_product = potential_dot_product;
    }
    else
    {
      ss << " ==>  REJECT";
      w[i] = 0;  // revert the change
    }
    ss << endl;
    // MRPT_LOG_DEBUG_STREAM(ss.str());
  }
  cout << "Outcome of discretization: " << w.transpose() << endl;
  // mrpt::system::pause();

  // Current hypothesis is to be registered.
  if (!w.asEigen().isZero())
  {
    for (int wi = 0; wi != w.size(); ++wi)
    {
      if (w[wi] == 1)
      {
        // search through the potential hypotheses, find the one with
        // the
        // correct ID and register it.
        valid_hypots->push_back(this->findHypotByID(hypots_pool, wi));
      }
    }
  }
  // mrpt::system::pause();

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::splitPartitionToGroups(
    std::vector<uint32_t>& partition,
    std::vector<uint32_t>* groupA,
    std::vector<uint32_t>* groupB,
    int max_nodes_in_group)
{
  MRPT_START

  using namespace mrpt;
  using namespace mrpt::math;
  using mrpt::graphs::TNodeID;

  // assertions
  ASSERTDEBMSG_(groupA, "Pointer to groupA is not valid");
  ASSERTDEBMSG_(groupB, "Pointer to groupB is not valid");
  ASSERTDEBMSG_(
      max_nodes_in_group == -1 || max_nodes_in_group > 0,
      format(
          "Value %d not permitted for max_nodes_in_group"
          "Either use a positive integer, "
          "or -1 for non-restrictive partition size",
          max_nodes_in_group));

  // find a large difference between successive nodeIDs - that's where the cut
  // is going to be
  TNodeID prev_nodeID = 0;
  size_t index_to_split = 1;
  for (auto it = partition.begin() + 1; it != partition.end(); ++it, ++index_to_split)
  {
    TNodeID curr_nodeID = *it;

    if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
    {
      break;
    }
    // update the last nodeID
    prev_nodeID = curr_nodeID;
  }
  ASSERTDEB_(partition.size() > index_to_split);

  // Fill the groups
  *groupA = std::vector<uint32_t>(partition.begin(), partition.begin() + index_to_split);
  *groupB = std::vector<uint32_t>(partition.begin() + index_to_split, partition.end());
  // limit the number of nodes in each group
  if (max_nodes_in_group != -1)
  {
    // groupA
    if (groupA->size() > static_cast<size_t>(max_nodes_in_group))
    {
      *groupA = std::vector<uint32_t>(groupA->begin(), groupA->begin() + max_nodes_in_group);
    }
    // groupB
    if (groupB->size() > static_cast<size_t>(max_nodes_in_group))
    {
      *groupB = std::vector<uint32_t>(groupB->end() - max_nodes_in_group, groupB->end());
    }
  }

  // mrpt::system::pause();
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::generateHypotsPool(
    const std::vector<uint32_t>& groupA,
    const std::vector<uint32_t>& groupB,
    hypotsp_t* generated_hypots,
    const TGenerateHypotsPoolAdParams* ad_params)
{
  MRPT_START
  using namespace mrpt;
  using namespace mrpt::containers;

  ASSERTDEBMSG_(generated_hypots, "generateHypotsPool: Given hypotsp_t pointer is invalid.");
  generated_hypots->clear();

  MRPT_LOG_DEBUG_STREAM("Generating hypotheses for groups: " << endl);
  MRPT_LOG_DEBUG_STREAM(
      "- groupA:\t" << getSTLContainerAsString(groupA) << " - size: " << groupA.size() << endl);
  MRPT_LOG_DEBUG_STREAM(
      "- groupB:\t" << getSTLContainerAsString(groupB) << " - size: " << groupB.size() << endl);

  // verify that the number of laserScans is the same as the number of poses
  // if
  // the TGenerateHyptsPoolAdParams is used
  // formulate into function and pass std::vector<uint32_t> and
  // ad_params->group
  // TODO
  if (ad_params)
  {
    const auto& p = ad_params->groupA_params;
    if (p.size())
    {
#if _DEBUG
      size_t nodes_count = groupA.size();
#endif
      // map should have same size
      ASSERTDEBMSG_(
          nodes_count == p.size(), format(
                                       "Size mismatch between nodeIDs in group [%lu]"
                                       " and corresponding properties map [%lu]",
                                       nodes_count, p.size()));
    }
  }

  // use a hypothesis ID with which the consistency matrix will then be
  // formed
  int hypot_counter = 0;
  int invalid_hypots = 0;  // just for keeping track of them.
  {
    // iterate over all the nodes in both groups
    for (unsigned int b_it : groupB)
    {
      for (unsigned int a_it : groupA)
      {
        // by default hypotheses will direct bi => ai; If the hypothesis
        // is
        // traversed the opposite way, take the opposite of the
        // constraint
        auto* hypot = new hypot_t;
        hypot->from = b_it;
        hypot->to = a_it;
        hypot->id = hypot_counter++;

        // [from] *b_it ====[edge]===> [to]  *a_it

        // Fetch and set the pose and LaserScan of from, to nodeIDs
        //
        // even if icp_ad_params NULL, it will be handled appropriately
        // by the
        // getICPEdge fun.
        // bool from_success, to_success;
        TGetICPEdgeAdParams* icp_ad_params = nullptr;
        if (ad_params)
        {
          icp_ad_params = new TGetICPEdgeAdParams;
          // from_success = fillNodePropsFromGroupParams(
          fillNodePropsFromGroupParams(b_it, ad_params->groupB_params, &icp_ad_params->from_params);
          // to_success = fillNodePropsFromGroupParams(
          fillNodePropsFromGroupParams(a_it, ad_params->groupA_params, &icp_ad_params->to_params);

          // MRPT_LOG_DEBUG_STREAM(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
          // MRPT_LOG_DEBUG_STREAM("From nodeID (other): " << *b_it);
          // MRPT_LOG_DEBUG_STREAM("From params (other): "
          //<< icp_ad_params->from_params.getAsString());
          // MRPT_LOG_DEBUG_STREAM("from_success: " << (from_success?
          // "TRUE" : "FALSE"));
          // MRPT_LOG_DEBUG_STREAM("**********");
          // MRPT_LOG_DEBUG_STREAM("To nodeID (own)   : " << *a_it);
          // MRPT_LOG_DEBUG_STREAM("To params (own)   : "
          //<< icp_ad_params->to_params.getAsString());
          // MRPT_LOG_DEBUG_STREAM("to_success: " << (to_success?
          // "TRUE" : "FALSE"));
          // MRPT_LOG_DEBUG_STREAM("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
        }

        // fetch the ICP constraint bi => ai
        mrpt::slam::CICP::TReturnInfo icp_info;
        constraint_t edge;
        bool found_edge = this->getICPEdge(b_it, a_it, &edge, &icp_info, icp_ad_params);

        hypot->setEdge(edge);
        hypot->goodness = icp_info.goodness;  // goodness related to the edge

        // Check if invalid
        //
        // Goodness Threshold
        double goodness_thresh =
            m_laser_params.goodness_threshold_win.getMedian() * m_lc_icp_constraint_factor;
        bool accept_goodness = icp_info.goodness > goodness_thresh;
        MRPT_LOG_DEBUG_STREAM(
            "generateHypotsPool:\nCurr. Goodness: "
            << icp_info.goodness << "|\t Threshold: " << goodness_thresh << " => "
            << (accept_goodness ? "ACCEPT" : "REJECT") << endl);

        if (!found_edge || !accept_goodness)
        {
          hypot->is_valid = false;
          invalid_hypots++;
        }
        generated_hypots->push_back(hypot);
        MRPT_LOG_DEBUG_STREAM(hypot->getAsString());

        // delete pointer to getICPEdge additional parameters if they
        // were initialized
        delete icp_ad_params;
      }
    }
    MRPT_LOG_DEBUG_STREAM(
        "Generated pool of hypotheses...\tsize = " << generated_hypots->size()
                                                   << "\tinvalid hypotheses: " << invalid_hypots);
  }

  MRPT_END
}  // end of generateHypotsPool

template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::computeDominantEigenVector(
    const mrpt::math::CMatrixDouble& consist_matrix,
    mrpt::math::CVectorDouble* eigvec,
    bool use_power_method)
{
  MRPT_START
  using namespace mrpt;
  using namespace mrpt::math;
  using namespace std;
  ASSERTDEB_(eigvec);

  this->m_time_logger.enter("DominantEigenvectorComputation");

  double lambda1, lambda2;  // eigenvalues to use
  bool is_valid_lambda_ratio = false;

  if (use_power_method)
  {
    THROW_EXCEPTION(
        "\nPower method for computing the first two "
        "eigenvectors/eigenvalues hasn't been implemented yet\n");
  }
  else
  {  // call to eigenVectors method
    CMatrixDouble eigvecs;
    std::vector<double> eigvals;
    consist_matrix.eig(eigvecs, eigvals);

    // assert that the eivenvectors, eigenvalues, consistency matrix are of
    // the same size
    ASSERTDEBMSG_(
        eigvecs.size() == eigvals.size() && consist_matrix.cols() == eigvals.size(),
        mrpt::format(
            "Size of eigvecs \"%lu\","
            "eigvalues \"%lu\","
            "consist_matrix \"%lu\" don't match",
            static_cast<unsigned long>(eigvecs.size()), static_cast<unsigned long>(eigvals.size()),
            static_cast<unsigned long>(consist_matrix.size())));

    // copy. I don't care about the sign of the eigenvector element
    for (int i = 0; i != eigvec->size(); ++i)
      (*eigvec)[i] = std::abs(eigvecs(i, eigvecs.cols() - 1));

    lambda1 = eigvals[eigvals.size() - 1];
    lambda2 = eigvals[eigvals.size() - 2];
  }

  // check the ratio of the two eigenvalues - reject hypotheses set if ratio
  // smaller than threshold
  if (approximatelyEqual(0.0, lambda2, 0.00001))
  {
    MRPT_LOG_ERROR_STREAM(
        "Bad lambda2 value: " << lambda2 << " => Skipping current evaluation." << endl);
    return false;
  }
  double curr_lambda_ratio = lambda1 / lambda2;
  MRPT_LOG_DEBUG_STREAM(
      "lambda1 = " << lambda1 << " | lambda2 = " << lambda2 << "| ratio = " << curr_lambda_ratio
                   << endl);

  is_valid_lambda_ratio = (curr_lambda_ratio > m_lc_params.LC_eigenvalues_ratio_thresh);

  this->m_time_logger.leave("DominantEigenvectorComputation");
  return is_valid_lambda_ratio;

  MRPT_END
}  // end of computeDominantEigenVector

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::generatePWConsistenciesMatrix(
    const std::vector<uint32_t>& groupA,
    const std::vector<uint32_t>& groupB,
    const hypotsp_t& hypots_pool,
    mrpt::math::CMatrixDouble* consist_matrix,
    const paths_t* groupA_opt_paths,
    const paths_t* groupB_opt_paths)
{
  MRPT_START

  using namespace mrpt;
  using namespace mrpt::math;
  using namespace mrpt::containers;
  using namespace std;
  using mrpt::graphs::TNodeID;

  ASSERTDEBMSG_(consist_matrix, "Invalid pointer to the Consistency matrix is given");
  ASSERTDEBMSG_(
      static_cast<unsigned long>(consist_matrix->rows()) == hypots_pool.size() &&
          static_cast<unsigned long>(consist_matrix->rows()) == hypots_pool.size(),
      "Consistency matrix dimensions aren't equal to the hypotheses pool "
      "size");

  MRPT_LOG_DEBUG_STREAM(
      ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << endl
                                         << "In generatePWConsistencyMatrix:\n"
                                         << "\tgroupA: " << getSTLContainerAsString(groupA) << endl
                                         << "\tgroupB: " << getSTLContainerAsString(groupB) << endl
                                         << "\tHypots pool Size: " << hypots_pool.size());

  // b1
  for (auto b1_it = groupB.begin(); b1_it != groupB.end(); ++b1_it)
  {
    TNodeID b1 = *b1_it;

    // b2
    for (auto b2_it = b1_it + 1; b2_it != groupB.end(); ++b2_it)
    {
      TNodeID b2 = *b2_it;

      // a1
      for (auto a1_it = groupA.begin(); a1_it != groupA.end(); ++a1_it)
      {
        TNodeID a1 = *a1_it;
        hypot_t* hypot_b2_a1 = this->findHypotByEnds(hypots_pool, b2, a1);
        // MRPT_LOG_DEBUG_STREAM("hypot_b2_a1: " <<
        // hypot_b2_a1->getAsString());

        // a2
        for (auto a2_it = a1_it + 1; a2_it != groupA.end(); ++a2_it)
        {
          TNodeID a2 = *a2_it;
          hypot_t* hypot_b1_a2 = this->findHypotByEnds(hypots_pool, b1, a2);
          // MRPT_LOG_DEBUG_STREAM("hypot_b1_a2: " <<
          // hypot_b1_a2->getAsString());

          double consistency;

          // compute consistency element
          if (hypot_b2_a1->is_valid && hypot_b1_a2->is_valid)
          {
            // extract vector of hypotheses that connect the given
            // nodes,
            // instead of passing the whole hypothesis pool.
            hypotsp_t extracted_hypots;
            extracted_hypots.push_back(hypot_b2_a1);
            extracted_hypots.push_back(hypot_b1_a2);

            paths_t* curr_opt_paths = nullptr;
            if (groupA_opt_paths || groupB_opt_paths)
            {  // fill curr_opt_paths
              curr_opt_paths = new paths_t();
            }

            // decide on additional optimal paths
            if (curr_opt_paths)
            {
              // groupA
              if (groupA_opt_paths)
              {  // a1 -> a2 optimal path
                const path_t* p = this->findPathByEnds(*groupA_opt_paths, a1, a2, true);
                curr_opt_paths->push_back(*p);
              }
              else
              {  // empty
                curr_opt_paths->push_back(path_t());
              }

              if (groupB_opt_paths)
              {  // b1 -> b2 optimal path
                const path_t* p = this->findPathByEnds(*groupB_opt_paths, b1, b2, true);
                curr_opt_paths->push_back(*p);
              }
              else
              {  // empty
                curr_opt_paths->push_back(path_t());
              }
            }

            consistency = this->generatePWConsistencyElement(
                a1, a2, b1, b2, extracted_hypots, curr_opt_paths);

            delete curr_opt_paths;
          }
          else
          {  //  null those that don't look good
            consistency = 0;
          }

          // MRPT_LOG_DEBUG_STREAM(
          //"Adding hypothesis consistency for nodeIDs: "
          //<< "[b1] " << b1 << ", [b2] -> " << b2
          //<< ", [a1] -> " << a1 << ", [a2] -> " << a2
          //<< " ==> " << consistency << endl);

          // fill the PW consistency matrix corresponding element -
          // symmetrical
          int id1 = hypot_b2_a1->id;
          int id2 = hypot_b1_a2->id;

          (*consist_matrix)(id1, id2) = consistency;
          (*consist_matrix)(id2, id1) = consistency;

          // MRPT_LOG_DEBUG_STREAM("id1 = " << id1 << "\t|"
          //<< "id2 = " << id2 << "\t|"
          //<< "consistency = " << consistency);
        }
      }
    }
  }

  // MRPT_LOG_WARN_STREAM("Consistency matrix:" << endl
  //<< header_sep << endl
  //<< *consist_matrix << endl);

  MRPT_END
}  // end of generatePWConsistenciesMatrix

template <class GRAPH_T>
double CLoopCloserERD<GRAPH_T>::generatePWConsistencyElement(
    const mrpt::graphs::TNodeID& a1,
    const mrpt::graphs::TNodeID& a2,
    const mrpt::graphs::TNodeID& b1,
    const mrpt::graphs::TNodeID& b2,
    const hypotsp_t& hypots,
    const paths_t* opt_paths)
{
  MRPT_START
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::math;
  using namespace mrpt::graphslam;
  using namespace mrpt::graphslam::detail;

  // MRPT_LOG_DEBUG_STREAM("In generatePWConsistencyElement.\n"
  //<< "\t[" << a1 << ", " << a2 << "]\n"
  //<< "\t[" << b1 << ", " << b2 << "]");
  // MRPT_LOG_DEBUG_STREAM("a1->a2 optimal path:"
  //<< (opt_paths && !opt_paths->begin()->isEmpty()?
  // opt_paths->begin()->getAsString() :
  //"NONE"));
  // MRPT_LOG_DEBUG_STREAM("b1->b2 optimal path: "
  //<< (opt_paths && !opt_paths->rbegin()->isEmpty()?
  // opt_paths->rbegin()->getAsString() :
  //"NONE"));

  // standard size assertions
  ASSERTDEB_(hypots.size() == 2);
  if (opt_paths)
  {
    ASSERTDEB_(opt_paths->size() == 2);
  }

  //
  // get the Dijkstra links
  //
  // a1 ==> a2
  const path_t* path_a1_a2;
  if (!opt_paths || opt_paths->begin()->isEmpty())
  {
    MRPT_LOG_DEBUG_STREAM("Running dijkstra [a1] " << a1 << " => [a2] " << a2);
    execDijkstraProjection(a1, a2);
    path_a1_a2 = this->queryOptimalPath(a2);
  }
  else
  {  // fetch the path from the opt_paths arg
    // TODO dubious practice
    path_a1_a2 = &(*opt_paths->begin());
  }
  ASSERTDEB_(path_a1_a2);
  path_a1_a2->assertIsBetweenNodeIDs(a1, a2);

  // b1 ==> b2
  const path_t* path_b1_b2;
  if (!opt_paths || opt_paths->rend()->isEmpty())
  {
    MRPT_LOG_DEBUG_STREAM("Running djkstra [b1] " << b1 << " => [b2] " << b2);
    execDijkstraProjection(b1, b2);
    path_b1_b2 = this->queryOptimalPath(b2);
  }
  else
  {  // fetch the path from the opt_paths arg
    path_b1_b2 = &(*opt_paths->rbegin());
  }
  ASSERTDEB_(path_b1_b2);
  path_b1_b2->assertIsBetweenNodeIDs(b1, b2);
  // get the edges of the hypotheses
  // by default hypotheses are stored bi => ai
  //
  // Backwards edge: a2=>b1
  hypot_t* hypot_b1_a2 = this->findHypotByEnds(hypots, b1, a2);
  // forward edge b2=>a1
  hypot_t* hypot_b2_a1 = this->findHypotByEnds(hypots, b2, a1);

  // Composition of Poses
  // Order : a1 ==> a2 ==> b1 ==> b2 ==> a1
  constraint_t res_transform(path_a1_a2->curr_pose_pdf);
  res_transform += hypot_b1_a2->getInverseEdge();
  res_transform += path_b1_b2->curr_pose_pdf;
  res_transform += hypot_b2_a1->getEdge();

  MRPT_LOG_DEBUG_STREAM(
      "\n-----------Resulting Transformation----------- Hypots: #"
      << hypot_b1_a2->id << ", #" << hypot_b2_a1->id << endl
      << "a1 --> a2 => b1 --> b2 => a1: " << a1 << " --> " << a2 << " => " << b1 << " --> " << b2
      << " => " << a1 << endl
      << res_transform << endl
      << endl
      << "DIJKSTRA: " << a1 << " --> " << a2 << ": " << path_a1_a2->curr_pose_pdf << endl
      << "DIJKSTRA: " << b1 << " --> " << b2 << ": " << path_b1_b2->curr_pose_pdf << endl
      << "hypot_b1_a2(inv):\n"
      << hypot_b1_a2->getInverseEdge() << endl
      << "hypot_b2_a1:\n"
      << hypot_b2_a1->getEdge() << endl);

  // get the vector of the corresponding transformation - [x, y, phi] form
  typename pose_t::vector_t T;
  res_transform.getMeanVal().asVector(T);

  // information matrix
  CMatrixDouble33 cov_mat;
  res_transform.getCovariance(cov_mat);

  // there has to be an error with the initial Olson formula - p.15.
  // There must be a minus in the exponent and the covariance matrix instead
  // of
  // the information matrix.
  double exponent = -mrpt::math::multiply_HtCH_scalar(T, cov_mat);
  double consistency_elem = exp(exponent);

  // cout << "T = " << endl << T << endl;
  // cout << "exponent = " << exponent << endl;
  // cout << "consistency_elem = " << consistency_elem << endl;
  // mrpt::system::pause();

  return consistency_elem;
  MRPT_END
}  // end of generatePWConsistencyElement

template <class GRAPH_T>
const mrpt::graphslam::TUncertaintyPath<GRAPH_T>* CLoopCloserERD<GRAPH_T>::findPathByEnds(
    const paths_t& vec_paths,
    const mrpt::graphs::TNodeID& src,
    const mrpt::graphs::TNodeID& dst,
    bool throw_exc)
{
  using namespace mrpt;

  ASSERTDEB_(vec_paths.size());
  const path_t* res = nullptr;

  for (auto cit = vec_paths.begin(); cit != vec_paths.end(); ++cit)
  {
    if (cit->getSource() == src && cit->getDestination() == dst)
    {
      res = &(*cit);
      break;
    }
  }
  if (throw_exc && !res)
  {
    THROW_EXCEPTION(format(
        "Path for %lu => %lu is not found. Exiting...\n", static_cast<unsigned long>(src),
        static_cast<unsigned long>(dst)));
  }

  return res;
}

template <class GRAPH_T>
mrpt::graphs::detail::THypothesis<GRAPH_T>* CLoopCloserERD<GRAPH_T>::findHypotByEnds(
    const hypotsp_t& vec_hypots,
    const mrpt::graphs::TNodeID& from,
    const mrpt::graphs::TNodeID& to,
    bool throw_exc)
{
  using namespace mrpt::graphslam::detail;
  using namespace std;

  for (auto v_cit = vec_hypots.begin(); v_cit != vec_hypots.end(); v_cit++)
  {
    if ((*v_cit)->hasEnds(from, to))
    {
      // cout << "findHypotByEnds: Found hypot " << from
      //<< " => " << to << " : " << (*v_cit)->getAsString() << endl;
      return *v_cit;
    }
  }

  // not found.
  if (throw_exc)
  {
    throw mrpt::graphs::HypothesisNotFoundException(from, to);
  }
  else
  {
    return nullptr;
  }
}  // end of findHypotByEnds

template <class GRAPH_T>
mrpt::graphs::detail::THypothesis<GRAPH_T>* CLoopCloserERD<GRAPH_T>::findHypotByID(
    const hypotsp_t& vec_hypots, size_t id, bool throw_exc)
{
  using namespace mrpt::graphslam::detail;

  for (auto v_cit = vec_hypots.begin(); v_cit != vec_hypots.end(); v_cit++)
  {
    if ((*v_cit)->id == id)
    {
      return *v_cit;
    }
  }

  // not found.
  if (throw_exc)
  {
    throw mrpt::graphs::HypothesisNotFoundException(id);
  }
  else
  {
    return nullptr;
  }
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::execDijkstraProjection(
    mrpt::graphs::TNodeID starting_node, mrpt::graphs::TNodeID ending_node)
{
  MRPT_START
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::math;
  using mrpt::graphs::TNodeID;

  // for the full algorithm see
  // - Recognizing places using spectrally clustered local matches - E.Olson,
  // p.6

  this->m_time_logger.enter("Dijkstra Projection");
  const std::string dijkstra_end = "----------- Done with Dijkstra Projection... ----------";

  // ending_node is either INVALID_NODEID or one of the already registered
  // nodeIDs
  ASSERTDEB_(
      ending_node == mrpt::graphs::INVALID_NODEID ||
      (ending_node >= 0 && ending_node < this->m_graph->nodeCount()));
  ASSERTDEBMSG_(starting_node != ending_node, "Starting and Ending nodes coincede");

  // debugging message
  stringstream ss_debug("");
  ss_debug << "Executing Dijkstra Projection: " << starting_node << " => ";
  if (ending_node == mrpt::graphs::INVALID_NODEID)
  {
    ss_debug << "..." << endl;
  }
  else
  {
    ss_debug << ending_node << endl;
  }

  if (this->m_graph->nodeCount() < m_dijkstra_node_count_thresh)
  {
    return;
  }

  // keep track of the nodes that I have visited
  std::vector<bool> visited_nodes(this->m_graph->nodeCount(), false);
  m_node_optimal_paths.clear();

  // get the neighbors of each node
  std::map<TNodeID, std::set<TNodeID>> neighbors_of;
  this->m_graph->getAdjacencyMatrix(neighbors_of);

  // initialize a pool of TUncertaintyPaths - draw the minimum-uncertainty
  // path during
  // execution
  std::set<path_t*> pool_of_paths;
  // get the edge to each one of the neighboring nodes of the starting node
  std::set<TNodeID> starting_node_neighbors(neighbors_of.at(starting_node));
  for (unsigned long starting_node_neighbor : starting_node_neighbors)
  {
    auto* path_between_neighbors = new path_t();
    this->getMinUncertaintyPath(starting_node, starting_node_neighbor, path_between_neighbors);

    pool_of_paths.insert(path_between_neighbors);
  }
  // just visited the first node
  visited_nodes.at(starting_node) = true;

  //// TODO Remove these - >>>>>>>>>>>>>>>>>>>>
  //// printing the pool for verification
  // cout << "Pool of Paths: " << endl;
  // for (typename std::set<path_t*>::const_iterator it =
  // pool_of_paths.begin();
  // it != pool_of_paths.end(); ++it) {
  // printSTLContainer((*it)->nodes_traversed);
  //}
  // cout << "------ Done with the starting node ... ------" << endl;
  // int iters = 0;
  //// TODO Remove these - <<<<<<<<<<<<<<<<<<<<< vvvUNCOMMENT BELOW AS WELLvvv

  while (true)
  {
    // if there is at least one false, exit loop
    for (auto it = visited_nodes.begin(); it != visited_nodes.end(); ++it)
    {
      if (!*it)
      {
        break;
      }
    }

    // if an ending nodeID has been specified, end the method when the path
    // to
    // it is found.
    if (ending_node != mrpt::graphs::INVALID_NODEID)
    {
      if (visited_nodes.at(ending_node))
      {
        // MRPT_LOG_DEBUG_STREAM(dijkstra_end);
        this->m_time_logger.leave("Dijkstra Projection");
        return;
      }
    }

    path_t* optimal_path = this->popMinUncertaintyPath(&pool_of_paths);
    TNodeID dest = optimal_path->getDestination();

    //// TODO Remove these - >>>>>>>>>>>>>>>>>>>> ^^^UNCOMMENT ABOVE AS
    /// WELL^^^
    // cout << iters << " " << std::string(40, '>') << endl;
    // cout << "current path Destination: " << dest << endl;
    //// printing the pool for verification
    // cout << "Pool of Paths: " << endl;
    // for (typename std::set<path_t*>::const_iterator
    // it = pool_of_paths.begin();
    // it != pool_of_paths.end(); ++it) {
    // printSTLContainer((*it)->nodes_traversed);
    //}
    // cout << "Nodes visited: " << endl;
    // std::vector<int> tmp_vec;
    // for (int i = 0; i != visited_nodes.size(); ++i) {
    // tmp_vec.push_back(i);
    //}
    // printSTLContainer(tmp_vec); cout << endl; // indices of numbers
    // printSTLContainer(visited_nodes);         // actual flags
    // cout << std::string(40, '<') << " " << iters++ << endl;
    // mrpt::system::pause();
    //// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<

    if (!visited_nodes.at(dest))
    {
      m_node_optimal_paths[dest] = optimal_path;
      visited_nodes.at(dest) = true;

      // for all the edges leaving this node .. compose the transforms
      // with the
      // current pool of paths.
      this->addToPaths(&pool_of_paths, *optimal_path, neighbors_of.at(dest));
    }
  }

  // MRPT_LOG_DEBUG_STREAM(dijkstra_end);
  this->m_time_logger.leave("Dijkstra Projection");
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::addToPaths(
    std::set<path_t*>* pool_of_paths,
    const path_t& current_path,
    const std::set<mrpt::graphs::TNodeID>& neighbors) const
{
  MRPT_START
  using namespace std;
  using namespace mrpt::graphslam;
  using mrpt::graphs::TNodeID;

  TNodeID node_to_append_from = current_path.getDestination();

  // compose transforms for every neighbor of node_to_append_from *except*
  // for the link connecting node_to_append_from and the second to last node
  // in
  // the current_path
  TNodeID second_to_last_node = current_path.nodes_traversed.rbegin()[1];
  for (unsigned long neighbor : neighbors)
  {
    if (neighbor == second_to_last_node) continue;

    // get the path between node_to_append_from, *node_it
    path_t path_between_nodes;
    this->getMinUncertaintyPath(node_to_append_from, neighbor, &path_between_nodes);

    // format the path to append
    auto* path_to_append = new path_t();
    *path_to_append = current_path;
    *path_to_append += path_between_nodes;

    pool_of_paths->insert(path_to_append);
  }

  MRPT_END
}

template <class GRAPH_T>
typename mrpt::graphslam::TUncertaintyPath<GRAPH_T>* CLoopCloserERD<GRAPH_T>::queryOptimalPath(
    const mrpt::graphs::TNodeID node) const
{
  using namespace std;

  path_t* path = nullptr;
  typename std::map<mrpt::graphs::TNodeID, path_t*>::const_iterator search;
  search = m_node_optimal_paths.find(node);
  if (search != m_node_optimal_paths.end())
  {
    path = search->second;
  }

  // MRPT_LOG_DEBUG_STREAM("Queried optimal path for nodeID: " << node
  //<< " ==> Path: " << (path? "Found" : "NOT Found"));
  return path;
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::getMinUncertaintyPath(
    const mrpt::graphs::TNodeID from,
    const mrpt::graphs::TNodeID to,
    path_t* path_between_nodes) const
{
  MRPT_START
  using namespace mrpt::math;
  using namespace std;

  ASSERTDEBMSG_(
      this->m_graph->edgeExists(from, to) || this->m_graph->edgeExists(to, from),
      mrpt::format(
          "\nEdge between the provided nodeIDs"
          "(%lu <-> %lu) does not exist\n",
          from, to));
  ASSERTDEB_(path_between_nodes);

  // cout << "getMinUncertaintyPath: " << from << " => " << to << endl;

  // don't add to the path_between_nodes, just fill it in afterwards
  path_between_nodes->clear();

  // iterate over all the edges, ignore the ones that are all 0s - find the
  // one that is with the lowest uncertainty
  double curr_determinant = 0;
  // forward edges from -> to
  std::pair<edges_citerator, edges_citerator> fwd_edges_pair = this->m_graph->getEdges(from, to);

  // cout << "Forward edges: " << endl;
  // for (edges_citerator e_it = fwd_edges_pair.first; e_it !=
  // fwd_edges_pair.second;
  //++e_it) {
  // cout << e_it->second << endl;
  //}

  for (auto edges_it = fwd_edges_pair.first; edges_it != fwd_edges_pair.second; ++edges_it)
  {
    // operate on a temporary object instead of the real edge - otherwise
    // function is non-const
    constraint_t curr_edge;
    curr_edge.copyFrom(edges_it->second);

    // is it all 0s?
    CMatrixDouble33 inf_mat;
    curr_edge.getInformationMatrix(inf_mat);

    if (inf_mat == CMatrixDouble33() || std::isnan(inf_mat(0, 0)))
    {
      inf_mat.setIdentity();
      curr_edge.cov_inv = inf_mat;
    }

    path_t curr_path(from);  // set the starting node
    curr_path.addToPath(to, curr_edge);

    // update the resulting path_between_nodes if its determinant is smaller
    // than the determinant of the current path_between_nodes
    if (curr_determinant < curr_path.getDeterminant())
    {
      curr_determinant = curr_path.getDeterminant();
      *path_between_nodes = curr_path;
    }
  }
  // backwards edges to -> from
  std::pair<edges_citerator, edges_citerator> bwd_edges_pair = this->m_graph->getEdges(to, from);

  // cout << "Backwards edges: " << endl;
  // for (edges_citerator e_it = bwd_edges_pair.first; e_it !=
  // bwd_edges_pair.second;
  //++e_it) {
  // cout << e_it->second << endl;
  //}

  for (auto edges_it = bwd_edges_pair.first; edges_it != bwd_edges_pair.second; ++edges_it)
  {
    // operate on a temporary object instead of the real edge - otherwise
    // function is non-const
    constraint_t curr_edge;
    (edges_it->second).inverse(curr_edge);

    // is it all 0s?
    CMatrixDouble33 inf_mat;
    curr_edge.getInformationMatrix(inf_mat);

    if (inf_mat == CMatrixDouble33() || std::isnan(inf_mat(0, 0)))
    {
      inf_mat.setIdentity();
      curr_edge.cov_inv = inf_mat;
    }

    path_t curr_path(from);  // set the starting node
    curr_path.addToPath(to, curr_edge);

    // update the resulting path_between_nodes if its determinant is smaller
    // than the determinant of the current path_between_nodes
    if (curr_determinant < curr_path.getDeterminant())
    {
      curr_determinant = curr_path.getDeterminant();
      *path_between_nodes = curr_path;
    }
  }

  MRPT_END
}

template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>* CLoopCloserERD<GRAPH_T>::popMinUncertaintyPath(
    typename std::set<path_t*>* pool_of_paths) const
{
  MRPT_START
  using namespace std;

  // cout << "Determinants: ";
  path_t* optimal_path = nullptr;
  double curr_determinant = 0;
  for (auto it = pool_of_paths->begin(); it != pool_of_paths->end(); ++it)
  {
    // cout << (*it)->getDeterminant() << ", ";

    // keep the largest determinant - we are in INFORMATION form.
    if (curr_determinant < (*it)->getDeterminant())
    {
      curr_determinant = (*it)->getDeterminant();
      optimal_path = *it;
    }
  }

  ASSERTDEB_(optimal_path);
  pool_of_paths->erase(optimal_path);  // erase it from the pool

  return optimal_path;
  MRPT_END
}

template <class GRAPH_T>
bool CLoopCloserERD<GRAPH_T>::mahalanobisDistanceOdometryToICPEdge(
    const mrpt::graphs::TNodeID& from,
    const mrpt::graphs::TNodeID& to,
    const constraint_t& rel_edge)
{
  MRPT_START

  using namespace std;
  using namespace mrpt::math;

  // mean difference
  pose_t initial_estim = this->m_graph->nodes.at(to) - this->m_graph->nodes.at(from);
  typename pose_t::vector_t mean_diff;
  (rel_edge.getMeanVal() - initial_estim).asVector(mean_diff);

  // covariance matrix
  CMatrixDouble33 cov_mat;
  rel_edge.getCovariance(cov_mat);

  // mahalanobis distance computation
  double mahal_distance = mrpt::math::mahalanobisDistance2(mean_diff, cov_mat);
  bool mahal_distance_null = std::isnan(mahal_distance);
  if (!mahal_distance_null)
  {
    m_laser_params.mahal_distance_ICP_odom_win.addNewMeasurement(mahal_distance);
  }

  // double threshold = m_laser_params.mahal_distance_ICP_odom_win.getMean() +
  // 2*m_laser_params.mahal_distance_ICP_odom_win.getStdDev();
  double threshold = m_laser_params.mahal_distance_ICP_odom_win.getMedian() * 4;
  bool accept_edge = (threshold >= mahal_distance && !mahal_distance_null) ? true : false;

  // cout << "Suggested Edge: " << rel_edge.getMeanVal() << "|\tInitial
  // Estim.: " << initial_estim
  //<< "|\tMahalanobis Dist: " << mahal_distance << "|\tThresh.: " <<
  // threshold
  //<< " => " << (accept_edge? "ACCEPT": "REJECT") << endl;

  return accept_edge;
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::registerHypothesis(const hypot_t& hypot)
{
  // MRPT_LOG_DEBUG_STREAM("Registering hypothesis: " <<
  // hypot.getAsString([>oneline=<] true));
  this->registerNewEdge(hypot.from, hypot.to, hypot.getEdge());
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::registerNewEdge(
    const mrpt::graphs::TNodeID& from,
    const mrpt::graphs::TNodeID& to,
    const constraint_t& rel_edge)
{
  MRPT_START
  using namespace mrpt::math;
  using namespace std;
  parent_t::registerNewEdge(from, to, rel_edge);

  //  keep track of the registered edges...
  m_edge_types_to_nums["ICP2D"]++;

  //  keep track of the registered edges...
  if (absDiff(to, from) > m_lc_params.LC_min_nodeid_diff)
  {
    m_edge_types_to_nums["LC"]++;
    this->m_just_inserted_lc = true;
    MRPT_LOG_INFO("\tLoop Closure edge!");
  }
  else
  {
    this->m_just_inserted_lc = false;
  }

  //  actuall registration
  this->m_graph->insertEdge(from, to, rel_edge);

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager)
{
  // call parent_t class method
  parent_t::setWindowManagerPtr(win_manager);

  // may still be null..
  if (this->m_win_manager)
  {
    if (this->m_win_observer)
    {
      this->m_win_observer->registerKeystroke(
          m_laser_params.keystroke_laser_scans, "Toggle LaserScans Visualization");
      this->m_win_observer->registerKeystroke(
          m_lc_params.keystroke_map_partitions, "Toggle Map Partitions Visualization");
    }

    MRPT_LOG_DEBUG("Fetched the window manager, window observer  successfully.");
  }
}
template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::notifyOfWindowEvents(
    const std::map<std::string, bool>& events_occurred)
{
  MRPT_START
  parent_t::notifyOfWindowEvents(events_occurred);

  // laser scans
  if (events_occurred.at(m_laser_params.keystroke_laser_scans))
  {
    this->toggleLaserScansVisualization();
  }
  // map partitions
  if (events_occurred.at(m_lc_params.keystroke_map_partitions))
  {
    this->toggleMapPartitionsVisualization();
  }

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::initMapPartitionsVisualization()
{
  using namespace mrpt;
  using namespace mrpt::gui;
  using namespace mrpt::math;
  using namespace mrpt::opengl;

  // textmessage - display the number of partitions
  this->m_win_manager->assignTextMessageParameters(
      &m_lc_params.offset_y_map_partitions, &m_lc_params.text_index_map_partitions);

  // just add an empty CSetOfObjects in the scene - going to populate it later
  CSetOfObjects::Ptr map_partitions_obj = std::make_shared<CSetOfObjects>();
  map_partitions_obj->setName("map_partitions");

  Scene::Ptr& scene = this->m_win->get3DSceneAndLock();
  scene->insert(map_partitions_obj);
  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::updateMapPartitionsVisualization()
{
  using namespace mrpt;
  using namespace mrpt::gui;
  using namespace mrpt::math;
  using namespace mrpt::opengl;
  using namespace mrpt::poses;

  // textmessage
  std::stringstream title;
  title << "# Partitions: " << m_curr_partitions.size();
  this->m_win_manager->addTextMessage(
      5, -m_lc_params.offset_y_map_partitions, title.str(),
      mrpt::img::TColorf(m_lc_params.balloon_std_color), m_lc_params.text_index_map_partitions);

  // update the partitioning visualization
  Scene::Ptr& scene = this->m_win->get3DSceneAndLock();

  // fetch the partitions CSetOfObjects
  CSetOfObjects::Ptr map_partitions_obj;
  {
    CRenderizable::Ptr obj = scene->getByName("map_partitions");
    // do not check for null ptr - must be properly created in the init*
    // method
    map_partitions_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
  }

  int partitionID = 0;
  bool partition_contains_last_node = false;
  bool found_last_node = false;  // last node must exist in one partition at all cost TODO
  MRPT_LOG_DEBUG_STREAM(
      "Searching for the partition of the last nodeID: " << (this->m_graph->nodeCount() - 1));

  for (auto p_it = m_curr_partitions.begin(); p_it != m_curr_partitions.end();
       ++p_it, ++partitionID)
  {
    // MRPT_LOG_DEBUG_STREAM("Working on Partition #" << partitionID);
    std::vector<uint32_t> nodes_list = *p_it;

    // finding the partition in which the last node is in
    if (std::find(nodes_list.begin(), nodes_list.end(), this->m_graph->nodeCount() - 1) !=
        nodes_list.end())
    {
      partition_contains_last_node = true;

      found_last_node = true;
    }
    else
    {
      partition_contains_last_node = false;
    }

    // fetch the current partition object if it exists - create otherwise
    std::string partition_obj_name = mrpt::format("partition_%d", partitionID);
    std::string balloon_obj_name = mrpt::format("#%d", partitionID);

    CRenderizable::Ptr obj = map_partitions_obj->getByName(partition_obj_name);
    CSetOfObjects::Ptr curr_partition_obj;
    if (obj)
    {
      // MRPT_LOG_DEBUG_STREAM(
      //"\tFetching CSetOfObjects partition object for partition #" <<
      // partitionID);
      curr_partition_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
      if (m_lc_params.LC_check_curr_partition_only)
      {  // make all but the last
         // partition invisible
        curr_partition_obj->setVisibility(partition_contains_last_node);
      }
    }
    else
    {
      MRPT_LOG_DEBUG_STREAM(
          "\tCreating a new CSetOfObjects partition object for partition "
          "#"
          << partitionID);
      curr_partition_obj = std::make_shared<CSetOfObjects>();
      curr_partition_obj->setName(partition_obj_name);
      if (m_lc_params.LC_check_curr_partition_only)
      {
        // make all but the last partition invisible
        curr_partition_obj->setVisibility(partition_contains_last_node);
      }

      // MRPT_LOG_DEBUG_STREAM("\t\tCreating a new CSphere balloon
      // object");
      CSphere::Ptr balloon_obj = std::make_shared<CSphere>();
      balloon_obj->setName(balloon_obj_name);
      balloon_obj->setRadius(m_lc_params.balloon_radius);
      balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
      balloon_obj->enableShowName();

      curr_partition_obj->insert(balloon_obj);

      // set of lines connecting the graph nodes to the balloon
      // MRPT_LOG_DEBUG_STREAM(
      //"\t\tCreating set of lines that will connect to the Balloon");
      CSetOfLines::Ptr connecting_lines_obj = std::make_shared<CSetOfLines>();
      connecting_lines_obj->setName("connecting_lines");
      connecting_lines_obj->setColor_u8(m_lc_params.connecting_lines_color);
      connecting_lines_obj->setLineWidth(0.1f);

      curr_partition_obj->insert(connecting_lines_obj);

      // add the created CSetOfObjects to the total CSetOfObjects
      // responsible
      // for the map partitioning
      map_partitions_obj->insert(curr_partition_obj);
      // MRPT_LOG_DEBUG_STREAM("\tInserted new CSetOfObjects
      // successfully");
    }
    // up to now the CSetOfObjects exists and the balloon inside it as
    // well..

    std::pair<double, double> centroid_coords;
    this->computeCentroidOfNodesVector(nodes_list, &centroid_coords);

    TPoint3D balloon_location(
        centroid_coords.first, centroid_coords.second, m_lc_params.balloon_elevation);

    // MRPT_LOG_DEBUG_STREAM("\tUpdating the balloon position");
    // set the balloon properties
    CSphere::Ptr balloon_obj;
    {
      // place the partitions baloon at the centroid elevated by a fixed Z
      // value
      CRenderizable::Ptr _obj = curr_partition_obj->getByName(balloon_obj_name);
      balloon_obj = std::dynamic_pointer_cast<CSphere>(_obj);
      balloon_obj->setLocation(balloon_location);
      if (partition_contains_last_node)
        balloon_obj->setColor_u8(m_lc_params.balloon_curr_color);
      else
        balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
    }

    // MRPT_LOG_DEBUG_STREAM("\tUpdating the lines connecting nodes to
    // balloon");
    // set the lines connecting the nodes of the partition to the partition
    // balloon - set it from scratch all the times since the node positions
    // tend to change according to the dijkstra position estimation
    CSetOfLines::Ptr connecting_lines_obj;
    {
      // place the partitions baloon at the centroid elevated by a fixed Z
      // value
      CRenderizable::Ptr _obj = curr_partition_obj->getByName("connecting_lines");
      connecting_lines_obj = std::dynamic_pointer_cast<CSetOfLines>(_obj);

      connecting_lines_obj->clear();

      for (auto it = nodes_list.begin(); it != nodes_list.end(); ++it)
      {
        CPose3D curr_pose(this->m_graph->nodes.at(*it));
        TPoint3D curr_node_location(curr_pose.asTPose());

        TSegment3D connecting_line(curr_node_location, balloon_location);
        connecting_lines_obj->appendLine(connecting_line);
      }
    }
    // MRPT_LOG_DEBUG_STREAM("Done working on partition #" << partitionID);
  }

  if (!found_last_node)
  {
    /// @todo Have some sorts of a string_view instead
    THROW_EXCEPTION("Last inserted nodeID was not found in any partition.");
  }

  // remove outdated partitions
  // these occur when more partitions existed during the previous
  // visualization
  // update, thus the partitions with higher ID than the maximum partitionID
  // would otherwise remain in the visual as zombie partitions
  const size_t prev_size = m_last_partitions.size();
  const size_t curr_size = m_curr_partitions.size();
  if (curr_size < prev_size)
  {
    MRPT_LOG_DEBUG_STREAM("Removing outdated partitions in visual");
    for (size_t pID = curr_size; pID != prev_size; ++pID)
    {
      MRPT_LOG_DEBUG_STREAM("\tRemoving partition " << pID);
      std::string partition_obj_name =
          mrpt::format("partition_%lu", static_cast<unsigned long>(pID));

      CRenderizable::Ptr obj = map_partitions_obj->getByName(partition_obj_name);
      if (!obj)
      {
        THROW_EXCEPTION_FMT("Partition: %s was not found", partition_obj_name.c_str());
      }
      map_partitions_obj->removeObject(obj);
    }
  }
  MRPT_LOG_DEBUG_STREAM("Done working on the partitions visualization.");

  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();
}  // end of updateMapPartitionsVisualization

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::toggleMapPartitionsVisualization()
{
  MRPT_START
  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
  using namespace mrpt::opengl;

  MRPT_LOG_INFO("Toggling map partitions  visualization...");
  mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();

  if (m_lc_params.visualize_map_partitions)
  {
    mrpt::opengl::CRenderizable::Ptr obj = scene->getByName("map_partitions");
    obj->setVisibility(!obj->isVisible());
  }
  else
  {
    this->dumpVisibilityErrorMsg("visualize_map_partitions");
  }

  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();

  MRPT_END
}  // end of toggleMapPartitionsVisualization

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::computeCentroidOfNodesVector(
    const std::vector<uint32_t>& nodes_list, std::pair<double, double>* centroid_coords) const
{
  MRPT_START

  // get the poses and find the centroid so that we can place the baloon over
  // and at their center
  double centroid_x = 0;
  double centroid_y = 0;
  for (unsigned int node_it : nodes_list)
  {
    pose_t curr_node_pos = this->m_graph->nodes.at(node_it);
    centroid_x += curr_node_pos.x();
    centroid_y += curr_node_pos.y();
  }

  // normalize by the size - assign to the given pair
  centroid_coords->first = centroid_x / static_cast<double>(nodes_list.size());
  centroid_coords->second = centroid_y / static_cast<double>(nodes_list.size());

  MRPT_END
}  // end of computeCentroidOfNodesVector

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::initLaserScansVisualization()
{
  MRPT_START

  // laser scan visualization
  if (m_laser_params.visualize_laser_scans)
  {
    mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();

    mrpt::opengl::CPlanarLaserScan::Ptr laser_scan_viz = mrpt::opengl::CPlanarLaserScan::Create();
    laser_scan_viz->enablePoints(true);
    laser_scan_viz->enableLine(true);
    laser_scan_viz->enableSurface(true);
    laser_scan_viz->setSurfaceColor(
        m_laser_params.laser_scans_color.R, m_laser_params.laser_scans_color.G,
        m_laser_params.laser_scans_color.B, m_laser_params.laser_scans_color.A);

    laser_scan_viz->setName("laser_scan_viz");

    scene->insert(laser_scan_viz);
    this->m_win->unlockAccess3DScene();
    this->m_win->forceRepaint();
  }

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::updateLaserScansVisualization()
{
  MRPT_START

  // update laser scan visual
  if (m_laser_params.visualize_laser_scans && m_last_laser_scan2D)
  {
    mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();

    mrpt::opengl::CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
    mrpt::opengl::CPlanarLaserScan::Ptr laser_scan_viz =
        std::dynamic_pointer_cast<mrpt::opengl::CPlanarLaserScan>(obj);
    laser_scan_viz->setScan(*m_last_laser_scan2D);

    // set the pose of the laser scan
    const auto search = this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
    if (search != this->m_graph->nodes.end())
    {
      auto p = mrpt::math::TPose3D(search->second.asTPose());
      // put the laser scan underneath the graph, so that you can still
      // visualize the loop closures with the nodes ahead
      p.z = -0.15;
      laser_scan_viz->setPose(p);
    }

    this->m_win->unlockAccess3DScene();
    this->m_win->forceRepaint();
  }

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::toggleLaserScansVisualization()
{
  MRPT_START
  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");

  MRPT_LOG_INFO("Toggling LaserScans visualization...");

  mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();

  if (m_laser_params.visualize_laser_scans)
  {
    mrpt::opengl::CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
    obj->setVisibility(!obj->isVisible());
  }
  else
  {
    this->dumpVisibilityErrorMsg("visualize_laser_scans");
  }

  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::getEdgesStats(std::map<std::string, int>* edge_types_to_num) const
{
  MRPT_START
  *edge_types_to_num = m_edge_types_to_nums;
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::initializeVisuals()
{
  MRPT_START
  parent_t::initializeVisuals();
  // MRPT_LOG_DEBUG_STREAM("Initializing visuals");
  this->m_time_logger.enter("Visuals");

  ASSERTDEBMSG_(m_laser_params.has_read_config, "Configuration parameters aren't loaded yet");
  if (m_laser_params.visualize_laser_scans)
  {
    this->initLaserScansVisualization();
  }
  if (m_lc_params.visualize_map_partitions)
  {
    this->initMapPartitionsVisualization();
  }

  if (m_visualize_curr_node_covariance)
  {
    this->initCurrCovarianceVisualization();
  }

  this->m_time_logger.leave("Visuals");
  MRPT_END
}
template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::updateVisuals()
{
  MRPT_START
  parent_t::updateVisuals();
  // MRPT_LOG_DEBUG_STREAM("Updating visuals");
  this->m_time_logger.enter("Visuals");

  if (m_laser_params.visualize_laser_scans)
  {
    this->updateLaserScansVisualization();
  }
  if (m_lc_params.visualize_map_partitions)
  {
    this->updateMapPartitionsVisualization();
  }
  if (m_visualize_curr_node_covariance)
  {
    this->updateCurrCovarianceVisualization();
  }

  this->m_time_logger.leave("Visuals");
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::initCurrCovarianceVisualization()
{
  MRPT_START
  using namespace std;
  using namespace mrpt::opengl;

  // text message for covariance ellipsis
  this->m_win_manager->assignTextMessageParameters(
      &m_offset_y_curr_node_covariance, &m_text_index_curr_node_covariance);

  std::string title("Position uncertainty");
  this->m_win_manager->addTextMessage(
      5, -m_offset_y_curr_node_covariance, title, mrpt::img::TColorf(m_curr_node_covariance_color),
      m_text_index_curr_node_covariance);

  // covariance ellipsis
  CEllipsoid3D::Ptr cov_ellipsis_obj = std::make_shared<CEllipsoid3D>();
  cov_ellipsis_obj->setName("cov_ellipsis_obj");
  cov_ellipsis_obj->setColor_u8(m_curr_node_covariance_color);
  cov_ellipsis_obj->setLocation(0, 0, 0);
  // cov_ellipsis_obj->setQuantiles(2.0);

  mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();
  scene->insert(cov_ellipsis_obj);
  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::updateCurrCovarianceVisualization()
{
  MRPT_START
  using namespace std;
  using namespace mrpt::math;
  using namespace mrpt::opengl;
  using namespace mrpt::gui;

  // get the optimal path to the current node
  mrpt::graphs::TNodeID curr_node = this->m_graph->nodeCount() - 1;
  path_t* path = queryOptimalPath(curr_node);
  if (!path) return;

  CMatrixDouble33 mat;
  path->curr_pose_pdf.getCovariance(mat);
  pose_t curr_position = this->m_graph->nodes.at(curr_node);

  MRPT_LOG_DEBUG_STREAM(
      "In updateCurrCovarianceVisualization\n"
      "Covariance matrix:\n"
      << mat
      << "\n"
         "determinant : "
      << mat.det());

  mrpt::opengl::Scene::Ptr scene = this->m_win->get3DSceneAndLock();
  CRenderizable::Ptr obj = scene->getByName("cov_ellipsis_obj");
  CEllipsoid3D::Ptr cov_ellipsis_obj = std::dynamic_pointer_cast<CEllipsoid3D>(obj);

  // set the pose and corresponding covariance matrix of the ellipsis
  cov_ellipsis_obj->setLocation(curr_position.x(), curr_position.y(), 0);
  // pose_t loc = path->curr_pose_pdf.getMeanVal();
  // cov_ellipsis_obj->setLocation(loc.x(), loc.y(), 0);
  cov_ellipsis_obj->setCovMatrix(mat);

  this->m_win->unlockAccess3DScene();
  this->m_win->forceRepaint();

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time)
{
  MRPT_START

  this->logFmt(
      mrpt::system::LVL_ERROR,
      "Cannot toggle visibility of specified object.\n "
      "Make sure that the corresponding visualization flag ( %s "
      ") is set to true in the .ini file.\n",
      viz_flag.c_str());
  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));

  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::loadParams(const std::string& source_fname)
{
  MRPT_START
  parent_t::loadParams(source_fname);

  m_partitioner.options.loadFromConfigFileName(source_fname, "EdgeRegistrationDeciderParameters");
  m_laser_params.loadFromConfigFileName(source_fname, "EdgeRegistrationDeciderParameters");
  m_lc_params.loadFromConfigFileName(source_fname, "EdgeRegistrationDeciderParameters");

  mrpt::config::CConfigFile source(source_fname);

  m_consec_icp_constraint_factor = source.read_double(
      "EdgeRegistrationDeciderParameters", "consec_icp_constraint_factor", 0.90, false);
  m_lc_icp_constraint_factor = source.read_double(
      "EdgeRegistrationDeciderParameters", "lc_icp_constraint_factor", 0.70, false);

  // set the logging level if given by the user
  int min_verbosity_level =
      source.read_int("EdgeRegistrationDeciderParameters", "class_verbosity", 1, false);

  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
  MRPT_END
}
template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::printParams() const
{
  MRPT_START
  using namespace std;

  cout << "------------------[Pair-wise Consistency of ICP Edges - "
          "Registration Procedure Summary]------------------"
       << endl;

  parent_t::printParams();
  m_partitioner.options.dumpToConsole();
  m_laser_params.dumpToConsole();
  m_lc_params.dumpToConsole();

  cout << "Scan-matching ICP Constraint factor: " << m_consec_icp_constraint_factor << endl;
  cout << "Loop-closure ICP Constraint factor:  " << m_lc_icp_constraint_factor << endl;

  MRPT_LOG_DEBUG_STREAM("Printed the relevant parameters");
  MRPT_END
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::getDescriptiveReport(std::string* report_str) const
{
  MRPT_START

  const static std::string report_sep(2, '\n');
  const static std::string header_sep(80, '#');

  // Report on graph
  std::stringstream class_props_ss;
  class_props_ss << "Pair-wise Consistency of ICP Edges - Registration "
                    "Procedure Summary: "
                 << std::endl;
  class_props_ss << header_sep << std::endl;

  // time and output logging
  const std::string time_res = this->m_time_logger.getStatsAsText();
  const std::string output_res = this->getLogAsString();

  // merge the individual reports
  report_str->clear();
  parent_t::getDescriptiveReport(report_str);

  *report_str += class_props_ss.str();
  *report_str += report_sep;

  *report_str += time_res;
  *report_str += report_sep;

  *report_str += output_res;
  *report_str += report_sep;

  MRPT_END
}  // end of getDescriptiveReport

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::getCurrentPartitions(partitions_t& partitions_out) const
{
  partitions_out = this->getCurrentPartitions();
}

template <class GRAPH_T>
const std::vector<std::vector<uint32_t>>& CLoopCloserERD<GRAPH_T>::getCurrentPartitions() const
{
  return m_curr_partitions;
}

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::updateMapPartitions(bool full_update, bool is_first_time_node_reg)
{
  MRPT_START
  using namespace mrpt::math;
  using namespace std;
  this->m_time_logger.enter("updateMapPartitions");

  // Initialize the nodeIDs => LaserScans map
  nodes_to_scans2D_t nodes_to_scans;
  if (full_update)
  {
    MRPT_LOG_INFO("updateMapPartitions: Full partitioning of map was issued");
    // clear the existing partitions and recompute the partitioned map for
    // all the nodes
    m_partitioner.clear();
    nodes_to_scans = this->m_nodes_to_laser_scans2D;
  }
  else
  {
    // if registering measurement for root node as well...
    if (is_first_time_node_reg)
    {
      nodes_to_scans.insert(
          make_pair(this->m_graph->root, this->m_nodes_to_laser_scans2D.at(this->m_graph->root)));
    }

    // just use the last node-laser scan pair
    nodes_to_scans.insert(make_pair(
        this->m_graph->nodeCount() - 1,
        this->m_nodes_to_laser_scans2D.at(this->m_graph->nodeCount() - 1)));
  }

  // TODO - Should always exist.
  // for each one of the above nodes - add its position and correspoding
  // laserScan to the partitioner object
  for (auto it = nodes_to_scans.begin(); it != nodes_to_scans.end(); ++it)
  {
    if (!it->second)
    {  // if laserScan invalid go to next...
      MRPT_LOG_WARN_STREAM("nodeID \"" << it->first << "\" has invalid laserScan");
      continue;
    }

    // find pose of node, if it exists...
    // TODO - investigate this case. Why should this be happening?
    const auto& search = this->m_graph->nodes.find(it->first);
    if (search == this->m_graph->nodes.end())
    {
      MRPT_LOG_WARN_STREAM("Couldn't find pose for nodeID " << it->first);
      continue;
    }

    // pose
    const auto curr_constraint = constraint_t(search->second);
    const auto pose3d(mrpt::poses::CPose3DPDF::createFrom2D(curr_constraint));

    // laser scan
    mrpt::obs::CSensoryFrame sf;
    sf.insert(it->second);

    m_partitioner.addMapFrame(sf, *pose3d);
  }

  // update the last partitions list
  size_t curr_size = m_curr_partitions.size();
  m_last_partitions.resize(curr_size);
  for (size_t i = 0; i < curr_size; i++)
  {
    m_last_partitions[i] = m_curr_partitions[i];
  }
  // update current partitions list
  m_partitioner.updatePartitions(m_curr_partitions);

  MRPT_LOG_DEBUG_STREAM("Updated map partitions successfully.");
  this->m_time_logger.leave("updateMapPartitions");
  MRPT_END
}  // end of updateMapPartitions

// TLaserParams
// //////////////////////////////////

template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::TLaserParams::TLaserParams()
{
  mahal_distance_ICP_odom_win.resizeWindow(200);  // use the last X mahalanobis distance values
  goodness_threshold_win.resizeWindow(200);       // use the last X ICP values
}

template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::TLaserParams::~TLaserParams() = default;

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::TLaserParams::dumpToTextStream(std::ostream& out) const
{
  MRPT_START

  out << "Use scan-matching constraints               = " << (use_scan_matching ? "TRUE" : "FALSE")
      << std::endl;
  out << "Num. of previous nodes to check ICP against =  " << prev_nodes_for_ICP << std::endl;
  out << "Visualize laser scans                       = "
      << (visualize_laser_scans ? "TRUE" : "FALSE") << std::endl;

  MRPT_END
}
template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::TLaserParams::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START

  use_scan_matching = source.read_bool(section, "use_scan_matching", true, false);
  prev_nodes_for_ICP = source.read_int(  // how many nodes to check ICP against
      section, "prev_nodes_for_ICP", 10, false);
  visualize_laser_scans =
      source.read_bool("VisualizationParameters", "visualize_laser_scans", true, false);

  has_read_config = true;
  MRPT_END
}
// TLoopClosureParams
// //////////////////////////////////

template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::TLoopClosureParams::TLoopClosureParams() :
    keystroke_map_partitions("b"),

    balloon_std_color(153, 0, 153),
    balloon_curr_color(62, 0, 80),
    connecting_lines_color(balloon_std_color)

{
}

template <class GRAPH_T>
CLoopCloserERD<GRAPH_T>::TLoopClosureParams::~TLoopClosureParams() = default;

template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::TLoopClosureParams::dumpToTextStream(std::ostream& out) const
{
  MRPT_START
  using namespace std;

  stringstream ss;
  ss << "Min. node difference for loop closure                 = " << LC_min_nodeid_diff << endl;
  ss << "Remote NodeIDs to consider the potential loop closure = " << LC_min_remote_nodes << endl;
  ss << "Min EigenValues ratio for accepting a hypotheses set  = " << LC_eigenvalues_ratio_thresh
     << endl;
  ss << "Check only current node's partition for loop closures = "
     << (LC_check_curr_partition_only ? "TRUE" : "FALSE") << endl;
  ss << "New registered nodes required for full partitioning   = " << full_partition_per_nodes
     << endl;
  ss << "Visualize map partitions                              = "
     << (visualize_map_partitions ? "TRUE" : "FALSE") << endl;

  out << mrpt::format("%s", ss.str().c_str());

  MRPT_END
}
template <class GRAPH_T>
void CLoopCloserERD<GRAPH_T>::TLoopClosureParams::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START
  LC_min_nodeid_diff = source.read_int("GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
  LC_min_remote_nodes = source.read_int(section, "LC_min_remote_nodes", 3, false);
  LC_eigenvalues_ratio_thresh =
      source.read_double(section, "LC_eigenvalues_ratio_thresh", 2, false);
  LC_check_curr_partition_only =
      source.read_bool(section, "LC_check_curr_partition_only", true, false);
  full_partition_per_nodes = source.read_int(section, "full_partition_per_nodes", 50, false);
  visualize_map_partitions =
      source.read_bool("VisualizationParameters", "visualize_map_partitions", true, false);

  has_read_config = true;
  MRPT_END
}
}  // namespace mrpt::graphslam::deciders
