/* +------------------------------------------------------------------------+
   |                     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

/** \file The main class in this file is mrpt::poses::CNetworkOfPoses<>, a
   generic
       basic template for predefined 2D/3D graphs of pose contraints.
*/

#include <mrpt/containers/yaml.h>
#include <mrpt/graphs/CDirectedGraph.h>
#include <mrpt/graphs/CDirectedTree.h>
#include <mrpt/graphs/THypothesis.h>
#include <mrpt/graphs/TMRSlamNodeAnnotations.h>
#include <mrpt/graphs/TNodeAnnotations.h>
#include <mrpt/graphs/dijkstra.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/math/utils.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/os.h>

#include <iostream>
#include <iterator>
#include <memory>
#include <optional>
#include <type_traits>  // is_base_of()

namespace mrpt
{
namespace graphs
{
/** Internal functions for MRPT */
namespace detail
{
template <class GRAPH_T>
struct graph_ops;

// forward declaration of CVisualizer
template <
    class CPOSE,  // Type of edges
    class MAPS_IMPLEMENTATION,
    class NODE_ANNOTATIONS,
    class EDGE_ANNOTATIONS>
class CVisualizer;
// forward declaration of CMRVisualizer
template <
    class CPOSE,  // Type of edges
    class MAPS_IMPLEMENTATION,
    class NODE_ANNOTATIONS,
    class EDGE_ANNOTATIONS>
class CMRVisualizer;
}  // namespace detail

/** A directed graph of pose constraints, with edges being the relative poses between pairs of nodes
 *identified by their numeric IDs (of type mrpt::graphs::TNodeID). A link or edge between two nodes
 *"i" and "j", that is, the pose \f$ p_{ij} \f$, holds the relative position of "j" with respect to
 *"i". These poses are stored in the edges in the format specified by the template argument CPOSE.
 *Users should employ the following derived classes depending on the desired representation of
 *edges:
 *
 * - mrpt::graphs::CNetworkOfPoses2D: 2D edges as a simple CPose2D (x y phi)
 * - mrpt::graphs::CNetworkOfPoses3D    : 3D edges as a simple mrpt::poses::CPose3D (x y z yaw pitch
 *roll)
 * - mrpt::graphs::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with information matrix
 *(CPosePDFGaussianInf)
 * - mrpt::graphs::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with information matrix
 *(CPose3DPDFGaussianInf)
 * - mrpt::graphs::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with covariance matrix
 *(CPosePDFGaussian). It's more efficient to use the information matrix version instead!
 * - mrpt::graphs::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with covariance matrix
 *(CPose3DPDFGaussian). It's more efficient to use the information matrix version instead!
 *
 * Two main members store all the information in this class:
 * - \a edges  (in the base class mrpt::graphs::CDirectedGraph::edges): A
 *   map from pairs of node ID -> pose constraints.
 * - \a nodes : A map from node ID -> estimated pose of that node
 *    (actually, read below on the template argument MAPS_IMPLEMENTATION).
 *
 * Graphs can be loaded and saved to text file in the format used by TORO &
 * HoG-man (more on the format <a href="http://www.mrpt.org/Robotics_file_formats" >here</a>),
 * using \a loadFromTextFile and \a saveToTextFile.
 *
 * This class is the base for representing networks of poses, which are the  main data type of a
 * series of SLAM algorithms implemented in the library mrpt-slam, in the namespace mrpt::graphslam.
 *
 * The template arguments are:
 * - CPOSE: The type of the edges, which hold a relative pose (2D/3D, just a value or a Gaussian,
 *   etc.)
 * - MAPS_IMPLEMENTATION: Can be either mrpt::containers::map_traits_stdmap or
 *   mrpt::containers::map_traits_map_as_vector. Determines the type of the list of global poses
 *   (member \a nodes).
 *
 * \sa mrpt::graphslam
 * \ingroup mrpt_graphs_grp
 */
template <
    class CPOSE,                                                      // Type of edges
    class MAPS_IMPLEMENTATION = mrpt::containers::map_traits_stdmap,  // std::map<> vs std::vector<>
    class NODE_ANNOTATIONS = mrpt::graphs::detail::TNodeAnnotationsEmpty,
    class EDGE_ANNOTATIONS = mrpt::graphs::detail::edge_annotations_empty>
class CNetworkOfPoses : public mrpt::graphs::CDirectedGraph<CPOSE, EDGE_ANNOTATIONS>
{
 public:
  /** @name Typedef's
    @{ */
  /** The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */
  using BASE = mrpt::graphs::CDirectedGraph<CPOSE, EDGE_ANNOTATIONS>;
  /** My own type */
  using self_t = CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;

