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

#include "hmtslam-precomp.h" // Precomp header

#include <mrpt/hmtslam/CRobotPosesGraph.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CText.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/opengl/CSimpleLine.h>
#include <mrpt/system/os.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/random.h>
#include <mrpt/graphslam.h>

using namespace std;
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::random;
using namespace mrpt::hmtslam;
using namespace mrpt::utils;
using namespace mrpt::system;
using namespace mrpt::poses;
using namespace mrpt::opengl;
using namespace mrpt::maps;
using namespace mrpt::math;


/*---------------------------------------------------------------
						nodeCount
  ---------------------------------------------------------------*/
size_t CHierarchicalMapMHPartition::nodeCount() const
{
	return m_nodes.size();
}

/*---------------------------------------------------------------
						arcCount
  ---------------------------------------------------------------*/
size_t CHierarchicalMapMHPartition::arcCount() const
{
	return m_arcs.size();
}


/*---------------------------------------------------------------
						getNodeByID
  ---------------------------------------------------------------*/
CHMHMapNodePtr  CHierarchicalMapMHPartition::getNodeByID(CHMHMapNode::TNodeID	id)
{
	MRPT_START
	if (id==AREAID_INVALID) return CHMHMapNodePtr();

	TNodeList::iterator it = m_nodes.find(id);
	return it==m_nodes.end() ?  CHMHMapNodePtr() : it->second;

	MRPT_END
}
/*---------------------------------------------------------------
						getNodeByID
  ---------------------------------------------------------------*/
const CHMHMapNodePtr  CHierarchicalMapMHPartition::getNodeByID(CHMHMapNode::TNodeID	id) const
{
	MRPT_START
	if (id==AREAID_INVALID) return CHMHMapNodePtr();

	TNodeList::const_iterator it = m_nodes.find(id);
	return it==m_nodes.end() ?  CHMHMapNodePtr() : it->second;

	MRPT_END
}

/*---------------------------------------------------------------
						getNodeByLabel
  ---------------------------------------------------------------*/
CHMHMapNodePtr  CHierarchicalMapMHPartition::getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID)
{
	MRPT_START

	// Look for the ID:
	for (TNodeList::const_iterator it=m_nodes.begin();it!=m_nodes.end();++it)
		if ( it->second->m_hypotheses.has(hypothesisID) )
			if ( !os::_strcmpi( it->second->m_label.c_str(), label.c_str() ) )
				return it->second;

	// Not found:
	return CHMHMapNodePtr();

	MRPT_END
}
/*---------------------------------------------------------------
						getNodeByLabel
  ---------------------------------------------------------------*/
const CHMHMapNodePtr  CHierarchicalMapMHPartition::getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const
{
	MRPT_START

	// Look for the ID:
	for (TNodeList::const_iterator it=m_nodes.begin();it!=m_nodes.end();++it)
		if ( it->second->m_hypotheses.has(hypothesisID) )
			if ( !os::_strcmpi( it->second->m_label.c_str(), label.c_str() ) )
				return it->second;

	// Not found:
	return CHMHMapNodePtr();

	MRPT_END
}

/*---------------------------------------------------------------
					getFirstNode
  ---------------------------------------------------------------*/
CHMHMapNodePtr  CHierarchicalMapMHPartition::getFirstNode()
{
	if (m_nodes.empty())
			return CHMHMapNodePtr();
	else	return (m_nodes.begin())->second;
}

/*---------------------------------------------------------------
					saveAreasDiagramForMATLAB
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB(
	const std::string			&filName,
	const CHMHMapNode::TNodeID	&idReferenceNode,
	const THypothesisID &hypothesisID) const
{
	MRPT_UNUSED_PARAM(filName);
	MRPT_UNUSED_PARAM(idReferenceNode);
	MRPT_UNUSED_PARAM(hypothesisID);
/*
	MRPT_START
	unsigned int	nAreaNodes=0;

	const CHMHMapNode	*refNode = getNodeByID( idReferenceNode );
	ASSERT_(refNode!=NULL);
	ASSERT_(refNode->nodeType.isType("Area"));

	FILE	*f=os::fopen(filName.c_str(),"wt");
	if (!f) THROW_EXCEPTION("Can not open output file!");

	// The list of all the m_nodes to be plotted:
	// --------------------------------------------
	std::map<CHMHMapNode::TNodeID,CPose2D>			nodesPoses;			// The ref. pose of each area
	std::map<CHMHMapNode::TNodeID,CPose2D>			nodesMeanPoses;		// The mean pose of the observations in the area
	std::map<CHMHMapNode::TNodeID,CPose2D>::iterator	it;

	// First, insert the reference node:
	nodesPoses[refNode->ID] = CPose2D(0,0,0);

	// Find the rest of "Area" m_nodes:
	for (unsigned int i=0;i<nodeCount();i++)
	{
		const CHMHMapNode	*nod = getNodeByIndex(i);

		// Is this a Area node?
		if (nod->nodeType.isType("Area"))
		{
			nAreaNodes++;	// Counter
			if (nod!=refNode)
			{
				CPosePDFParticles	posePDF;

				computeCoordinatesTransformationBetweenNodes(
								refNode->ID,
								nod->ID,
								posePDF,
								hypothesisID,
								100);

				nodesPoses[nod->ID] = posePDF.getEstimatedPose();
			}
		}
	} // end for each node "i"

	// Assure that all area m_nodes have been localized:
	ASSERT_(nAreaNodes == nodesPoses.size() );


	// Find the mean of the observations in each area:
	for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		CPose3D		meanPose = it->second;
		const CHMHMapNode	*node = getNodeByID( it->first );

		CSimpleMap	*localizedSFs = (CSimpleMap*) node->annotations.get("localizedObservations");

		if (localizedSFs->size())
		{
			// Compute the mean pose:
			CPose3D		meanSFs(0,0,0);

			for (unsigned int k=0;k<localizedSFs->size();k++)
			{
				CPose3DPDF			*pdf;
				CSensoryFrame		*dummy_sf;
				CPose3D				pdfMean;

				localizedSFs->get(k,pdf,dummy_sf);

				pdfMean = pdf->getEstimatedPose();
				meanSFs.addComponents( pdfMean );
			}
			meanSFs *= 1.0f/(localizedSFs->size());
			meanSFs.normalizeAngles();

			meanPose = meanPose + meanSFs;
		}
		else
		{
			// Let the ref. pose to represent the node
		}

		nodesMeanPoses[it->first] = meanPose;
	}

	// --------------------------------------------------------------
	//  Now we have the global poses of all the m_nodes: Draw them!
	// -------------------------------------------------------------
	// Header:
	os::fprintf(f,"%%-------------------------------------------------------\n");
	os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
	os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB'\n");
	os::fprintf(f,"%%\n");
	os::fprintf(f,"%%                        ~ MRPT ~\n");
	os::fprintf(f,"%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
	os::fprintf(f,"%%  http://www.isa.uma.es/ \n");
	os::fprintf(f,"%%-------------------------------------------------------\n\n");

	os::fprintf(f,"hold on;\n");

	float	nodesRadius = 1.5f;

	// Draw the m_nodes:
	// ----------------------
	for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
	{
		const CHMHMapNode	*node = getNodeByID( it->first );
		CPose2D		pose( it->second );

		os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
		os::fprintf(f,"rectangle('Curvature',[1 1],'Position',[%f %f %f %f],'FaceColor',[1 1 1]);\n",
						pose.x - nodesRadius,
						pose.y - nodesRadius,
						2*nodesRadius,
						2*nodesRadius);

		// Draw the node's label:
		os::fprintf(f,"text(%f,%f,'%s','FontSize',7,'HorizontalAlignment','center','Interpreter','none');\n",
						pose.x,
						pose.y,
						node->label.c_str() );

		// And their m_arcs:
		// ----------------------
		for (unsigned int a=0;a<node->m_arcs.size();a++)
		{
			CHMHMapArc	*arc = node->m_arcs[a];
			if (arc->nodeFrom==node->ID)
			{
				CPose2D		poseTo( nodesMeanPoses.find(arc->nodeTo)->second );
				float		x1,x2,y1,y2, Ax,Ay;

				// Compute a unitary direction vector:
				Ax = poseTo.x - pose.x;
				Ay = poseTo.y - pose.y;

				float	len = sqrt(square(Ax)+square(Ay));

				if (len>nodesRadius)
				{
					Ax /= len;
					Ay /= len;

					x1 = pose.x + Ax * nodesRadius;
					y1 = pose.y + Ay * nodesRadius;

					x2 = pose.x + Ax * (len-nodesRadius);
					y2 = pose.y + Ay * (len-nodesRadius);

					os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
				}
			}
		}
	}

	os::fprintf(f,"axis equal; zoom on;");
	os::fclose(f);
	MRPT_END
*/
}

