/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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