  /** The type of PDF poses in the contraints (edges) (=CPOSE template
   * argument) */
  using constraint_t = CPOSE;
  /** The extra annotations in nodes, apart from a \a constraint_no_pdf_t */
  using node_annotations_t = NODE_ANNOTATIONS;
  /** The extra annotations in edges, apart from a \a constraint_t */
  using edge_annotations_t = EDGE_ANNOTATIONS;

  /** The type of map's implementation (=MAPS_IMPLEMENTATION template
   * argument) */
  using maps_implementation_t = MAPS_IMPLEMENTATION;
  /** The type of edges or their means if they are PDFs (that is, a simple
   * "edge" value) */
  using constraint_no_pdf_t = typename CPOSE::type_value;

  constexpr static auto getClassName()
  {
    using namespace mrpt::typemeta;
    return literal("mrpt::graphs::CNetworkOfPoses<") + TTypeName<CPOSE>::get() + literal(",") +
           TTypeName<MAPS_IMPLEMENTATION>::get() + literal(",") +
           TTypeName<NODE_ANNOTATIONS>::get() + literal(",") + TTypeName<EDGE_ANNOTATIONS>::get() +
           literal(">");
  }

  /** The type of each global pose in \a nodes: an extension of the \a
   * constraint_no_pdf_t pose with any optional user-defined data
   */
  struct global_pose_t : public constraint_no_pdf_t, public NODE_ANNOTATIONS
  {
    using self_t =
        typename CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>::
            global_pose_t;

    constexpr static auto getClassName()
    {
      using namespace mrpt::typemeta;
      return literal("global_pose_t<") + TTypeName<constraint_no_pdf_t>::get() + literal(",") +
             TTypeName<NODE_ANNOTATIONS>::get() + literal(">");
    }

    /**\brief Potential class constructors
     */
    /**\{ */
    inline global_pose_t() = default;

    template <typename ARG1>
    inline global_pose_t(const ARG1& a1) : constraint_no_pdf_t(a1)
    {
    }
    template <typename ARG1, typename ARG2>
    inline global_pose_t(const ARG1& a1, const ARG2& a2) : constraint_no_pdf_t(a1, a2)
    {
    }
    /**\} */

    bool operator==(const global_pose_t& other) const
    {
      return (
          static_cast<const constraint_no_pdf_t>(*this) ==
              static_cast<const constraint_no_pdf_t>(other) &&
          this->equal(other));
    }
    bool operator!=(const global_pose_t& other) const { return (!(*this == other)); }

    inline friend std::ostream& operator<<(std::ostream& o, const self_t& global_pose)
    {
      o << global_pose.asString() << "| " << global_pose.retAnnotsAsString();
      return o;
    }
  };

  /** A map from pose IDs to their global coordinate estimates, with
   * uncertainty */
  using global_poses_pdf_t =
      typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, CPOSE>;

  /** A map from pose IDs to their global coordinate estimates, without
   * uncertainty (the "most-likely value") */
  using global_poses_t =
      typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, global_pose_t>;

  /** @} */

  /** @name Data members
    @{ */

  /** The nodes (vertices) of the graph, with their estimated "global"
   * (with respect to \a root) position, without an associated covariance.
   * \sa dijkstra_nodes_estimate
   */
  global_poses_t nodes;

  /** The ID of the node that is the origin of coordinates, used as
   * reference by all coordinates in \a nodes. By default, root is the ID
   * "0". */
  mrpt::graphs::TNodeID root{0};

  /** False (default) if an edge i->j stores the normal relative pose of j
   * as seen from i: \f$ \Delta_i^j = j \ominus i \f$ True if an edge i->j
   * stores the inverse relateive pose, that is, i as seen from j: \f$
   * \Delta_i^j = i \ominus j \f$
   */
  bool edges_store_inverse_poses{false};

  /** @} */

  /** @name I/O methods
    @{ */

  /** Saves to a text file in the format used by TORO, HoG-man, G2O.
   * See: https://www.mrpt.org/Graph-SLAM_maps
   * \sa saveToBinaryFile, loadFromTextFile, writeAsText
   * \exception On any error
   */
  void saveToTextFile(const std::string& fileName) const
  {
    detail::graph_ops<self_t>::save_graph_of_poses_to_text_file(this, fileName);
  }
  /** Writes as text in the format used by TORO, HoG-man, G2O.
   * See: https://www.mrpt.org/Graph-SLAM_maps
   * \sa saveToBinaryFile, loadFromTextFile, saveToTextFile, readAsText
   * \exception On any error
   */
  void writeAsText(std::ostream& o) const
  {
    detail::graph_ops<self_t>::save_graph_of_poses_to_ostream(this, o);
  }

