CNodeRegistrationDecider_impl.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 |
   +------------------------------------------------------------------------+ */
#ifndef CNODEREGISTRATIONDECIDER_IMPL_H
#define CNODEREGISTRATIONDECIDER_IMPL_H

using namespace mrpt::graphslam::deciders;
using namespace std;

#include <sstream>

// Implementation of classes defined in the CNodeRegistrationDecider class
// template.
//
template <class GRAPH_T>
CNodeRegistrationDecider<GRAPH_T>::CNodeRegistrationDecider()
        : m_prev_registered_nodeID(INVALID_NODEID)
{
        using namespace mrpt::poses;

        m_init_inf_mat.unit();
        m_init_inf_mat *= 10000;
        resetPDF(&this->m_since_prev_node_PDF);
}

template <class GRAPH_T>
CNodeRegistrationDecider<GRAPH_T>::~CNodeRegistrationDecider()
{
}

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::getDescriptiveReport(
        std::string* report_str) const
{
        MRPT_START;

        stringstream ss("");
        parent_t::getDescriptiveReport(report_str);

        ss << "Node Registration Decider Strategy [NRD]: " << endl;
        *report_str += ss.str();

        MRPT_END;
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::checkRegistrationCondition()
{
        return false;
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::registerNewNodeAtEnd(
        const typename GRAPH_T::constraint_t& constraint)
{
        MRPT_START;
        using namespace mrpt::utils;
        using namespace std;

        // register the initial node if it doesn't exist.
        // Runs only once.
        if (this->m_prev_registered_nodeID == INVALID_NODEID)
        {  // root
                MRPT_LOG_WARN_STREAM("Registering root node..." << endl);
                global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
                this->addNodeAnnotsToPose(&tmp_pose);

                // make sure that this pair hasn't been registered yet.
                std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
                        this->m_graph->nodes.insert(
                                make_pair(this->m_graph->root, tmp_pose));
                ASSERTMSG_(
                        res.second, mrpt::format(
                                                        "nodeID \"%lu\" with pose \"%s\" seems to be "
                                                        "already registered.",
                                                        this->m_graph->root, tmp_pose.asString().c_str()));

                this->m_prev_registered_nodeID = this->m_graph->root;
        }

        // FROM nodeID
        TNodeID from = this->m_prev_registered_nodeID;
        // TO nodeID
        // In all cases this is going to be ONE AFTER the last registered nodeID
        TNodeID to = this->m_graph->nodeCount();

        // add the new pose.
        {
                global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
                this->addNodeAnnotsToPose(&tmp_pose);

                // make sure that this pair hasn't been registered yet.
                std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
                        this->m_graph->nodes.insert(make_pair(to, tmp_pose));
                ASSERTMSG_(
                        res.second, mrpt::format(
                                                        "nodeID \"%lu\" with pose \"%s\" seems to be "
                                                        "already registered.",
                                                        to, tmp_pose.asString().c_str()));
                this->m_graph->insertEdgeAtEnd(from, to, constraint);
        }

        m_prev_registered_nodeID = to;

        MRPT_LOG_DEBUG_STREAM(
                "Registered new node:" << endl
                                                           << "\t" << from << " => " << to << endl
                                                           << "\tEdge: "
                                                           << constraint.getMeanVal().asString());

        return true;
        MRPT_END;
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::registerNewNodeAtEnd()
{
        bool res = this->registerNewNodeAtEnd(this->m_since_prev_node_PDF);

        // reset the PDF since the last registered node position
        this->resetPDF(&m_since_prev_node_PDF);

        return res;
}

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::resetPDF(constraint_t* c)
{
        MRPT_START;
        ASSERT_(c);

        *c = constraint_t();
        ASSERT_(c->isInfType());
        c->cov_inv = this->m_init_inf_mat;

        MRPT_END;
}  // end of resetPDF

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::addNodeAnnotsToPose(
        global_pose_t* pose) const
{
}

template <class GRAPH_T>
typename GRAPH_T::global_pose_t
        CNodeRegistrationDecider<GRAPH_T>::getCurrentRobotPosEstimation() const
{
        global_pose_t pose_out;

        if (this->m_prev_registered_nodeID != INVALID_NODEID)
        {
                pose_out = this->m_graph->nodes.at(this->m_prev_registered_nodeID);
        }

        pose_out += m_since_prev_node_PDF.getMeanVal();
        return pose_out;
}

#endif /* end of include guard: CNODEREGISTRATIONDECIDER_IMPL_H */