CHMHMapNode.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/random.h>
 
using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace std;
 
IMPLEMENTS_SERIALIZABLE(CHMHMapNode, CSerializable, mrpt::hmtslam)
 
/*---------------------------------------------------------------
                                                Default constructor
  ---------------------------------------------------------------*/
CHMHMapNode::CHMHMapNode(
        CHierarchicalMHMap* parent, const THypothesisIDSet& hyps)
        : m_hypotheses(hyps),
          m_ID(),
          m_arcs(),
          m_parent(parent),
          m_nodeType(NODE_TYPES, DEFAULT_NODE_TYPE),
          m_label("none")
{
        // Assure that ID is unique in the graph:
        // -----------------------------------------
        if (m_parent.get())
        {
                // Parent will be nullptr only in the default constructor for a
                // temporary
                //  initialization before loading the object from "readFromStream"
                m_ID = 0;
                do
                {
                        m_ID++; /* =
                                (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF))
                                << 32) |
                                (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)) <<
                                16) |
                                (((uint64_t)getRandomGenerator().drawUniform(0.0f,0xFFFF)));*/
                } while (m_parent->getNodeByID(m_ID));
        }
}
 
/*---------------------------------------------------------------
                                        Destructor
  ---------------------------------------------------------------*/
CHMHMapNode::~CHMHMapNode()
{
        // To the graph:
        if (m_parent.get()) m_parent->onNodeDestruction(this);
 
        // To the arcs:
        for (TArcList::iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
                (*it)->onNodeDestruction(this);
}
 
uint8_t CHMHMapNode::serializeGetVersion() const { return 0; }
void CHMHMapNode::serializeTo(mrpt::serialization::CArchive& out) const
{
        out << m_ID << m_label;
        out << m_nodeType;
        out << m_annotations;
        out << m_hypotheses;
}
 
void CHMHMapNode::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 0:
                {
                        in >> m_ID >> m_label >> m_nodeType >> m_annotations >>
                                m_hypotheses;
 
                        // It's not necessary since at ::Create this is already done
                        // (but...check!)
                        // if (m_parent.get())
                        //      m_parent->onNodeAddition(this);
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
void CHMHMapNode::onArcDestruction(CHMHMapArc* arc)
{
        MRPT_START
 
        // Important note: We cannot create a temporary smart pointer here, since
        //  it will lead to an infinity recursion!  (BUGFIX, JLBC SEP-2009)
 
        // Check if arc is from/to this node:
        if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
        {
                // Remove from the list:
                TArcList::iterator it = m_arcs.find_ptr_to(arc);
                if (it != m_arcs.end()) m_arcs.erase(it);
        }
 
        MRPT_END
}
 
/*---------------------------------------------------------------
                                        onArcAddition
  ---------------------------------------------------------------*/
void CHMHMapNode::onArcAddition(const CHMHMapArc::Ptr& arc)
{
        MRPT_START
 
        // Check if arc is from/to this node:
        if (arc->m_nodeFrom == m_ID || arc->m_nodeTo == m_ID)
        {
                // Already in the list?
                TArcList::iterator it = m_arcs.find(arc);
                if (it == m_arcs.end()) m_arcs.push_back(arc);  // Add to the list:
        }
 
        MRPT_END
}
/*---------------------------------------------------------------
                                        onArcAddition
  ---------------------------------------------------------------*/
CHMHMapNode::TNodeID CHMHMapNode::getID() const { return m_ID; }
/*---------------------------------------------------------------
                                        getLevelInTheHierarchy
  ---------------------------------------------------------------*/
unsigned int CHMHMapNode::getLevelInTheHierarchy()
{
        TArcList::iterator itArc;
        unsigned int level = 0;
 
        for (itArc = m_arcs.begin(); itArc != m_arcs.end(); itArc++)
        {
                // I am a "level+1" from the level below if a "belongs" arc points to
                // me:
                if ((*itArc)->m_arcType == "Membership" &&
                        (*itArc)->m_nodeTo == this->m_ID)
                {
                        unsigned int L = m_parent->getNodeByID((*itArc)->m_nodeFrom)
                                                                 ->getLevelInTheHierarchy();
                        level = max(L + 1, level);
                }
        }
 
        return level;
}
 
/*---------------------------------------------------------------
                                        getRelatedArcsCount
  ---------------------------------------------------------------*/
unsigned int CHMHMapNode::getRelatedArcsCount()
{
        return (unsigned int)m_arcs.size();
}
 
/*---------------------------------------------------------------
                                        getArcs
  ---------------------------------------------------------------*/
void CHMHMapNode::getArcs(TArcList& out, const THypothesisID& hyp_id) const
{
        out.clear();
        for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
                if ((*it)->m_hypotheses.has(hyp_id)) out.push_back(*it);
}
 
/*---------------------------------------------------------------
                                        getArcs
  ---------------------------------------------------------------*/
void CHMHMapNode::getArcs(
        TArcList& out, const char* arcType, const THypothesisID& hyp_id) const
{
        out.clear();
        for (const auto& a : m_arcs)
                if (a->m_hypotheses.has(hyp_id) && a->m_arcType == arcType)
                        out.push_back(a);
}
 
/*---------------------------------------------------------------
                                        isNeighbor
  ---------------------------------------------------------------*/
bool CHMHMapNode::isNeighbor(
        const TNodeID& otherArea, const THypothesisID& hyp_id) const
{
        for (TArcList::const_iterator it = m_arcs.begin(); it != m_arcs.end(); ++it)
                if ((*it)->m_hypotheses.has(hyp_id) &&
                        ((*it)->m_nodeFrom == otherArea || (*it)->m_nodeTo == otherArea))
                        return true;
        return false;  // Nope
}