  /** Loads from a text file in the format used by TORO & HoG-man (more on the
   * format <a href="http://www.mrpt.org/Robotics_file_formats" >here</a>)
   *   Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
   *   If an unknown entry is found, a warning is dumped to std::cerr (only
   * once for each unknown keyword).
   *   An exception will be raised if trying to load a 3D graph into a 2D
   * class (in the opposite case, missing 3D data will default to zero).
   *
   * \param[in] fileName The file to load.
   * \param[in] collapse_dup_edges If true, \a collapseDuplicatedEdges will be
   * called automatically after loading (note that this operation may take
   * significant time for very large graphs).
   *
   * \sa loadFromBinaryFile, saveToTextFile
   *
   * \exception On any error, as a malformed line or loading a 3D graph in
   * a 2D graph.
   */
  inline void loadFromTextFile(const std::string& fileName, bool collapse_dup_edges = true)
  {
    detail::graph_ops<self_t>::load_graph_of_poses_from_text_file(this, fileName);
    if (collapse_dup_edges)
    {
      [[maybe_unused]] auto n = this->collapseDuplicatedEdges();
    }
  }

  /** Reads as text in the format used by TORO, HoG-man, G2O.
   * See: https://www.mrpt.org/Graph-SLAM_maps
   * \sa saveToBinaryFile, loadFromTextFile, saveToTextFile
   * \exception On any error
   */
  void readAsText(std::istream& i)
  {
    detail::graph_ops<self_t>::load_graph_of_poses_from_text_stream(this, i);
  }

  /** @} */

  /** @name Utility methods
    @{ */
  /**\brief Return 3D Visual Representation of the edges and nodes in the
   * network of poses
   *
   * Method makes the call to the corresponding method of the CVisualizer
   * class instance.
   */
  inline void getAs3DObject(
      mrpt::opengl::CSetOfObjects::Ptr object, const mrpt::containers::yaml& viz_params) const
  {
    using visualizer_t = mrpt::graphs::detail::CVisualizer<
        CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
    using visualizer_multirobot_t = mrpt::graphs::detail::CMRVisualizer<
        CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;

    bool is_multirobot = false;
    std::unique_ptr<visualizer_t> viz;
    is_multirobot =
        (std::is_base_of_v<mrpt::graphs::detail::TMRSlamNodeAnnotations, global_pose_t>);
    if (is_multirobot)
    {
      viz.reset(new visualizer_multirobot_t(*this));
    }
    else
    {
      viz.reset(new visualizer_t(*this));
    }
    viz->getAs3DObject(object, viz_params);
  }

  /** Spanning tree computation of a simple estimation of the global
   * coordinates of each node just from the information in all edges,
   * sorted in a Dijkstra tree based on the current "root" node.
   *
   * \note The "global" coordinates are with respect to the node with the
   * ID specified in \a root.
   *
   * \note This method takes into account the
   * value of \a edges_store_inverse_poses
   *
   * \sa node, root
   */
  inline void dijkstra_nodes_estimate(
      std::optional<std::reference_wrapper<std::map<TNodeID, size_t>>> topological_distances =
          std::nullopt)
  {
    detail::graph_ops<self_t>::graph_of_poses_dijkstra_init(
        this, topological_distances ? &topological_distances.value().get() : nullptr);
  }

  /** Look for duplicated edges (even in opposite directions) between all
   * pairs of nodes and fuse them.  Upon return, only one edge remains
   * between each pair of nodes with the mean & covariance (or information
   * matrix) corresponding to the Bayesian fusion of all the Gaussians.
   *
   * \return Overall number of removed edges.
   */
  [[nodiscard]] size_t collapseDuplicatedEdges()
  {
    return detail::graph_ops<self_t>::graph_of_poses_collapse_dup_edges(this);
  }

  /** Returns the total chi-squared error of the graph. Shortcut for
   * getGlobalSquareError(false). */
  [[nodiscard]] double chi2() const { return getGlobalSquareError(false); }