/*---------------------------------------------------------------
				saveAreasDiagramWithEllipsedForMATLAB
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB(
	const std::string				&filName,
	const CHMHMapNode::TNodeID		&idReferenceNode,
	const THypothesisID					&hypothesisID,
	float							uncertaintyExagerationFactor,
	bool							drawArcs,
	unsigned int					numberOfIterationsForOptimalGlobalPoses
	) const
{
	MRPT_UNUSED_PARAM(filName);
	MRPT_UNUSED_PARAM(idReferenceNode);
	MRPT_UNUSED_PARAM(hypothesisID);
	MRPT_UNUSED_PARAM(uncertaintyExagerationFactor);
	MRPT_UNUSED_PARAM(drawArcs);
	MRPT_UNUSED_PARAM(numberOfIterationsForOptimalGlobalPoses);
/*	MRPT_START

	const CHMHMapNode	*refNode = getNodeByID( idReferenceNode );
	ASSERT_(refNode!=NULL);
	ASSERT_(refNode->nodeType.isType("Area"));

	FILE	*f=os::fopen(filName.c_str(),"wt");
	if (!f) THROW_EXCEPTION("Can not open output file!");

	// The list of all the m_nodes to be plotted:
	// --------------------------------------------
	std::map<CHMHMapNode::TNodeID,CPosePDFGaussian>	nodesPoses;			// The ref. pose of each area
	std::map<CHMHMapNode::TNodeID,CPosePDFGaussian>::iterator	it;
	std::map<CHMHMapNode::TNodeID,CPosePDFGaussian>	nodesMeanPoses;		// The mean pose of the observations in the area
	std::map<CHMHMapNode::TNodeID,CPose2D>::iterator	it2;

	computeGloballyConsistentNodeCoordinates( nodesPoses, idReferenceNode, numberOfIterationsForOptimalGlobalPoses );


	// Find the mean of the observations in each area:
	for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		//CPosePDFGaussian		posePDF = it->second;
		CPose2D					meanPose = it->second.mean;

		const CHMHMapNode			*node = getNodeByID( it->first );

		CSimpleMap	*localizedSFs = (CSimpleMap*) node->annotations.get("localizedObservations");

		if (localizedSFs->size())
		{
			// Compute the mean pose:
			CPose3D		meanSFs(0,0,0);

			for (unsigned int k=0;k<localizedSFs->size();k++)
			{
				CPose3DPDF			*pdf;
				CSensoryFrame		*dummy_sf;
				CPose3D				pdfMean;

				localizedSFs->get(k,pdf,dummy_sf);

				pdfMean = pdf->getEstimatedPose();
				meanSFs.addComponents( pdfMean );
			}
			meanSFs *= 1.0f/(localizedSFs->size());
			meanSFs.normalizeAngles();

			meanPose = meanPose + meanSFs;
		}
		else
		{
			// Let the ref. pose to represent the node
		}
		nodesMeanPoses[it->first].mean = meanPose;
		nodesMeanPoses[it->first].cov  = it->second.cov;
	}

	// --------------------------------------------------------------
	//  Now we have the global poses of all the m_nodes: Draw them!
	// -------------------------------------------------------------
	// Header:
	os::fprintf(f,"%%-------------------------------------------------------\n");
	os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
	os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB'\n");
	os::fprintf(f,"%%\n");
	os::fprintf(f,"%%                        ~ MRPT ~\n");
	os::fprintf(f,"%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
	os::fprintf(f,"%%  http://www.isa.uma.es/ \n");
	os::fprintf(f,"%%-------------------------------------------------------\n\n");

	os::fprintf(f,"hold on;\n");

	//float	nodesRadius = 0; //2.0f;

	// Draw the m_nodes:
	// ----------------------
	for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
	{
		const CHMHMapNode			*node = getNodeByID( it->first );
		CPosePDFGaussian	posePDF = it->second;
		CMatrix				C( posePDF.cov );
		CPose2D				pose( posePDF.mean );

		if (C.det()==0)
		{
			C.unit();
			C(0,0)=1e-4f;
			C(1,1)=1e-4f;
		}

		os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
		os::fprintf(f,"c=[%e %e;%e %e];m=[%f %f];error_ellipse(%f*c,m,'conf',0.95);\n",
				C(0,0), C(0,1),
				C(1,0), C(1,1),
				pose.x,pose.y,
				square(uncertaintyExagerationFactor) );

		// Draw the node's label:
		os::fprintf(f,"text(%f,%f,'  %s','FontSize',8,'HorizontalAlignment','center','Interpreter','none');\n",
						pose.x+1.5,
						pose.y+1.5,
						node->label.c_str() );
	}

	// The m_arcs:
	// ----------------------
	if ( drawArcs )
	for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
	{
		const CHMHMapNode		*node = getNodeByID( it->first );
		CPose2D				pose( it->second.mean );

		float nodesRadius = 0;
		for (unsigned int a=0;a<node->m_arcs.size();a++)
		{
			CHMHMapArc	*arc = node->m_arcs[a];
			if (arc->nodeFrom==node->ID)
			{
				CPose2D		poseTo( nodesMeanPoses.find(arc->nodeTo)->second.mean );
				float		x1,x2,y1,y2, Ax,Ay;

				// Compute a unitary direction vector:
				Ax = poseTo.x - pose.x;
				Ay = poseTo.y - pose.y;

				float	len = sqrt(square(Ax)+square(Ay));

				if (len>nodesRadius)
				{
					Ax /= len;
					Ay /= len;

					x1 = pose.x + Ax * nodesRadius;
					y1 = pose.y + Ay * nodesRadius;

					x2 = pose.x + Ax * (len-nodesRadius);
					y2 = pose.y + Ay * (len-nodesRadius);

					os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
				}
			}
		}
	}

	os::fprintf(f,"axis equal; zoom on;");

	os::fclose(f);

	// Free memory:
	nodesPoses.clear();

	MRPT_END
	*/
}

