/* +------------------------------------------------------------------------+
   |                     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/graphs/TNodeAnnotations.h>
#include <mrpt/graphs/dijkstra.h>
#include <mrpt/io/CTextFileLinesParser.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/math/TPose3DQuat.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/math/ops_matrices.h>  // multiply_*()
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussianInf.h>
#include <mrpt/system/string_utils.h>

namespace mrpt::graphs::detail
{
using namespace std;
using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::graphs;

template <class POSE_PDF>
struct TPosePDFHelper
{
  static inline void copyFrom2D(POSE_PDF& p, const CPosePDFGaussianInf& pdf) { p.copyFrom(pdf); }
  static inline void copyFrom3D(POSE_PDF& p, const CPose3DPDFGaussianInf& pdf) { p.copyFrom(pdf); }
};
template <>
struct TPosePDFHelper<CPose2D>
{
  static inline void copyFrom2D(CPose2D& p, const CPosePDFGaussianInf& pdf) { p = pdf.mean; }
  static inline void copyFrom3D(CPose2D& p, const CPose3DPDFGaussianInf& pdf)
  {
    p = CPose2D(pdf.mean);
  }
};
template <>
struct TPosePDFHelper<CPose3D>
{
  static inline void copyFrom2D(CPose3D& p, const CPosePDFGaussianInf& pdf)
  {
    p = CPose3D(pdf.mean);
  }
  static inline void copyFrom3D(CPose3D& p, const CPose3DPDFGaussianInf& pdf) { p = pdf.mean; }
};

/// a helper struct with static template functions \sa CNetworkOfPoses
template <class graph_t>
struct graph_ops
{
  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D& p, std::ostream& f)
  {
    //  VERTEX2 id x y phi
    f << "VERTEX_SE2 " << id << " " << p.x() << " " << p.y() << " " << p.phi();
  }
  static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D& p, std::ostream& f)
  {
    //  VERTEX3 id x y z roll pitch yaw
    // **CAUTION** In the TORO graph format angles are in the RPY order vs.
    // MRPT's YPR.
    f << "VERTEX3 " << id << " " << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " "
      << p.pitch() << " " << p.yaw();
  }

  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const CPosePDFGaussianInf& edge, std::ostream& f)
  {
    //  G2O: EDGE_SE2 from_id to_id Ax Ay Aphi i_xx i_xy i_xp i_yy i_yp
    //  inf_pp
    f << "EDGE_SE2 " << edgeIDs.first << " " << edgeIDs.second << " " << edge.mean.x() << " "
      << edge.mean.y() << " " << edge.mean.phi() << " " << edge.cov_inv(0, 0) << " "
      << edge.cov_inv(0, 1) << " " << edge.cov_inv(0, 2) << " " << edge.cov_inv(1, 1) << " "
      << edge.cov_inv(1, 2) << " " << edge.cov_inv(2, 2) << endl;
  }
  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const CPose3DPDFGaussianInf& edge, std::ostream& f)
  {
    //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 ..
    //  inf_16 inf_22 .. inf_66
    // **CAUTION** In the TORO graph format angles are in the RPY order vs.
    // MRPT's YPR.
    // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order,
    // but it seems from the data that this is the correct expected format.
    f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " " << edge.mean.x() << " "
      << edge.mean.y() << " " << edge.mean.z() << " " << edge.mean.roll() << " "
      << edge.mean.pitch() << " " << edge.mean.yaw() << " " << edge.cov_inv(0, 0) << " "
      << edge.cov_inv(0, 1) << " " << edge.cov_inv(0, 2) << " " << edge.cov_inv(0, 5) << " "
      << edge.cov_inv(0, 4) << " " << edge.cov_inv(0, 3) << " " << edge.cov_inv(1, 1) << " "
      << edge.cov_inv(1, 2) << " " << edge.cov_inv(1, 5) << " " << edge.cov_inv(1, 4) << " "
      << edge.cov_inv(1, 3) << " " << edge.cov_inv(2, 2) << " " << edge.cov_inv(2, 5) << " "
      << edge.cov_inv(2, 4) << " " << edge.cov_inv(2, 3) << " " << edge.cov_inv(5, 5) << " "
      << edge.cov_inv(5, 4) << " " << edge.cov_inv(5, 3) << " " << edge.cov_inv(4, 4) << " "
      << edge.cov_inv(4, 3) << " " << edge.cov_inv(3, 3) << endl;
  }
  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const CPosePDFGaussian& edge, std::ostream& f)
  {
    CPosePDFGaussianInf p;
    p.copyFrom(edge);
    write_EDGE_line(edgeIDs, p, f);
  }
  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const CPose3DPDFGaussian& edge, std::ostream& f)
  {
    CPose3DPDFGaussianInf p;
    p.copyFrom(edge);
    write_EDGE_line(edgeIDs, p, f);
  }
  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const mrpt::poses::CPose2D& edge, std::ostream& f)
  {
    CPosePDFGaussianInf p;
    p.mean = edge;
    p.cov_inv.setIdentity();
    write_EDGE_line(edgeIDs, p, f);
  }
  static void write_EDGE_line(
      const TPairNodeIDs& edgeIDs, const mrpt::poses::CPose3D& edge, std::ostream& f)
  {
    CPose3DPDFGaussianInf p;
    p.mean = edge;
    p.cov_inv.setIdentity();
    write_EDGE_line(edgeIDs, p, f);
  }

  static void save_graph_of_poses_to_ostream(const graph_t* g, std::ostream& f)
  {
    // 1st: Nodes
    for (const auto& n : g->nodes)
    {
      write_VERTEX_line(n.first, n.second, f);
      // Node annotations:
      const auto sAnnot = n.second.retAnnotsAsString();
      if (!sAnnot.empty()) f << " | " << sAnnot;
      f << endl;
      // Root?
      if (n.first == g->root) f << "FIX " << n.first << endl;
    }

    // 2nd: Edges:
    for (const auto& e : *g)
      if (e.first.first != e.first.second)  // ignore EQUIV edges
        write_EDGE_line(e.first, e.second, f);

  }  // end save_graph

  // =================================================================
  //                     save_graph_of_poses_to_text_file
  // =================================================================
  static void save_graph_of_poses_to_text_file(const graph_t* g, const std::string& fil)
  {
    std::ofstream f;
    f.open(fil.c_str());
    if (!f.is_open()) THROW_EXCEPTION_FMT("Error opening file '%s' for writing", fil.c_str());
    save_graph_of_poses_to_ostream(g, f);
  }

  // =================================================================
  //                     save_graph_of_poses_to_binary_file
  // =================================================================
  static void save_graph_of_poses_to_binary_file(
      const graph_t* g, mrpt::serialization::CArchive& out)
  {
    // Store class name:
    const std::string sClassName = mrpt::typemeta::TTypeName<graph_t>::get().c_str();
    out << sClassName;

    // Store serialization version & object data:
    const uint32_t version = 0;
    out << version;
    out << g->nodes << g->edges << g->root;
  }

  // =================================================================
  //                     read_graph_of_poses_from_binary_file
  // =================================================================
  static void read_graph_of_poses_from_binary_file(graph_t* g, mrpt::serialization::CArchive& in)
  {
    // Compare class name:
    const std::string sClassName = mrpt::typemeta::TTypeName<graph_t>::get().c_str();
    std::string sStoredClassName;
    in >> sStoredClassName;
    ASSERT_EQUAL_(sStoredClassName, sClassName);

    // Check serialization version:
    uint32_t stored_version;
    in >> stored_version;

    g->clear();
    switch (stored_version)
    {
      case 0:
        in >> g->nodes >> g->edges >> g->root;
        break;
      default:
        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(stored_version);
    }
  }

  // =================================================================
  //                     load_graph_of_poses_from_text_file
  // =================================================================
  static void load_graph_of_poses_from_text_stream(
      graph_t* g, std::istream& f, const std::string& fil = std::string("(none)"))
  {
    using mrpt::system::strCmpI;
    using namespace mrpt::math;

    using CPOSE = typename graph_t::constraint_t;

    set<string> alreadyWarnedUnknowns;  // for unknown line types, show a
    // warning to cerr just once.

    // First, empty the graph:
    g->clear();

    // Determine if it's a 2D or 3D graph, just to raise an error if loading
    // a 3D graph in a 2D one, since
    //  it would be an unintentional loss of information:
    const bool graph_is_3D = CPOSE::is_3D();

    mrpt::io::CTextFileLinesParser filParser(f);

    // -------------------------------------------
    // 1st PASS: Read EQUIV entries only
    //  since processing them AFTER loading the data
    //  is much much slower.
    // -------------------------------------------
    map<TNodeID, TNodeID> lstEquivs;  // List of EQUIV entries: NODEID ->
    // NEWNODEID. NEWNODEID will be always
    // the lowest ID number.

    // Read & process lines each at once until EOF:
    istringstream s;
    while (filParser.getNextLine(s))
    {
      const unsigned int lineNum = filParser.getCurrentLineNumber();
      const string lin = s.str();

      string key;
      if (!(s >> key) || key.empty())
        THROW_EXCEPTION(
            format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str()));

      if (mrpt::system::strCmpI(key, "EQUIV"))
      {
        // Process these ones at the end, for now store in a list:
        TNodeID id1, id2;
        if (!(s >> id1 >> id2))
          THROW_EXCEPTION(
              format("Line %u: Can't read id1 & id2 in EQUIV line: '%s'", lineNum, lin.c_str()));
        lstEquivs[std::max(id1, id2)] = std::min(id1, id2);
      }
    }  // end 1st pass

    // -------------------------------------------
    // 2nd PASS: Read all other entries
    // -------------------------------------------
    filParser.rewind();

    // Read & process lines each at once until EOF:
    while (filParser.getNextLine(s))
    {
      const unsigned int lineNum = filParser.getCurrentLineNumber();
      const string lin = s.str();

      // Recognized strings:
      //  VERTEX2 id x y phi
      //   =(VERTEX_SE2)
      //  EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp
      //  inf_xp inf_yp
      //   =(EDGE or EDGE_SE2 or ODOMETRY)
      //  VERTEX3 id x y z roll pitch yaw
      //  VERTEX_SE3:QUAT id x y z qx qy qz qw
      //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 ..
      //  inf_16 inf_22 .. inf_66
      //  EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11 inf_12
      //  .. inf_16 inf_22 .. inf_66
      //  EQUIV id1 id2
      string key;
      if (!(s >> key) || key.empty())
        THROW_EXCEPTION(
            format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str()));

      if (strCmpI(key, "VERTEX2") || strCmpI(key, "VERTEX") || strCmpI(key, "VERTEX_SE2"))
      {
        TNodeID id;
        TPose2D p2D;
        if (!(s >> id >> p2D.x >> p2D.y >> p2D.phi))
          THROW_EXCEPTION(
              format("Line %u: Error parsing VERTEX2 line: '%s'", lineNum, lin.c_str()));

        // Make sure the node is new:
        if (g->nodes.find(id) != g->nodes.end())
          THROW_EXCEPTION(format(
              "Line %u: Error, duplicated verted ID %u in line: "
              "'%s'",
              lineNum, static_cast<unsigned int>(id), lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(id);
          if (itEq != lstEquivs.end()) id = itEq->second;
        }

        // Add to map: ID -> absolute pose:
        if (g->nodes.find(id) == g->nodes.end())
        {
          using pose_t = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value;
          pose_t& newNode = g->nodes[id];
          newNode = pose_t(CPose2D(p2D));  // Convert to mrpt::poses::CPose3D if needed
        }
      }
      else if (strCmpI(key, "VERTEX3"))
      {
        if (!graph_is_3D)
          THROW_EXCEPTION(format(
              "Line %u: Try to load VERTEX3 into a 2D graph: "
              "'%s'",
              lineNum, lin.c_str()));

        //  VERTEX3 id x y z roll pitch yaw
        TNodeID id;
        TPose3D p3D;
        // **CAUTION** In the TORO graph format angles are in the RPY
        // order vs. MRPT's YPR.
        if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >> p3D.pitch >> p3D.yaw))
          THROW_EXCEPTION(
              format("Line %u: Error parsing VERTEX3 line: '%s'", lineNum, lin.c_str()));

        // Make sure the node is new:
        if (g->nodes.find(id) != g->nodes.end())
          THROW_EXCEPTION(format(
              "Line %u: Error, duplicated verted ID %u in line: "
              "'%s'",
              lineNum, static_cast<unsigned int>(id), lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(id);
          if (itEq != lstEquivs.end()) id = itEq->second;
        }

        // Add to map: ID -> absolute pose:
        if (g->nodes.find(id) == g->nodes.end())
        {
          g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value(
              CPose3D(p3D));  // Auto converted to CPose2D if needed
        }
      }
      else if (strCmpI(key, "VERTEX_SE3:QUAT"))
      {
        if (!graph_is_3D)
          THROW_EXCEPTION(format(
              "Line %u: Try to load VERTEX_SE3:QUAT into a 2D "
              "graph: '%s'",
              lineNum, lin.c_str()));

        // VERTEX_SE3:QUAT id x y z qx qy qz qw
        TNodeID id;
        TPose3DQuat p3D;
        if (!(s >> id >> p3D.x >> p3D.y >> p3D.z >> p3D.qx >> p3D.qy >> p3D.qz >> p3D.qr))
          THROW_EXCEPTION(
              format("Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'", lineNum, lin.c_str()));

        // Make sure the node is new:
        if (g->nodes.find(id) != g->nodes.end())
          THROW_EXCEPTION(format(
              "Line %u: Error, duplicated verted ID %u in line: "
              "'%s'",
              lineNum, static_cast<unsigned int>(id), lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(id);
          if (itEq != lstEquivs.end()) id = itEq->second;
        }

        // Add to map: ID -> absolute pose:
        if (g->nodes.find(id) == g->nodes.end())
        {
          g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value(
              CPose3D(CPose3DQuat(p3D)));  // Auto converted to
                                           // CPose2D if needed
        }
      }
      else if (
          strCmpI(key, "EDGE2") || strCmpI(key, "EDGE") || strCmpI(key, "ODOMETRY") ||
          strCmpI(key, "EDGE_SE2"))
      {
        //  EDGE_SE2 from_id to_id Ax Ay Aphi inf_xx i_xy i_xp i_yy i_yp
        //  i_pp Read values are:
        //    [ s00 s01 s02 ]
        //    [  -  s11 s12 ]
        //    [  -   -  s22 ]
        //
        TNodeID to_id, from_id;
        if (!(s >> from_id >> to_id))
          THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(to_id);
          if (itEq != lstEquivs.end()) to_id = itEq->second;
        }
        {
          const auto itEq = lstEquivs.find(from_id);
          if (itEq != lstEquivs.end()) from_id = itEq->second;
        }

        if (from_id != to_id)  // Don't load self-edges! (probably come
        // from an EQUIV)
        {
          TPose2D Ap_mean;
          mrpt::math::CMatrixDouble33 Ap_cov_inv;
          if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >> Ap_cov_inv(0, 0) >>
                Ap_cov_inv(0, 1) >> Ap_cov_inv(0, 2) >> Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
                Ap_cov_inv(2, 2)))
            THROW_EXCEPTION(
                format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str()));

          // Complete low triangular part of inf matrix:
          Ap_cov_inv(1, 0) = Ap_cov_inv(0, 1);
          Ap_cov_inv(2, 0) = Ap_cov_inv(0, 2);
          Ap_cov_inv(2, 1) = Ap_cov_inv(1, 2);

          // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
          typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
          TPosePDFHelper<CPOSE>::copyFrom2D(
              newEdge, CPosePDFGaussianInf(CPose2D(Ap_mean), Ap_cov_inv));
          g->insertEdge(from_id, to_id, newEdge);
        }
      }
      else if (strCmpI(key, "EDGE3"))
      {
        if (!graph_is_3D)
          THROW_EXCEPTION(
              format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str()));

        //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
        //  .. inf_16 inf_22 .. inf_66
        TNodeID to_id, from_id;
        if (!(s >> from_id >> to_id))
          THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(to_id);
          if (itEq != lstEquivs.end()) to_id = itEq->second;
        }
        {
          const auto itEq = lstEquivs.find(from_id);
          if (itEq != lstEquivs.end()) from_id = itEq->second;
        }

        if (from_id != to_id)  // Don't load self-edges! (probably come
        // from an EQUIV)
        {
          TPose3D Ap_mean;
          mrpt::math::CMatrixDouble66 Ap_cov_inv;
          // **CAUTION** In the TORO graph format angles are in the
          // RPY order vs. MRPT's YPR.
          if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.roll >> Ap_mean.pitch >>
                Ap_mean.yaw))
            THROW_EXCEPTION(
                format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str()));

          // **CAUTION** Indices are shuffled to the change YAW(3) <->
          // ROLL(5) in the order of the data.
          if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >> Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
                Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >> Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
                Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >> Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
                Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >> Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
                Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >> Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
                Ap_cov_inv(3, 3)))
          {
            // Cov may be omitted in the file:
            Ap_cov_inv.setIdentity();

            if (alreadyWarnedUnknowns.find("MISSING_3D") == alreadyWarnedUnknowns.end())
            {
              alreadyWarnedUnknowns.insert("MISSING_3D");
              cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum
                   << ": Warning: Information matrix missing, "
                      "assuming unity.\n";
            }
          }
          else
          {
            // Complete low triangular part of inf matrix:
            for (size_t r = 1; r < 6; r++)
              for (size_t c = 0; c < r; c++) Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
          }

          // Convert as needed:
          typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
          TPosePDFHelper<CPOSE>::copyFrom3D(
              newEdge, CPose3DPDFGaussianInf(CPose3D(Ap_mean), Ap_cov_inv));
          g->insertEdge(from_id, to_id, newEdge);
        }
      }
      else if (strCmpI(key, "EDGE_SE3:QUAT"))
      {
        if (!graph_is_3D)
          THROW_EXCEPTION(
              format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str()));

        //  EDGE_SE3:QUAT from_id to_id Ax Ay Az qx qy qz qw inf_11
        //  inf_12 .. inf_16 inf_22 .. inf_66
        //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12
        //  .. inf_16 inf_22 .. inf_66
        TNodeID to_id, from_id;
        if (!(s >> from_id >> to_id))
          THROW_EXCEPTION(
              format("Line %u: Error parsing EDGE_SE3:QUAT line: '%s'", lineNum, lin.c_str()));

        // EQUIV? Replace ID by new one.
        {
          const auto itEq = lstEquivs.find(to_id);
          if (itEq != lstEquivs.end()) to_id = itEq->second;
        }
        {
          const auto itEq = lstEquivs.find(from_id);
          if (itEq != lstEquivs.end()) from_id = itEq->second;
        }

        if (from_id != to_id)  // Don't load self-edges! (probably come
        // from an EQUIV)
        {
          TPose3DQuat Ap_mean;
          mrpt::math::CMatrixDouble66 Ap_cov_inv;
          if (!(s >> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.qx >> Ap_mean.qy >>
                Ap_mean.qz >> Ap_mean.qr))
            THROW_EXCEPTION(format(
                "Line %u: Error parsing EDGE_SE3:QUAT line: "
                "'%s'",
                lineNum, lin.c_str()));

          // **CAUTION** Indices are shuffled to the change YAW(3) <->
          // ROLL(5) in the order of the data.
          if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >> Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
                Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >> Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
                Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >> Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
                Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >> Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
                Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >> Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
                Ap_cov_inv(3, 3)))
          {
            // Cov may be omitted in the file:
            Ap_cov_inv.setIdentity();

            if (alreadyWarnedUnknowns.find("MISSING_3D") == alreadyWarnedUnknowns.end())
            {
              alreadyWarnedUnknowns.insert("MISSING_3D");
              cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum
                   << ": Warning: Information matrix missing, "
                      "assuming unity.\n";
            }
          }
          else
          {
            // Complete low triangular part of inf matrix:
            for (size_t r = 1; r < 6; r++)
              for (size_t c = 0; c < r; c++) Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
          }

          // Convert as needed:
          typename CNetworkOfPoses<CPOSE>::edge_t newEdge;
          TPosePDFHelper<CPOSE>::copyFrom3D(
              newEdge, CPose3DPDFGaussianInf(CPose3D(CPose3DQuat(Ap_mean)), Ap_cov_inv));
          g->insertEdge(from_id, to_id, newEdge);
        }
      }
      else if (strCmpI(key, "EQUIV"))
      {
        // Already read in the 1st pass.
      }
      else if (strCmpI(key, "FIX"))
      {
        TNodeID id;
        if (!(s >> id))
          THROW_EXCEPTION(format("Line %u: Can't read id in FIX line: '%s'", lineNum, lin.c_str()));
        g->root = id;
      }
      else
      {  // Unknown entry: Warn the user just once:
        if (alreadyWarnedUnknowns.find(key) == alreadyWarnedUnknowns.end())
        {
          alreadyWarnedUnknowns.insert(key);
          cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum
               << ": Warning: unknown entry type: " << key << endl;
        }
      }
    }  // end while

  }  // end load_graph

  static void load_graph_of_poses_from_text_file(graph_t* g, const std::string& fil)
  {
    std::ifstream f(fil);
    if (!f.is_open()) THROW_EXCEPTION_FMT("Error opening file '%s' for reading", fil.c_str());
    load_graph_of_poses_from_text_stream(g, f, fil);
  }

  // --------------------------------------------------------------------------------
  //               Implements: collapseDuplicatedEdges
  //
  // 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.
  // --------------------------------------------------------------------------------
  static size_t graph_of_poses_collapse_dup_edges(graph_t* g)
  {
    MRPT_START
    using TEdgeIterator = typename graph_t::edges_map_t::iterator;

    // Data structure: (id1,id2) -> all edges between them
    //  (with id1 < id2)
    using TListAllEdges =
        map<pair<TNodeID, TNodeID>,
            vector<TEdgeIterator>>;  // For god's sake... when will ALL
    // compilers support
    // auto!! :'-(
    TListAllEdges lstAllEdges;

    // Clasify all edges to identify duplicated ones:
    for (auto itEd = g->edges.begin(); itEd != g->edges.end(); ++itEd)
    {
      // Build a pair <id1,id2> with id1 < id2:
      const pair<TNodeID, TNodeID> arc_id = make_pair(
          std::min(itEd->first.first, itEd->first.second),
          std::max(itEd->first.first, itEd->first.second));
      // get (or create the first time) the list of edges between them:
      vector<TEdgeIterator>& lstEdges = lstAllEdges[arc_id];
      // And add this one:
      lstEdges.push_back(itEd);
    }

    // Now, remove all but the first edge:
    size_t nRemoved = 0;
    for (auto it = lstAllEdges.begin(); it != lstAllEdges.end(); ++it)
    {
      const size_t N = it->second.size();
      for (size_t i = 1; i < N; i++)  // i=0 is NOT removed
        g->edges.erase(it->second[i]);

      if (N >= 2) nRemoved += N - 1;
    }

    return nRemoved;
    MRPT_END
  }  // end of graph_of_poses_collapse_dup_edges

  // --------------------------------------------------------------------------------
  //               Implements: dijkstra_nodes_estimate
  //
  //	Compute 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 that "global" coordinates are with respect to the node with the ID
  // specified in \a root.
  // --------------------------------------------------------------------------------
  static void graph_of_poses_dijkstra_init(
      graph_t* g, std::map<TNodeID, size_t>* topological_distances = nullptr)
  {
    MRPT_START
    using namespace std;

    // Do Dijkstra shortest path from "root" to all other nodes:
    using dijkstra_t = CDijkstra<graph_t, typename graph_t::maps_implementation_t>;
    using constraint_t = typename graph_t::constraint_t;

    // initialize corresponding dijkstra object from root.
    dijkstra_t dijkstra(*g, g->root);

    // Get the tree representation of the graph and traverse it
    //  from its root toward the leafs:
    typename dijkstra_t::tree_graph_t treeView;
    dijkstra.getTreeGraph(treeView);

    // This visitor class performs the real job of
    struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
    {
      graph_t* m_g;  // The original graph
      std::map<TNodeID, size_t>* m_topo_dists{nullptr};

      VisitorComputePoses(graph_t* g) : m_g(g) {}
      void OnVisitNode(
          const TNodeID parent_id,
          const typename dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo& edge_to_child,
          [[maybe_unused]] size_t depth_level) override
      {
        const TNodeID child_id = edge_to_child.id;

        // topological distance:
        if (m_topo_dists) (*m_topo_dists)[child_id] = depth_level;

        // Compute the pose of "child_id" as parent_pose (+)
        // edge_delta_pose,
        //  taking into account that that edge may be in reverse order
        //  and then have to invert the delta_pose:
        if ((!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
            (edge_to_child.reverse && m_g->edges_store_inverse_poses))
        {  // pose_child = p_parent (+)
           // p_delta
          m_g->nodes[child_id].composeFrom(
              m_g->nodes[parent_id], edge_to_child.data->getPoseMean());
        }
        else
        {  // pose_child = p_parent (+) [(-)p_delta]
          m_g->nodes[child_id].composeFrom(
              m_g->nodes[parent_id], -edge_to_child.data->getPoseMean());
        }
      }
    };

    // Remove all global poses except for the root node, which is the
    // origin:
    //
    // Keep track of the NODE_ANNOTATIONS for each node and put it after
    // the global pose computation
    bool empty_node_annots = g->nodes.begin()->second.is_node_annots_empty;
    map<const TNodeID, TNodeAnnotations*> nodeID_to_annots;
    if (!empty_node_annots)
    {
      for (auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end(); ++poses_cit)
      {
        nodeID_to_annots.insert(make_pair(poses_cit->first, poses_cit->second.getCopyOfAnnots()));
      }
    }

    g->nodes.clear();
    g->nodes[g->root] = typename constraint_t::type_value();  // Typ: CPose2D() or CPose3D()

    // Run the visit thru all nodes in the tree:
    VisitorComputePoses myVisitor(g);
    myVisitor.m_topo_dists = topological_distances;
    treeView.visitBreadthFirst(treeView.root, myVisitor);

    // The distance of "root" is zero (it's not visited in the loop above!):
    if (topological_distances) (*topological_distances)[g->root] = 0;

    // Fill the NODE_ANNOTATIONS part again
    if (!empty_node_annots)
    {
      for (auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end(); ++poses_cit)
      {
        TNodeAnnotations* node_annots = nodeID_to_annots.at(poses_cit->first);
        bool res = poses_cit->second.setAnnots(*node_annots);

        // free dynamically allocated mem
        delete node_annots;
        node_annots = nullptr;

        // make sure setting annotations was successful
        ASSERTMSG_(
            res, mrpt::format(
                     "Setting annotations for nodeID \"%lu\" was "
                     "unsuccessful",
                     static_cast<unsigned long>(poses_cit->first)));
      }
    }

    MRPT_END
  }  // end of graph_of_poses_dijkstra_init

  // Auxiliary funcs:
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const CPosePDFGaussianInf& p)
  {
    math::wrapToPiInPlace(err[2]);
    return mrpt::math::multiply_HtCH_scalar(err, p.cov_inv);
  }
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const CPose3DPDFGaussianInf& p)
  {
    math::wrapToPiInPlace(err[3]);
    math::wrapToPiInPlace(err[4]);
    math::wrapToPiInPlace(err[5]);
    return mrpt::math::multiply_HtCH_scalar(err, p.cov_inv);
  }
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const CPosePDFGaussian& p)
  {
    math::wrapToPiInPlace(err[2]);
    // err^t*cov_inv*err
    return mrpt::math::multiply_HCHt_scalar(err, p.cov.inverse_LLt());
  }
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const CPose3DPDFGaussian& p)
  {
    math::wrapToPiInPlace(err[3]);
    math::wrapToPiInPlace(err[4]);
    math::wrapToPiInPlace(err[5]);
    // err^t*cov_inv*err
    return mrpt::math::multiply_HtCH_scalar(err, p.cov.inverse_LLt());
  }
  // These two are for simulating maha2 distances for non-PDF types: fallback
  // to squared-norm:
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const mrpt::poses::CPose2D& p)
  {
    math::wrapToPiInPlace(err[2]);
    return square(err[0]) + square(err[1]) + square(err[2]);
  }
  template <class VEC>
  static inline double auxMaha2Dist(VEC& err, const mrpt::poses::CPose3D& p)
  {
    math::wrapToPiInPlace(err[3]);
    math::wrapToPiInPlace(err[4]);
    math::wrapToPiInPlace(err[5]);
    return square(err[0]) + square(err[1]) + square(err[2]) + square(err[3]) + square(err[4]) +
           square(err[5]);
  }

  static inline double auxEuclid2Dist(
      const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2)
  {
    return square(p1.x() - p2.x()) + square(p1.y() - p2.y()) +
           square(mrpt::math::wrapToPi(p1.phi() - p2.phi()));
  }
  static inline double auxEuclid2Dist(
      const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2)
  {
    return square(p1.x() - p2.x()) + square(p1.y() - p2.y()) + square(p1.z() - p2.z()) +
           square(mrpt::math::wrapToPi(p1.yaw() - p2.yaw())) +
           square(mrpt::math::wrapToPi(p1.pitch() - p2.pitch())) +
           square(mrpt::math::wrapToPi(p1.roll() - p2.roll()));
  }

  // --------------------------------------------------------------------------------
  //               Implements: detail::graph_edge_sqerror
  //
  //	Compute the square error of a single edge, in comparison to the nodes
  // global poses.
  // --------------------------------------------------------------------------------
  static double graph_edge_sqerror(
      const graph_t* g,
      const typename mrpt::graphs::CDirectedGraph<
          typename graph_t::constraint_t>::edges_map_t::const_iterator& itEdge,
      bool ignoreCovariances)
  {
    MRPT_START

    // Get node IDs:
    const TNodeID from_id = itEdge->first.first;
    const TNodeID to_id = itEdge->first.second;

    // And their global poses as stored in "nodes"
    auto itPoseFrom = g->nodes.find(from_id);
    auto itPoseTo = g->nodes.find(to_id);
    ASSERTMSG_(
        itPoseFrom != g->nodes.end(),
        format(
            "Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(from_id)));
    ASSERTMSG_(
        itPoseTo != g->nodes.end(),
        format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(to_id)));

    // The global poses:
    using constraint_t = typename graph_t::constraint_t;

    const typename constraint_t::type_value& from_mean = itPoseFrom->second;
    const typename constraint_t::type_value& to_mean = itPoseTo->second;

    // The delta_pose as stored in the edge:
    const constraint_t& edge_delta_pose = itEdge->second;
    const typename constraint_t::type_value& edge_delta_pose_mean = edge_delta_pose.getPoseMean();

    if (ignoreCovariances)
    {  // Square Euclidean distance: Just use the mean
       // values, ignore covs.
      // from_plus_delta = from_mean (+) edge_delta_pose_mean
      typename constraint_t::type_value from_plus_delta(UNINITIALIZED_POSE);
      from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);

      // (auxMaha2Dist will also take into account the 2PI wrapping)
      return auxEuclid2Dist(from_plus_delta, to_mean);
    }
    else
    {
      // Square Mahalanobis distance
      // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
      constraint_t from_plus_delta = edge_delta_pose;
      from_plus_delta.changeCoordinatesReference(from_mean);

      // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to
      // "to_mean":
      //  We want to compute the squared Mahalanobis distance:
      //       err^t * INV_COV * err
      //
      mrpt::math::CVectorFixedDouble<constraint_t::type_value::static_size> err;
      for (size_t i = 0; i < constraint_t::type_value::static_size; i++)
        err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];

      // (auxMaha2Dist will also take into account the 2PI wrapping)
      return auxMaha2Dist(err, from_plus_delta);
    }
    MRPT_END
  }

};  // end of graph_ops<graph_t>

}  // namespace mrpt::graphs::detail