  /** Evaluates the graph total square error (ignoreCovariances=true) or
   * chi2 (ignoreCovariances=false) from all the pose constraints (edges)
   * with respect to the global poses in \a nodes.
   * \sa getEdgeSquareError
   * \exception std::exception On global poses not in \a nodes
   */
  [[nodiscard]] double getGlobalSquareError(bool ignoreCovariances = true) const
  {
    double sqErr = 0;
    for (auto it = BASE::edges.begin(); it != BASE::edges.end(); ++it)
      sqErr += detail::graph_ops<self_t>::graph_edge_sqerror(this, it, ignoreCovariances);
    return sqErr;
  }

  /**\brief Find the edges between the nodes in the node_IDs set and fill
   * given graph pointer accordingly.
   *
   * \param[in] node_IDs Set of nodes, between which, edges should be found
   * and inserted in the given sub_graph pointer
   * \param[in] root_node_in Node ID to be used as the root node of
   * sub_graph. If this is not given, the lowest nodeID is to be used.
   * \param[out] CNetworkOfPoses pointer that is to be filled.
   *
   * \param[in] auto_expand_set If true and in case the node_IDs set
   * contains non-consecutive nodes the returned set is expanded with the
   * in-between nodes. This makes sure that the final graph is always
   * connected.
   * If auto_expand_set is false  but there exist
   * non-consecutive nodes, virtual edges are inserted in the parts that
   * the graph is not connected
   */
  void extractSubGraph(
      const std::set<TNodeID>& node_IDs,
      self_t* sub_graph,
      const TNodeID root_node_in = INVALID_NODEID,
      bool auto_expand_set = true) const
  {
    using namespace std;
    using namespace mrpt;
    using namespace mrpt::math;
    using namespace mrpt::graphs::detail;

    using dijkstra_t = CDijkstra<self_t, MAPS_IMPLEMENTATION>;

    // assert that the given pointers are valid
    ASSERTMSG_(
        sub_graph,
        "\nInvalid pointer to a CNetworkOfPoses instance is given. "
        "Exiting..\n");
    sub_graph->clear();

    // assert that the root_node actually exists in the given node_IDs set
    TNodeID root_node = root_node_in;
    if (root_node != INVALID_NODEID)
    {
      ASSERTMSG_(
          node_IDs.find(root_node) != node_IDs.end(),
          "\nRoot_node does not exist in the given node_IDs set. "
          "Exiting.\n");
    }

    // ask for at least 2 nodes
    ASSERTMSG_(
        node_IDs.size() >= 2, format(
                                  "Very few nodes [%lu] for which to extract a subgraph. "
                                  "Exiting\n",
                                  static_cast<unsigned long>(node_IDs.size())));

    // find out if querry contains non-consecutive nodes.
    // Assumption: Set elements are in standard, ascending order.
    bool is_fully_connected_graph = true;
    std::set<TNodeID> node_IDs_real;  // actual set of nodes to be used.
    if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size())
    {
      node_IDs_real = node_IDs;
    }
    else
    {  // contains non-consecutive nodes
      is_fully_connected_graph = false;

      if (auto_expand_set)
      {  // set auto-expansion
        for (TNodeID curr_node_ID = *node_IDs.begin(); curr_node_ID != *node_IDs.rbegin();
             ++curr_node_ID)
        {
          node_IDs_real.insert(curr_node_ID);
        }
      }
      else
      {  // virtual_edge_addition strategy
        node_IDs_real = node_IDs;
      }
    }

    // add all the nodes of the node_IDs_real set to sub_graph
    for (unsigned long node_IDs_it : node_IDs_real)
    {
      // assert that current node exists in *own* graph
      typename global_poses_t::const_iterator own_it;
      for (own_it = nodes.begin(); own_it != nodes.end(); ++own_it)
      {
        if (node_IDs_it == own_it->first)
        {
          break;  // I throw exception afterwards
        }
      }
      ASSERTMSG_(
          own_it != nodes.end(), format(
                                     "NodeID [%lu] can't be found in the initial graph.",
                                     static_cast<unsigned long>(node_IDs_it)));

      global_pose_t curr_node(nodes.at(node_IDs_it));
      sub_graph->nodes.insert(make_pair(node_IDs_it, curr_node));
    }
    // cout << "Extracting subgraph for nodeIDs: " <<
    // getSTLContainerAsString(node_IDs_real) << endl;

    // set the root of the extracted graph
    if (root_node == INVALID_NODEID)
    {
      // smallest nodeID by default: already ordered inside std::map
      root_node = sub_graph->nodes.begin()->first;
    }
    sub_graph->root = root_node;