/*---------------------------------------------------------------
					saveGlobalMapForMATLAB
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::saveGlobalMapForMATLAB(
	const std::string			&filName,
	const THypothesisID				&hypothesisID,
	const CHMHMapNode::TNodeID	&idReferenceNode ) const
{
	MRPT_UNUSED_PARAM(filName);
	MRPT_UNUSED_PARAM(hypothesisID);
	MRPT_UNUSED_PARAM(idReferenceNode);
	MRPT_START
/*
	unsigned int	nAreaNodes=0;

	const CHMHMapNode	*refNode = getNodeByID( idReferenceNode );
	ASSERT_(refNode!=NULL);
	ASSERT_(refNode->nodeType.isType("Area"));

	FILE	*f=os::fopen(filName.c_str(),"wt");
	if (!f) THROW_EXCEPTION("Can not open output file!");

	// The list of all the m_nodes to be plotted:
	// --------------------------------------------
	std::map<CHMHMapNode::TNodeID,CPose2D>			nodesPoses;			// The ref. pose of each area
	std::map<CHMHMapNode::TNodeID,CPose2D>			nodesMeanPoses;		// The mean pose of the observations in the area
	std::map<CHMHMapNode::TNodeID,CPose2D>::iterator	it;

	// First, insert the reference node:
	nodesPoses[refNode->ID] = CPose2D(0,0,0);

	// Find the rest of "Area" m_nodes:
	for (unsigned int i=0;i<nodeCount();i++)
	{
		const CHMHMapNode	*nod = getNodeByIndex(i);

		// Is this a Area node?
		if (nod->nodeType.isType("Area"))
		{
			nAreaNodes++;	// Counter
			if (nod!=refNode)
			{
				CPosePDFParticles	posePDF;

				computeCoordinatesTransformationBetweenNodes(
								refNode->ID,
								nod->ID,
								posePDF,
								hypothesisID,
								100);

				nodesPoses[nod->ID] = posePDF.getEstimatedPose();
			}
		}
	} // end for each node "i"

	// Assure that all area m_nodes have been localized:
	ASSERT_(nAreaNodes == nodesPoses.size() );

	// Find the mean of the observations in each area:
	for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		CPose2D		meanPose = it->second;
		const CHMHMapNode	*node = getNodeByID( it->first );

		CSimpleMap	*localizedSFs = (CSimpleMap*) node->annotations.get("localizedObservations");

		if (localizedSFs->size())
		{
			// Compute the mean pose:
			CPose3D		meanSFs(0,0,0);

			for (unsigned int k=0;k<localizedSFs->size();k++)
			{
				CPose3DPDF			*pdf;
				CSensoryFrame		*dummy_sf;
				CPose3D				pdfMean;

				localizedSFs->get(k,pdf,dummy_sf);

				pdfMean = pdf->getEstimatedPose();
				meanSFs.addComponents( pdfMean );
			}
			meanSFs *= 1.0f/(localizedSFs->size());
			meanSFs.normalizeAngles();

			meanPose = meanPose + meanSFs;
		}
		else
		{
			// Let the ref. pose to represent the node
		}

		nodesMeanPoses[it->first] = meanPose;
	}

	// --------------------------------------------------------------
	//  Now we have the global poses of all the m_nodes: Draw them!
	// -------------------------------------------------------------
	// Header:
	os::fprintf(f,"%%-------------------------------------------------------\n");
	os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
	os::fprintf(f,"%%'CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB'\n");
	os::fprintf(f,"%%\n");
	os::fprintf(f,"%%                        ~ MRPT ~\n");
	os::fprintf(f,"%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
	os::fprintf(f,"%%  http://www.isa.uma.es/ \n");
	os::fprintf(f,"%%-------------------------------------------------------\n\n");

	os::fprintf(f,"hold on;\n");

	float	nodesRadius = 1.5f;

	// Draw the metric maps of each area:
	for (it = nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		CPose2D		meanPose = it->second;
		const CHMHMapNode	*node = getNodeByID( it->first );

		CMultiMetricMap		*metricMap = (CMultiMetricMap*) node->annotations.get("hybridMetricMap");
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
		ASSERT_(metricMap!=NULL);
		ASSERT_(metricMap->m_pointsMap);
#endif
		int			n;
		float		*xs,*ys,*zs;
		CPoint2D	pL,pG;

		metricMap->m_pointsMap->getPointsBuffer(n,xs,ys,zs);

		os::fprintf(f,"\n%% Metric map for node: %s\n", node->label.c_str() );

		if (n)
		{
			os::fprintf(f,"map=[" );
			for (int j=0;j<n;j+=10)
			{
				if (j!=0)	os::fprintf(f,";" );
				// Local coords:
				pL.x = xs[j];
				pL.y = ys[j];

				// Global coords:
				pG = meanPose + pL;

				// write:
				os::fprintf(f,"%.03f %.03f",pG.x,pG.y );
			}
			os::fprintf(f,"];\n" );
			os::fprintf(f,"plot(map(:,1),map(:,2),'.k','MarkerSize',3);\n" );
		}

	}


	// Draw the m_nodes:
	// ----------------------
	for (it = nodesMeanPoses.begin();it!=nodesMeanPoses.end();it++)
	{
		const CHMHMapNode	*node = getNodeByID( it->first );
		CPose2D		pose( it->second );

		os::fprintf(f,"\n%% Node: %s\n", node->label.c_str() );
		os::fprintf(f,"rectangle('Curvature',[1 1],'Position',[%f %f %f %f],'FaceColor',[1 1 1]);\n",
						pose.x - nodesRadius,
						pose.y - nodesRadius,
						2*nodesRadius,
						2*nodesRadius);

		// Draw the node's label:
		os::fprintf(f,"text(%f,%f,'%s','FontSize',7,'HorizontalAlignment','center','Interpreter','none');\n",
						pose.x,
						pose.y,
						node->label.c_str() );

		// And their m_arcs:
		// ----------------------
		for (unsigned int a=0;a<node->m_arcs.size();a++)
		{
			CHMHMapArc	*arc = node->m_arcs[a];
			if (arc->nodeFrom==node->ID)
			{
				CPose2D		poseTo( nodesMeanPoses.find(arc->nodeTo)->second );
				float		x1,x2,y1,y2, Ax,Ay;

				// Compute a unitary direction vector:
				Ax = poseTo.x - pose.x;
				Ay = poseTo.y - pose.y;

				float	len = sqrt(square(Ax)+square(Ay));

				if (len>nodesRadius)
				{
					Ax /= len;
					Ay /= len;

					x1 = pose.x + Ax * nodesRadius;
					y1 = pose.y + Ay * nodesRadius;

					x2 = pose.x + Ax * (len-nodesRadius);
					y2 = pose.y + Ay * (len-nodesRadius);

					os::fprintf(f,"line([%f %f],[%f,%f]);\n", x1,x2,y1,y2);
				}

			}
		}

	}



	os::fprintf(f,"axis equal; zoom on;");

	os::fclose(f);
*/
	MRPT_END
}


// Variables:
struct TDistance
{
	TDistance() : dist( std::numeric_limits<unsigned>::max() )
	{}

	TDistance(const unsigned &D) : dist(D) { }
	const TDistance & operator =(const unsigned &D) { dist = D; return *this;}

	unsigned dist;
};

struct TPrevious
{
	TPrevious() : id( AREAID_INVALID )
	{}

	CHMHMapNode::TNodeID  id;
};



/*---------------------------------------------------------------
					findPathBetweenNodes
  ---------------------------------------------------------------*/
void CHierarchicalMapMHPartition::findPathBetweenNodes(
	 const CHMHMapNode::TNodeID		&source,
	 const CHMHMapNode::TNodeID		&target,
	 const THypothesisID			&hypothesisID,
	 TArcList						&ret,
	 bool							direction) const
{
	MRPT_START

	/*
	1  function Dijkstra(G, w, s)
	2     for each vertex v in V[G]                        // Initializations
	3           d[v] := infinity
	4           previous[v] := undefined
	5     d[s] := 0
	6     S := empty set
	7     Q := V[G]
	8     while Q is not an empty set                      // The algorithm itself
	9           u := Extract_Min(Q)
								------>	(u=t)? -> end!
	10           S := S union {u}
	11           for each edge (u,v) outgoing from u
	12                  if d[u] + w(u,v) < d[v]             // Relax (u,v)
	13                        d[v] := d[u] + w(u,v)
	14                        previous[v] := u
	*/

	map<CHMHMapNode::TNodeID,TDistance>					d;		// distance
	map<CHMHMapNode::TNodeID,TPrevious>					previous;
	map<CHMHMapNode::TNodeID, CHMHMapArcPtr>		previous_arcs;
	map<CHMHMapNode::TNodeID, bool>						visited;

	unsigned 						visitedCount = 0;

	ASSERTMSG_( m_nodes.find(source)!=m_nodes.end(), format("Source node %u not found in the H-Map",(unsigned int)source)  );
	ASSERTMSG_( m_nodes.find(target)!=m_nodes.end(), format("Target node %u not found in the H-Map",(unsigned int)target)  );

	ASSERT_( m_nodes.find(source)->second->m_hypotheses.has(hypothesisID) );
	ASSERT_( m_nodes.find(target)->second->m_hypotheses.has(hypothesisID) );

	// Init:
	// d: already initialized to infinity by default.
	// previous: idem
	// previous_arcs: idem
	// visited: idem

	d[source] = 0;

	TNodeList::const_iterator u;

	// The algorithm:
	do
	{
		// Finds the index of the minimum:
		//unsigned int	i;
		unsigned		min_d = std::numeric_limits<unsigned>::max();

		u = m_nodes.end();

		for (TNodeList::const_iterator i=m_nodes.begin();i!=m_nodes.end();++i)
		{
			if (i->second->m_hypotheses.has(hypothesisID))
			{
				if (d[i->first].dist < min_d && !visited[i->first])
				{
					u = i;
					min_d = d[u->first].dist;
				}
			}
		}

		ASSERT_(u!=m_nodes.end());

		visited[u->first] = true;
		visitedCount++;

		// For each arc from "u":
		const CHMHMapNodePtr nodeU = getNodeByID(u->first);
		TArcList  arcs;
		nodeU->getArcs( arcs, hypothesisID );
		for (TArcList::const_iterator i=arcs.begin();i!=arcs.end();++i)
		{
			CHMHMapNode::TNodeID	vID;
			if (!direction)
			{
				if ( (*i)->getNodeFrom() != nodeU->getID() )
						vID = (*i)->getNodeFrom();
				else	vID = (*i)->getNodeTo();
			}
			else
			{
				if ( (*i)->getNodeFrom() != nodeU->getID() )
						continue;
				else	vID = (*i)->getNodeTo();
			}

			if ( (min_d+1) < d[vID].dist)
			{
				d[vID].dist = min_d+1;
				previous[vID].id = u->first;
				previous_arcs[vID] = *i;
			}
		}
	} while ( u->first !=target && visitedCount<m_nodes.size() );

	// Arrived to target?
	ret.clear();

	if (u->first==target)
	{
		// Path found:
		CHMHMapNode::TNodeID nod = target;
		do
		{
			ret.push_front( previous_arcs[nod] );
			nod = previous[nod].id;
		} while (nod!=source);
	}

	MRPT_END
}