    // find all edges (in the initial graph), that exist in the given set
    // of nodes; add them to the given graph
    sub_graph->clearEdges();
    for (const auto& e : BASE::edges)
    {
      const TNodeID& from = e.first.first;
      const TNodeID& to = e.first.second;
      const typename BASE::edge_t& curr_edge = e.second;

      // if both nodes exist in the given set, add the corresponding edge
      if (sub_graph->nodes.find(from) != sub_graph->nodes.end() &&
          sub_graph->nodes.find(to) != sub_graph->nodes.end())
      {
        sub_graph->insertEdge(from, to, curr_edge);
      }
    }

    if (!auto_expand_set && !is_fully_connected_graph)
    {
      // Addition of virtual edges between non-connected graph parts is
      // necessary

      // make sure that the root nodeID is connected to at least one node
      {
        std::set<TNodeID> root_neighbors;
        sub_graph->getNeighborsOf(sub_graph->root, root_neighbors);

        if (root_neighbors.empty())
        {
          // add an edge between the root and an adjacent nodeID
          typename global_poses_t::iterator root_it = sub_graph->nodes.find(sub_graph->root);
          ASSERT_(root_it != sub_graph->nodes.end());
          if ((*root_it == *sub_graph->nodes.rbegin()))
          {  // is the last nodeID
            // add with previous node
            TNodeID next_to_root = (--root_it)->first;
            self_t::addVirtualEdge(sub_graph, next_to_root, sub_graph->root);
            // cout << "next_to_root = " << next_to_root;
          }
          else
          {
            TNodeID next_to_root = (++root_it)->first;
            // cout << "next_to_root = " << next_to_root;
            self_t::addVirtualEdge(sub_graph, sub_graph->root, next_to_root);
          }
        }
      }

      // as long as the graph is unconnected (as indicated by Dijkstra)
      // add a virtual edge between
      bool dijkstra_runs_successfully = false;

      // loop until the graph is fully connected (i.e. I can reach every
      // node of the graph starting from its root)
      while (!dijkstra_runs_successfully)
      {
        try
        {
          dijkstra_t dijkstra(*sub_graph, sub_graph->root);
          dijkstra_runs_successfully = true;
        }
        catch (const mrpt::graphs::detail::NotConnectedGraph& ex)
        {
          dijkstra_runs_successfully = false;

          set<TNodeID> unconnected_nodeIDs;
          ex.getUnconnectedNodeIDs(&unconnected_nodeIDs);
          // cout << "Unconnected nodeIDs: " <<
          // mrpt::math::getSTLContainerAsString(unconnected_nodeIDs)
          // << endl;
          // mainland: set of nodes that the root nodeID is in
          // island: set of nodes that the Dijkstra graph traversal
          // can't
          // reach starting from the root.
          // [!] There may be multiple sets of these nodes

          // set::rend() is the element with the highest value
          // set::begin() is the element with the lowest value
          const TNodeID& island_highest = *unconnected_nodeIDs.rbegin();
          const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
          // cout << "island_highest: " << island_highest << endl;
          // cout << "island_lowest: " << island_lowest << endl;
          // cout << "root: " << sub_graph->root << endl;

          // find out which nodes are in the same partition with the
          // root
          // (i.e. mainland)
          std::set<TNodeID> mainland;
          // for all nodes in sub_graph
          for (typename global_poses_t::const_iterator n_it = sub_graph->nodes.begin();
               n_it != sub_graph->nodes.end(); ++n_it)
          {
            bool is_there = false;

            // for all unconnected nodes
            for (unsigned long unconnected_nodeID : unconnected_nodeIDs)
            {
              if (n_it->first == unconnected_nodeID)
              {
                is_there = true;
                break;
              }
            }

            if (!is_there)
            {
              mainland.insert(n_it->first);
            }
          }

          bool is_single_island =
              (island_highest - island_lowest + 1 == unconnected_nodeIDs.size());

          if (is_single_island)
          {  // single island
            // Possible scenarios:
            // | island                       |
            // | mainland                                   |
            // | <low nodeIDs>  island_highest|  --- <virtual_edge>
            // --->>  | mainland_lowest <high nodeIDs> ... root ...|
            // --- OR ---
            // | mainland                       |
            // | island                       |
            // | <low nodeIDs>  mainland_highest|  ---
            // <virtual_edge> --->>  | island_lowest <high nodeIDs>
            // |

            const std::set<TNodeID>& island = unconnected_nodeIDs;
            this->connectGraphPartitions(sub_graph, island, mainland);
          }
          else
          {  // multiple islands
            // add a virtual edge between the last group before the
            // mainland and the mainland

            // split the unconnected_nodeIDs to smaller groups of
            // nodes
            // we only care about the nodes that are prior to the
            // root
            std::vector<std::set<TNodeID>> vec_of_islands;
            std::set<TNodeID> curr_island;
            TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
            curr_island.insert(prev_nodeID);  // add the initial node;
            for (auto it = ++unconnected_nodeIDs.begin();
                 *it < sub_graph->root && it != unconnected_nodeIDs.end(); ++it)
            {
              if (!(absDiff(*it, prev_nodeID) == 1))
              {
                vec_of_islands.push_back(curr_island);
                curr_island.clear();
              }
              curr_island.insert(*it);

              // update the previous nodeID
              prev_nodeID = *it;
            }
            vec_of_islands.push_back(curr_island);

            // cout << "last_island: " <<
            // getSTLContainerAsString(vec_of_islands.back()) <<
            // endl;
            // cout << "mainland: " <<
            // getSTLContainerAsString(mainland) << endl;
            this->connectGraphPartitions(sub_graph, vec_of_islands.back(), mainland);
          }
        }
      }
    }

    // estimate the node positions according to the edges - root is (0, 0,
    // 0)
    // just execute dijkstra once for grabbing the updated node positions.
    sub_graph->dijkstra_nodes_estimate();

  }  // end of extractSubGraph

  /**\brief Integrate given graph into own graph using the list of provided
   * common THypotheses. Nodes of the other graph are renumbered upon
   * integration in own graph.
   *
   * \param[in] other Graph (of the same type) that is to be integrated with
   * own graph.
   * \param[in] Hypotheses that join own and other graph.
   * \param[in] hypots_from_other_to_self Specify the direction of the
   * THypothesis objects in the common_hypots. If true (default) they are
   * directed from other to own graph (other \rightarrow own),
   *
   * \param[out] old_to_new_nodeID_mappings_out Map from the old nodeIDs
   * that are in the given graph to the new nodeIDs that have been inserted
   * (by this method) in own graph.
   */
  inline void mergeGraph(
      const self_t& other,
      const typename std::vector<detail::THypothesis<self_t>>& common_hypots,
      const bool hypots_from_other_to_self = true,
      std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out = nullptr)
  {
    MRPT_START
    using namespace mrpt::graphs;
    using namespace mrpt::graphs::detail;
    using namespace std;

    using hypots_cit_t = typename vector<THypothesis<self_t>>::const_iterator;
    using nodes_cit_t = typename global_poses_t::const_iterator;

    const self_t& graph_from = (hypots_from_other_to_self ? other : *this);
    const self_t& graph_to = (hypots_from_other_to_self ? *this : other);

    // assert that both own and other graph have at least two nodes.
    ASSERT_(graph_from.nodes.size() >= 2);
    ASSERT_(graph_to.nodes.size() >= 2);

    // Assert that from-nodeIds, to-nodeIDs in common_hypots exist in own
    // and other graph respectively
    for (hypots_cit_t h_cit = common_hypots.begin(); h_cit != common_hypots.end(); ++h_cit)
    {
      ASSERTMSG_(
          graph_from.nodes.find(h_cit->from) != graph_from.nodes.end(),
          format("NodeID %lu is not found in (from) graph", h_cit->from));
      ASSERTMSG_(
          graph_to.nodes.find(h_cit->to) != graph_to.nodes.end(),
          format("NodeID %lu is not found in (to) graph", h_cit->to));
    }

    // find the max nodeID in existing graph
    TNodeID max_nodeID = 0;
    for (nodes_cit_t n_cit = this->nodes.begin(); n_cit != this->nodes.end(); ++n_cit)
    {
      if (n_cit->first > max_nodeID)
      {
        max_nodeID = n_cit->first;
      }
    }
    TNodeID renum_start = max_nodeID + 1;
    size_t renum_counter = 0;
    // cout << "renum_start: " << renum_start << endl;

    // Renumber nodeIDs of other graph so that they don't overlap with own
    // graph nodeIDs
    std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;

    // map of TNodeID->TNodeID correspondences to address to if the
    // old_to_new_nodeID_mappings_out is not given.
    // Handy for not having to allocate old_to_new_nodeID_mappings in the
    // heap
    std::map<TNodeID, TNodeID> mappings_tmp;

    // If given, use the old_to_new_nodeID_mappings map.
    if (old_to_new_nodeID_mappings_out)
    {
      old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
    }
    else
    {
      old_to_new_nodeID_mappings = &mappings_tmp;
    }
    old_to_new_nodeID_mappings->clear();

    // add all nodes of other graph - Take care of renumbering them
    // cout << "Adding nodes of other graph" << endl;
    // cout << "====================" << endl;
    for (nodes_cit_t n_cit = other.nodes.begin(); n_cit != other.nodes.end(); ++n_cit)
    {
      TNodeID new_nodeID = renum_start + renum_counter++;
      old_to_new_nodeID_mappings->insert(make_pair(n_cit->first, new_nodeID));
      this->nodes.insert(make_pair(new_nodeID, n_cit->second));

      // cout << "Adding nodeID: " << new_nodeID << endl;
    }

    // add common constraints
    // cout << "Adding common constraints" << endl;
    // cout << "====================" << endl;
    for (hypots_cit_t h_cit = common_hypots.begin(); h_cit != common_hypots.end(); ++h_cit)
    {
      TNodeID from, to;
      if (hypots_from_other_to_self)
      {
        from = old_to_new_nodeID_mappings->at(h_cit->from);
        to = h_cit->to;
      }
      else
      {
        from = h_cit->from;
        to = old_to_new_nodeID_mappings->at(h_cit->to);
      }
      this->insertEdge(from, to, h_cit->getEdge());
      // cout << from << " -> " << to << " => " << h_cit->getEdge() <<
      // endl;
    }

    // add all constraints of the other graph
    // cout << "Adding constraints of other graph" << endl;
    // cout << "====================" << endl;
    for (typename self_t::const_iterator g_cit = other.begin(); g_cit != other.end(); ++g_cit)
    {
      TNodeID new_from = old_to_new_nodeID_mappings->at(g_cit->first.first);
      TNodeID new_to = old_to_new_nodeID_mappings->at(g_cit->first.second);
      this->insertEdge(new_from, new_to, g_cit->second);

      // cout << "[" << new_from << "] -> [" << new_to << "]" << " => " <<
      // g_cit->second << endl;
    }

    // run Dijkstra to update the node positions
    this->dijkstra_nodes_estimate();

    MRPT_END
  }