/*---------------------------------------------------------------
		computeCoordinatesTransformationBetweenNodes
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes(
	const CHMHMapNode::TNodeID	&nodeFrom,
	const CHMHMapNode::TNodeID	&nodeTo,
	CPose3DPDFParticles			&posePDF,
	const THypothesisID			&hypothesisID,
	unsigned int				particlesCount,
	float						additionalNoiseXYratio,
	float						additionalNoisePhiRad ) const
{
	MRPT_START

	TArcList							arcsPath;
	TArcList::const_iterator			arcsIt;
	static const CPose3D				nullPose(0,0,0);
	CHMHMapNode::TNodeID				lastNode,nextNode;
	size_t								pathLength;

	typedef mrpt::aligned_containers<CPose3D>::vector_t TPose3DList;
	std::vector<TPose3DList>			listSamples;
	std::vector<TPose3DList>::iterator	lstIt;
	TPose3DList							dummyList;
	TPose3DList::iterator				poseIt;

	// Find the shortest sequence of arcs connecting the nodes:
	findPathBetweenNodes( nodeFrom, nodeTo, hypothesisID, arcsPath );
	ASSERT_( arcsPath.size() );
	pathLength = arcsPath.size();

	// Prepare the PDF:
	posePDF.resetDeterministic( nullPose, particlesCount );

	// Draw the pose sample:
	lastNode = nodeFrom;
	dummyList.resize( particlesCount );
	listSamples.resize( pathLength, dummyList );
	for (arcsIt=arcsPath.begin(),lstIt=listSamples.begin();arcsIt!=arcsPath.end();lstIt++,arcsIt++)
	{
		// Flag for the pose PDF needing to be reversed:
		bool	reversedArc = (*arcsIt)->getNodeTo() == lastNode;
		nextNode = reversedArc ? (*arcsIt)->getNodeFrom() : (*arcsIt)->getNodeTo();

		// Check reference pose IDs between arcs:
		// ------------------------------------------------
		TPoseID		curNodeRefPoseID, nextNodeRefPoseID;
		getNodeByID((*arcsIt)->getNodeFrom())->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, curNodeRefPoseID, hypothesisID, true);
		getNodeByID((*arcsIt)->getNodeTo())->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, nextNodeRefPoseID, hypothesisID, true);

		TPoseID		srcRefPoseID, trgRefPoseID;
		(*arcsIt)->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, srcRefPoseID, hypothesisID, true );
		(*arcsIt)->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, trgRefPoseID, hypothesisID, true );

		ASSERT_( curNodeRefPoseID  == srcRefPoseID );
		ASSERT_( nextNodeRefPoseID == trgRefPoseID);


		// Get the pose PDF:
		CSerializablePtr anotation = (*arcsIt)->m_annotations.get( ARC_ANNOTATION_DELTA, hypothesisID );
		ASSERT_( anotation );

		CPose3DPDFGaussian pdf; // Convert to gaussian
		pdf.copyFrom(*CPose3DPDFPtr(anotation));

		vector<CVectorDouble>  samples;

		pdf.drawManySamples(lstIt->size(),samples);
		ASSERT_( samples.size() == lstIt->size() );
		ASSERT_( samples[0].size() == 6 );

		vector<CVectorDouble>::const_iterator  samplIt;
		for (poseIt=lstIt->begin(),samplIt=samples.begin();poseIt!=lstIt->end();poseIt++,samplIt++)
		{
			// Minimum added noise:
			poseIt->setFromValues(
				(*samplIt)[0] + additionalNoiseXYratio * randomGenerator.drawGaussian1D_normalized(),
				(*samplIt)[1] + additionalNoiseXYratio * randomGenerator.drawGaussian1D_normalized(),
				(*samplIt)[2],
				(*samplIt)[3] + additionalNoisePhiRad * randomGenerator.drawGaussian1D_normalized(),
				(*samplIt)[4],
				(*samplIt)[5]
				);

			// Pose composition:
			if (reversedArc)	*poseIt = nullPose - *poseIt;
		}

		// for the next iter:
		lastNode = nextNode;
	}

	// Compute the pose composition:
	for (unsigned int i=0;i<particlesCount;i++)
	{
		CPose3D		&sample = *posePDF.m_particles[i].d;

		for (unsigned int j=0;j<pathLength;j++)
		{
			if (j)
					sample = sample + listSamples[j][i];
			else	sample = listSamples[j][i];
		}
	}

	posePDF.normalizeWeights();

#if 0
	CPose3DPDFGaussian	auxPDF;
	auxPDF.copyFrom( posePDF );
	cout << "[computeCoordinatesTransformationBetweenNodes] Nodes: " << nodeFrom << " - " << nodeTo << ": " << auxPDF;
#endif

	MRPT_END
}


/*---------------------------------------------------------------
			computeMatchProbabilityBetweenNodes
  ---------------------------------------------------------------*/
float  CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes(
	const CHMHMapNode::TNodeID	&nodeFrom,
	const CHMHMapNode::TNodeID	&nodeTo,
	float						&maxMatchProb,
	CPose3DPDFSOG				&estimatedRelativePose,
	const THypothesisID				&hypothesisID,
	unsigned int				monteCarloSamplesPose )
{
	MRPT_UNUSED_PARAM(nodeFrom);
	MRPT_UNUSED_PARAM(nodeTo);
	MRPT_UNUSED_PARAM(maxMatchProb);
	MRPT_UNUSED_PARAM(estimatedRelativePose);
	MRPT_UNUSED_PARAM(hypothesisID);
	MRPT_UNUSED_PARAM(monteCarloSamplesPose);
	MRPT_START
	THROW_EXCEPTION("TO DO")
	MRPT_END
}

/*---------------------------------------------------------------
					findArcsBetweenNodes
  ---------------------------------------------------------------*/
void CHierarchicalMapMHPartition::findArcsBetweenNodes(
	const CHMHMapNode::TNodeID	&node1id,
	const CHMHMapNode::TNodeID	&node2id,
	const THypothesisID			&hypothesisID,
	TArcList					&ret ) const
{
	MRPT_START

	ret.clear();
	const CHMHMapNodePtr node1 = getNodeByID( node1id );

	TArcList		lstArcs;
	TArcList::const_iterator 	itArc;

	node1->getArcs( lstArcs );
	for (itArc=lstArcs.begin();itArc!=lstArcs.end();itArc++)
	{
		if ((*itArc)->m_hypotheses.has(hypothesisID))
			if ( (*itArc)->m_nodeFrom == node2id ||
				 (*itArc)->m_nodeTo == node2id)
			{
				ret.push_back( *itArc );
			}
	}

	MRPT_END
}

/*---------------------------------------------------------------
					findArcsOfTypeBetweenNodes
  ---------------------------------------------------------------*/
void CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes(
	const CHMHMapNode::TNodeID	&node1id,
	const CHMHMapNode::TNodeID	&node2id,
	const THypothesisID			&hypothesisID,
	const std::string			&arcType,
	TArcList					&ret) const
{
	MRPT_START

	ret.clear();
	const CHMHMapNodePtr node1 = getNodeByID( node1id );

	TArcList		lstArcs;
	TArcList::const_iterator 	itArc;

	node1->getArcs( lstArcs );
	for (itArc=lstArcs.begin();itArc!=lstArcs.end();itArc++)
	{
		if ((*itArc)->m_hypotheses.has(hypothesisID))
			if ( (*itArc)->m_nodeFrom == node2id ||
				 (*itArc)->m_nodeTo == node2id)
			{
				if ((*itArc)->m_arcType.isType(arcType) )
					ret.push_back( *itArc );
			}
	}

	MRPT_END
}


/*---------------------------------------------------------------
					areNodesNeightbour
  ---------------------------------------------------------------*/
bool	 CHierarchicalMapMHPartition::areNodesNeightbour(
	const CHMHMapNode::TNodeID	&node1id,
	const CHMHMapNode::TNodeID	&node2id,
	const THypothesisID				&hypothesisID,
	const char					*requiredAnnotation ) const
{
	MRPT_START

	const CHMHMapNodePtr node1 = getNodeByID( node1id );

	TArcList		lstArcs;
	TArcList::const_iterator 	itArc;

	node1->getArcs( lstArcs );
	for (itArc=lstArcs.begin();itArc!=lstArcs.end();itArc++)
	{
		if ((*itArc)->m_hypotheses.has(hypothesisID))
			if ( (*itArc)->m_nodeFrom == node2id ||
				 (*itArc)->m_nodeTo == node2id)
			{
				if (!requiredAnnotation)
					return true;
				else
				if ( (*itArc)->m_annotations.get(requiredAnnotation, hypothesisID) )
					return true;
			}
	}

	return false;

	MRPT_END
}


/*---------------------------------------------------------------
						getAs3DScene
  ---------------------------------------------------------------*/

void  CHierarchicalMapMHPartition::getAs3DScene(
	COpenGLScene				&outScene,
	const CHMHMapNode::TNodeID	&idReferenceNode,
	const THypothesisID			&hypothesisID,
	const unsigned int			&numberOfIterationsForOptimalGlobalPoses,
	const bool					&showRobotPoseIDs
	) const
{
	MRPT_START

	// First: Clear and add the cool "ground grid" :-P
	outScene.clear();
	{
		mrpt::opengl::CGridPlaneXYPtr obj = mrpt::opengl::CGridPlaneXY::Create( -500,500,-500,500,0,5 );
		obj->setColor(0.3,0.3,0.3);
		outScene.insert( obj );
	}

	typedef std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>,Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > > TMapID2PosePDF;
	TMapID2PosePDF	nodesPoses;			// The ref. pose of each area
	TMapID2PosePDF::iterator	it;

	typedef std::map<CHMHMapNode::TNodeID,CPose2D, std::less<CHMHMapNode::TNodeID>,Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose2D> > > TMapID2Pose2D;
	TMapID2Pose2D	nodesMeanPoses;		// The mean pose of the observations in the area
	TMapID2Pose2D::iterator	it2;

	// Only those nodes in the "hypothesisID" are computed.
	computeGloballyConsistentNodeCoordinates( nodesPoses, idReferenceNode, hypothesisID, numberOfIterationsForOptimalGlobalPoses);

	// Find the mean of the observations in each area:
	for (it=nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		CPose2D		meanPose = CPose2D(it->second.mean);
		const CHMHMapNodePtr node = getNodeByID( it->first );

		CRobotPosesGraphPtr posesGraph = node->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH,hypothesisID);

		if (posesGraph && posesGraph->size())
		{
			// Compute the mean pose:
			CPose3D		meanSFs(0,0,0);

			for (CRobotPosesGraph::const_iterator it=posesGraph->begin();it!=posesGraph->end();++it)
				meanSFs.addComponents( it->second.pdf.getMeanVal() );

			meanSFs *= 1.0f/(posesGraph->size());
			meanSFs.normalizeAngles();

			meanPose = meanPose + CPose2D(meanSFs);
		}
		else
		{
			// Let the ref. pose to represent the node
		}

		nodesMeanPoses[it->first] = meanPose;
	}

	// ----------------------------------------------------------------
	//  Now we have the global poses of all the m_nodes: Draw'em all
	// ---------------------------------------------------------------
	// Draw the (grid maps) metric maps of each area:
	for (it = nodesPoses.begin();it!=nodesPoses.end();it++)
	{
		const CPose3D	&pose = it->second.mean;
		const CHMHMapNodePtr node = getNodeByID( it->first );

		CMultiMetricMapPtr metricMap = node->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS, hypothesisID);
		if (metricMap) //ASSERT_(metricMap);
		{
			mrpt::opengl::CSetOfObjectsPtr objTex = mrpt::opengl::CSetOfObjects::Create();
			metricMap->getAs3DObject( objTex );
			objTex->setPose( pose );
			outScene.insert( objTex );
		}
	}


	float	nodes_height = 10.0f;
	float	nodes_radius = 1.0f;

	// Draw the m_nodes:
	// ----------------------
	for (it = nodesPoses.begin(),it2=nodesMeanPoses.begin();it!=nodesPoses.end();it++,it2++)
	{
		const CHMHMapNodePtr node = getNodeByID( it->first );
		const CPose3D		&pose = it->second.mean ;
		const CPose3D		&meanPose= it2->second ;

		// The sphere of the node:
		mrpt::opengl::CSpherePtr objSphere = mrpt::opengl::CSphere::Create();

		objSphere->setName( node->m_label );
		objSphere->setColor(0,0,1);
		objSphere->setLocation(meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);
		objSphere->setRadius( nodes_radius );
		objSphere->setNumberDivsLongitude(16);
		objSphere->setNumberDivsLatitude(16);

		objSphere->enableRadiusIndependentOfEyeDistance();

		outScene.insert( objSphere );

		// The label with the name of the node:
		mrpt::opengl::CTextPtr objText = mrpt::opengl::CText::Create();
	//	objText->m_str = node->m_label;
		objText->setString( format("%li",(long int)node->getID()) );
		//objText->m_fontHeight = 20;
		objText->setColor(1,0,0);
		objText->setLocation(meanPose.x(), meanPose.y(), meanPose.z() + nodes_height);

		outScene.insert( objText );

		// And the observations "on the ground" as disks
		// ---------------------------------------------------
		CRobotPosesGraphPtr posesGraph = node->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH,hypothesisID);
		//ASSERT_(posesGraph);

		if (posesGraph)
		{
			for (CRobotPosesGraph::const_iterator it=posesGraph->begin();it!=posesGraph->end();++it)
			{
				CPose3D	SF_pose;
				it->second.pdf.getMean(SF_pose);

				CPose3D auxPose( pose + SF_pose );

				mrpt::opengl::CDiskPtr glObj = mrpt::opengl::CDisk::Create();

				glObj->setColor(1,0,0);

				glObj->setPose( auxPose );
				//glObj->m_z = 0;

				glObj->setDiskRadius(0.05);
				glObj->setSlicesCount(20);
				glObj->setLoopsCount(10);

				if (showRobotPoseIDs)
				{
					glObj->setName( format("%i",(int)it->first ) );
					glObj->enableShowName();
				}

				outScene.insert( glObj );

				// And a line up-to the node:
				mrpt::opengl::CSimpleLinePtr objLine = mrpt::opengl::CSimpleLine::Create();

				objLine->setColor(1,0,0, 0.2);
				objLine->setLineWidth(1.5);

				objLine->setLineCoords(meanPose.x(),meanPose.y(),nodes_height,auxPose.x(), auxPose.y(),0);

				outScene.insert( objLine );
			} // end for observation "disks"
		}


		// And their m_arcs:
		// ----------------------
		TArcList   arcs;
		node->getArcs( arcs, hypothesisID );
		for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
		{
			CHMHMapArcPtr arc = *a;

			if (arc->getNodeFrom()==node->getID())
			{
				CPose3D		poseTo( nodesMeanPoses.find(arc->getNodeTo())->second );

				// Draw the line:
				mrpt::opengl::CSimpleLinePtr objLine = mrpt::opengl::CSimpleLine::Create();

				objLine->setColor(0,1,0, 0.5);
				objLine->setLineWidth(5);

				objLine->setLineCoords(meanPose.x(),meanPose.y(),meanPose.z()+nodes_height,poseTo.x(), poseTo.y(),poseTo.z() + nodes_height);

				outScene.insert( objLine );
			}
		}

	}


	MRPT_END
}