  /**\brief Add an edge between the last node of the group with the lower
   * nodeIDs and the first node of the higher nodeIDs.
   *
   * Given groups of nodes should only contain consecutive nodeIDs and
   * there should be no overlapping between them
   *
   * \note It is assumed that the sets of nodes are \b already in ascending
   * order (default std::set behavior.
   */
  inline static void connectGraphPartitions(
      self_t* sub_graph, const std::set<TNodeID>& groupA, const std::set<TNodeID>& groupB)
  {
    using namespace mrpt::math;

    ASSERTMSG_(
        sub_graph,
        "\nInvalid pointer to a CNetworkOfPoses instance is given. "
        "Exiting..\n");
    ASSERTMSG_(!groupA.empty(), "\ngroupA is empty.");
    ASSERTMSG_(!groupB.empty(), "\ngroupB is empty.");

    // assertion - non-overlapping groups
    ASSERTMSG_(
        *groupA.rend() < *groupB.rbegin() || *groupA.rbegin() > *groupB.rend(),
        "Groups A, B contain overlapping nodeIDs");

    // decide what group contains the low/high nodeIDs
    // just compare any two nodes of the sets (they are non-overlapping
    const std::set<TNodeID>& low_nodeIDs = *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
    const std::set<TNodeID>& high_nodeIDs = *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;

    // add virtual edge
    const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
    const TNodeID& to_nodeID = *high_nodeIDs.begin();
    self_t::addVirtualEdge(sub_graph, from_nodeID, to_nodeID);
  }

  /** Computes the square error of one pose constraints (edge) with respect
   * to the global poses in \a nodes If \a ignoreCovariances is false, the
   * squared Mahalanobis distance will be computed instead of the straight
   * square error.
   *
   * \exception std::exception On global poses not in \a nodes
   */
  inline double getEdgeSquareError(
      const typename BASE::edges_map_t::const_iterator& itEdge, bool ignoreCovariances = true) const
  {
    return detail::graph_ops<self_t>::graph_edge_sqerror(this, itEdge, ignoreCovariances);
  }

  /** Computes the square error of one pose constraints (edge) with respect
   * to the global poses in \a nodes If \a ignoreCovariances is false, the
   * squared Mahalanobis distance will be computed instead of the straight
   * square error.
   *
   * \exception std::exception On edge not existing or global poses not in
   * \a nodes
   */
  double getEdgeSquareError(
      const mrpt::graphs::TNodeID from_id,
      const mrpt::graphs::TNodeID to_id,
      bool ignoreCovariances = true) const
  {
    const typename BASE::edges_map_t::const_iterator itEdge =
        BASE::edges.find(std::make_pair(from_id, to_id));
    ASSERTMSG_(
        itEdge != BASE::edges.end(),
        format(
            "Request for edge %u->%u that doesn't exist in graph.",
            static_cast<unsigned int>(from_id), static_cast<unsigned int>(to_id)));
    return getEdgeSquareError(itEdge, ignoreCovariances);
  }