/*---------------------------------------------------------------
			computeGloballyConsistentNodeCoordinates
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates(
	std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > >		&nodePoses,
	const CHMHMapNode::TNodeID							&idReferenceNode,
	const THypothesisID										&hypothesisID,
	const unsigned int									&numberOfIterations) const
{
	MRPT_START

	nodePoses.clear();
	
	if (m_arcs.empty())
		return; // Nothing we can do!


	// 1) Convert hmt-slam graph into graphslam graph... (this should be avoided in future version of HTML-SLAM!!)
	graphs::CNetworkOfPoses3DInf  pose_graph;

	for (TArcList::const_iterator it_arc=m_arcs.begin();it_arc!=m_arcs.end();++it_arc)
	{
		if (!(*it_arc)->m_hypotheses.has(hypothesisID)) continue;

		const CHMHMapNode::TNodeID id_from = (*it_arc)->getNodeFrom();
		const CHMHMapNode::TNodeID id_to   = (*it_arc)->getNodeTo();

		CSerializablePtr anotation = (*it_arc)->m_annotations.get( ARC_ANNOTATION_DELTA, hypothesisID );
		if (!anotation) continue;
		
		CPose3DPDFGaussianInf edge_rel_pose_pdf; // Convert to gaussian
		edge_rel_pose_pdf.copyFrom(*CPose3DPDFPtr(anotation));

		pose_graph.insertEdgeAtEnd(id_from,id_to,edge_rel_pose_pdf);
	}

	// 2) Initialize global poses of nodes with Dijkstra:
	pose_graph.root = idReferenceNode;
	pose_graph.dijkstra_nodes_estimate();

	// 3) Optimize with graph-slam:
	graphslam::TResultInfoSpaLevMarq out_info;
	TParametersDouble  graphslam_params;
	graphslam_params["max_iterations"] = numberOfIterations;

	graphslam::optimize_graph_spa_levmarq(
		pose_graph,
		out_info,
		NULL,   // Optimize all nodes
		graphslam_params
		);

	// 4) Copy back optimized results into the HMT-SLAM graph:
	for (graphs::CNetworkOfPoses3DInf::global_poses_t::const_iterator it_node=pose_graph.nodes.begin();it_node!=pose_graph.nodes.end();++it_node)
	{
		const CHMHMapNode::TNodeID node_id = it_node->first;
		
		// To the output map:
		CPose3DPDFGaussian & new_pose = nodePoses[node_id];
		new_pose.mean = it_node->second;
		new_pose.cov.setIdentity();  // *** At present, graphslam does not output the uncertainty of poses... ***
	}

#if __computeGloballyConsistentNodeCoordinates__VERBOSE
	for ( map<CHMHMapNode::TNodeID,CPose3DPDFGaussian>::const_iterator it=nodePoses.begin();it!=nodePoses.end();++it)
		cout << it->first << ": " << it->second.mean << endl;
	cout << endl;
#endif

	MRPT_END
}


/*---------------------------------------------------------------
				dumpAsText
  ---------------------------------------------------------------*/
void  CHierarchicalMapMHPartition::dumpAsText(utils::CStringList &st) const
{
	st.clear();
	st << "LIST OF NODES";
	st << "================";

	for (TNodeList::const_iterator it=m_nodes.begin();it!=m_nodes.end();++it)
	{
		std::string  s;
		s += format("NODE ID: %i\t LABEL:%s\tARCS: ", (int)it->second->getID(), it->second->m_label.c_str() );
		TArcList arcs;
		it->second->getArcs(arcs);
		for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
			s += format("%i-%i, ", (int)(*a)->getNodeFrom(), (int)(*a)->getNodeTo() );

		st << s;

		for (CMHPropertiesValuesList::const_iterator ann = it->second->m_annotations.begin(); ann != it->second->m_annotations.end(); ++ann)
		{
			s= format("   [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID, ann->name.c_str() );
			if ( ann->value )
					s+= string(ann->value->GetRuntimeClass()->className);
			else	s+= "(NULL)";

			st << s;

			if ( ann->name == NODE_ANNOTATION_REF_POSEID )
			{
				TPoseID refID;
				it->second->m_annotations.getElemental(NODE_ANNOTATION_REF_POSEID, refID, ann->ID );
				st << format("     VALUE: %i",(int)refID);
			}
			else if ( ann->name == NODE_ANNOTATION_POSES_GRAPH )
			{
				CRobotPosesGraphPtr posesGraph = it->second->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH,ann->ID);
				ASSERT_(posesGraph);

				st << format("     CRobotPosesGraph has %i poses:",(int)posesGraph->size());
				CPose3D  pdfMean;
				for (CRobotPosesGraph::const_iterator p=posesGraph->begin();p!=posesGraph->end();++p)
				{
					const CPose3DPDFParticles &pdf = p->second.pdf;
					pdf.getMean(pdfMean);
					st << format("       Pose %i \t (%.03f,%.03f,%.03fdeg)",
						(int)p->first,
						pdfMean.x(),
						pdfMean.y(),
						RAD2DEG(pdfMean.yaw()));
				}
			}
		}

		st << "";

	}


	st << "";
	st << "";
	st << "LIST OF ARCS";
	st << "================";

	for (TArcList::const_iterator it=m_arcs.begin();it!=m_arcs.end();++it)
	{
		std::string  s;
		s += format("ARC: %i -> %i\n", (int)(*it)->getNodeFrom(), (int)(*it)->getNodeTo() );

		s+= string("   Arc type: ")+(*it)->m_arcType.getType();

		st << s;

		for (CMHPropertiesValuesList::const_iterator ann = (*it)->m_annotations.begin(); ann != (*it)->m_annotations.end(); ++ann)
		{
			s= format("   [HYPO ID #%02i] Annotation '%s' Class: ", (int)ann->ID, ann->name.c_str() );
			if ( ann->value )
					s+= string(ann->value->GetRuntimeClass()->className);
			else	s+= "(NULL)";

			st << s;

			if ( ann->name == ARC_ANNOTATION_DELTA_SRC_POSEID )
			{
				TPoseID refID;
				(*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, refID, ann->ID );
				st << format("     VALUE: %i",(int)refID);
			}
			else if ( ann->name == ARC_ANNOTATION_DELTA_TRG_POSEID )
			{
				TPoseID refID;
				(*it)->m_annotations.getElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, refID, ann->ID );
				st << format("     VALUE: %i",(int)refID);
			}
			else if ( ann->name == ARC_ANNOTATION_DELTA )
			{
				CSerializablePtr o = (*it)->m_annotations.get(ARC_ANNOTATION_DELTA, ann->ID );
				ASSERT_(o);

				CPose3DPDFGaussian relativePoseAcordToArc;
				relativePoseAcordToArc.copyFrom(*CPose3DPDFPtr(o));

				st << format("     VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
					relativePoseAcordToArc.mean.x(),
					relativePoseAcordToArc.mean.y(),
					relativePoseAcordToArc.mean.z(),
					RAD2DEG( relativePoseAcordToArc.mean.yaw() ),
					RAD2DEG( relativePoseAcordToArc.mean.pitch() ),
					RAD2DEG( relativePoseAcordToArc.mean.roll() ) );
			}
		}

		st << "";
	}

}



/*---------------------------------------------------------------
					findArcOfTypeBetweenNodes
  ---------------------------------------------------------------*/
CHMHMapArcPtr CHierarchicalMapMHPartition::findArcOfTypeBetweenNodes(
	const CHMHMapNode::TNodeID	&node1id,
	const CHMHMapNode::TNodeID	&node2id,
	const THypothesisID			&hypothesisID,
	const std::string			&arcType,
	bool						&isInverted ) const
{
	MRPT_START

	TArcList lstArcs;
	findArcsOfTypeBetweenNodes( node1id, node2id, hypothesisID, arcType, lstArcs);

	for (TArcList::const_iterator a=lstArcs.begin();a!=lstArcs.end();++a)
	{
		if ( (*a)->getNodeFrom() == node1id )
		{
			// Found, and in the correct direction:
			isInverted = false;
			return *a;
		}
		else
		{
			// Found, in the opossite direction:
			isInverted = true;
			return *a;
		}
	}

	return CHMHMapArcPtr();
	MRPT_END
}

/*---------------------------------------------------------------
			computeOverlapProbabilityBetweenNodes
  ---------------------------------------------------------------*/
double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes(
	const CHMHMapNode::TNodeID	&nodeFrom,
	const CHMHMapNode::TNodeID	&nodeTo,
	const THypothesisID			&hypothesisID,
	const size_t &monteCarloSamples,
	const float margin_to_substract
	) const
{
	MRPT_START

	size_t 		i,hits = 0;
	CPose3DPDFParticles	posePDF;
	const CHMHMapNodePtr from	= getNodeByID( nodeFrom );
	const CHMHMapNodePtr to		= getNodeByID( nodeTo );

	// Draw pose samples:
	computeCoordinatesTransformationBetweenNodes(
		nodeFrom,
		nodeTo,
		posePDF,
		hypothesisID,
		monteCarloSamples);
		/*0.15f,
		DEG2RAD(3.0f) );*/

	// Get the extends of grid maps:
	float			r1_x_min, r1_x_max, r1_y_min, r1_y_max,r2_x_min, r2_x_max, r2_y_min, r2_y_max;

	// MAP1:
	CMultiMetricMapPtr hMap1 = from->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);

	ASSERT_(hMap1->m_gridMaps.size()>0);

	r1_x_min = hMap1->m_gridMaps[0]->getXMin();
	r1_x_max = hMap1->m_gridMaps[0]->getXMax();
	r1_y_min = hMap1->m_gridMaps[0]->getYMin();
	r1_y_max = hMap1->m_gridMaps[0]->getYMax();

	if (r1_x_max-r1_x_min>2*margin_to_substract)
	{
		r1_x_max -= margin_to_substract;
		r1_x_min += margin_to_substract;
	}
	if (r1_y_max-r1_y_min>2*margin_to_substract)
	{
		r1_y_max -= margin_to_substract;
		r1_y_min += margin_to_substract;
	}


	// MAP2:
	CMultiMetricMapPtr hMap2 = to->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS, hypothesisID, false);

	ASSERT_(hMap2->m_gridMaps.size()>0);

	r2_x_min = hMap2->m_gridMaps[0]->getXMin();
	r2_x_max = hMap2->m_gridMaps[0]->getXMax();
	r2_y_min = hMap2->m_gridMaps[0]->getYMin();
	r2_y_max = hMap2->m_gridMaps[0]->getYMax();

	if (r2_x_max-r2_x_min>2*margin_to_substract)
	{
		r2_x_max -= margin_to_substract;
		r2_x_min += margin_to_substract;
	}
	if (r2_y_max-r2_y_min>2*margin_to_substract)
	{
		r2_y_max -= margin_to_substract;
		r2_y_min += margin_to_substract;
	}

	// Compute the probability:
	for (i=0;i<monteCarloSamples;i++)
	{
		if (math::RectanglesIntersection(	r1_x_min, r1_x_max, r1_y_min, r1_y_max,
											r2_x_min, r2_x_max, r2_y_min, r2_y_max,
											posePDF.m_particles[i].d->x(),
											posePDF.m_particles[i].d->y(),
											posePDF.m_particles[i].d->yaw() ) )
		{
			hits++;
		}
	}

	return static_cast<double>(hits) / monteCarloSamples;

	MRPT_END
}