  /** Empty all edges, nodes and set root to ID 0. */
  inline void clear()
  {
    BASE::edges.clear();
    nodes.clear();
    root = 0;
    edges_store_inverse_poses = false;
  }

  /** Return number of nodes in the list \a nodes of global coordinates
   * (may be different that all nodes appearing in edges)
   *
   * \sa mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges
   */
  inline size_t nodeCount() const { return nodes.size(); }
  /**  @} */

 private:
  /**\brief Add a virtual edge between two nodes in the given graph.
   *
   * Edge is called virtual as its value will be determined solely on the
   * pose difference of the given nodeIDs
   */
  inline static void addVirtualEdge(self_t* graph, const TNodeID& from, const TNodeID& to)
  {
    ASSERTMSG_(graph, "Invalid pointer to the graph instance was provided.");

    typename self_t::global_pose_t& p_from = graph->nodes.at(from);
    typename self_t::global_pose_t& p_to = graph->nodes.at(to);
    const typename BASE::edge_t& virt_edge(p_to - p_from);

    graph->insertEdge(from, to, virt_edge);
  }
};

/** Binary serialization (write) operator "stream << graph" */
template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out,
    const CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
{
  using graph_t = CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
  detail::graph_ops<graph_t>::save_graph_of_poses_to_binary_file(&obj, out);
  return out;
}

/** Binary serialization (read) operator "stream >> graph" */
template <class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in,
    CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
{
  using graph_t = CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
  detail::graph_ops<graph_t>::read_graph_of_poses_from_binary_file(&obj, in);
  return in;
}

/** \addtogroup mrpt_graphs_grp
 * \name Handy typedefs for CNetworkOfPoses commonly used types
 @{ */

/** The specialization of CNetworkOfPoses for poses of type CPose2D (not a
 * PDF!), also implementing serialization. */
using CNetworkOfPoses2D = CNetworkOfPoses<mrpt::poses::CPose2D>;
/** The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D
 * (not a PDF!), also implementing serialization. */
using CNetworkOfPoses3D = CNetworkOfPoses<mrpt::poses::CPose3D>;
/** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian,
 * also implementing serialization. */
using CNetworkOfPoses2DCov = CNetworkOfPoses<mrpt::poses::CPosePDFGaussian>;
/** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian,
 * also implementing serialization. */
using CNetworkOfPoses3DCov = CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussian>;
/** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf,
 * also implementing serialization. */
using CNetworkOfPoses2DInf = CNetworkOfPoses<mrpt::poses::CPosePDFGaussianInf>;
/** The specialization of CNetworkOfPoses for poses of type
 * CPose3DPDFGaussianInf, also implementing serialization. */
using CNetworkOfPoses3DInf = CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussianInf>;

/**\brief Specializations of CNetworkOfPoses for graphs whose nodes inherit from
 * TMRSlamNodeAnnotations struct */
/**\{ */
using CNetworkOfPoses2DInf_NA = CNetworkOfPoses<
    mrpt::poses::CPosePDFGaussianInf,
    mrpt::containers::map_traits_stdmap,
    mrpt::graphs::detail::TMRSlamNodeAnnotations>;
using CNetworkOfPoses3DInf_NA = CNetworkOfPoses<
    mrpt::poses::CPose3DPDFGaussianInf,
    mrpt::containers::map_traits_stdmap,
    mrpt::graphs::detail::TMRSlamNodeAnnotations>;
/**\} */

/** @} */  // end of grouping

}  // namespace graphs

// Specialization of TTypeName must occur in the same namespace:
// These ones are here instead of into mrpt-containers, to avoid
// the dependency mrpt-containers -> mrpt-typemeta
namespace typemeta
{
MRPT_DECLARE_TTYPENAME(mrpt::containers::map_traits_stdmap)
MRPT_DECLARE_TTYPENAME(mrpt::containers::map_traits_map_as_vector)
}  // namespace typemeta

}  // namespace mrpt

// Implementation of templates (in a separate file for clarity)
#include "CNetworkOfPoses_impl.h"

// Visualization related template classes
#include <mrpt/graphs/CMRVisualizer.h>
#include <mrpt/graphs/CVisualizer.h>
