TUncertaintyPath_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 TUNCERTAINTYPATH_IMPL_H
#define TUNCERTAINTYPATH_IMPL_H

// Implementattion file for TUncertaintyPath struct

namespace mrpt
{
namespace graphslam
{
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath()
{
        this->clear();
}
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath(
        const mrpt::utils::TNodeID& starting_node)
{
        this->clear();
        nodes_traversed.push_back(starting_node);
}
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath(
        const mrpt::utils::TNodeID& starting_node,
        const mrpt::utils::TNodeID& ending_node, const constraint_t& edge)
{
        this->clear();
        nodes_traversed.push_back(starting_node);
        this->addToPath(ending_node, edge);
}

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

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::clear()
{
        using namespace mrpt;
        using namespace mrpt::math;
        using namespace mrpt::poses;
        using namespace std;

        // clear the vector of traversed nodes
        nodes_traversed.clear();

        // clear the relative edge
        curr_pose_pdf.mean = pose_t();
        // by default the information matrix is set to the unit matrix
        CMatrixDouble33 init_path_mat;
        init_path_mat.unit();
        // put a really large number - we are certain of this position
        init_path_mat *= 10000;  // TODO - justify this..
        curr_pose_pdf.cov_inv = init_path_mat;

        determinant_is_updated = false;
        determinant_cached = 0;

}  // end of clear

template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::isEmpty() const
{
        return *this == self_t();
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::assertIsBetweenNodeIDs(
        const mrpt::utils::TNodeID& from, const mrpt::utils::TNodeID& to) const
{
        ASSERTMSG_(
                this->getSource() == from,
                format(
                        "\nnodeID %lu is not the source of the path\n%s\n\n",
                        static_cast<unsigned long>(from), this->getAsString().c_str()));
        ASSERTMSG_(
                this->getDestination() == to,
                format(
                        "\nnodeID %lu is not the destination of the path\n%s\n\n",
                        static_cast<unsigned long>(to), this->getAsString().c_str()));
}

template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>& TUncertaintyPath<GRAPH_T>::operator+=(
        const self_t& other)
{
        using namespace mrpt;
        using namespace mrpt::math;
        using namespace mrpt::poses;
        using namespace std;

        // other path should start where this ends
        ASSERTMSG_(
                other.nodes_traversed.begin()[0] == this->nodes_traversed.rbegin()[0],
                "\"other\" instance must start from the nodeID that this "
                "TUncertaintyPath has ended.");
        ASSERTMSG_(
                other.nodes_traversed.size(),
                "\"other\" instance doesn't have an initialized list of traversed "
                "nodes");
        ASSERTMSG_(
                this->nodes_traversed.size(),
                "\"this\" instance doesn't have an initialized list of traversed "
                "nodes");

        //////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
        // cout << string(20, '-') << "Aggregating 2 paths.."
        //<< string(20, '-') << endl;
        // this->dumpToConsole(); other.dumpToConsole();
        ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<

        // aggregate the two gaussian - mean & information matrix
        this->curr_pose_pdf += other.curr_pose_pdf;

        // add the traversed nodes
        this->nodes_traversed.insert(
                this->nodes_traversed.end(), other.nodes_traversed.begin() + 1,
                other.nodes_traversed.end());

        ////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
        // cout << std::string(10, '%') << endl << "AFTER Aggregation..." << endl;
        // this->dumpToConsole();
        // cout << string(50, '-') << endl;
        // mrpt::system::pause();
        ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<

        determinant_is_updated = false;
        return *this;
}
template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::operator==(const self_t& other) const
{
        using namespace mrpt;
        using namespace mrpt::math;
        using namespace mrpt::poses;
        using namespace std;

        // check if the traversed nodes are the same as well as the
        // CPoseGaussianInfs are the same..
        return (
                this->nodes_traversed == other.nodes_traversed &&
                this->curr_pose_pdf == other.curr_pose_pdf);
}
template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::operator!=(const self_t& other) const
{
        return !(*this == other);
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::addToPath(
        const mrpt::utils::TNodeID& node, const constraint_t& edge)
{
        // update the path
        curr_pose_pdf += edge;

        // update the traversed nodes
        nodes_traversed.push_back(node);

        determinant_is_updated = false;
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& source, const std::string& section)
{
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::dumpToTextStream(
        mrpt::utils::CStream& out) const
{
        out.printf("%s\n", this->getAsString().c_str());
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::getAsString(std::string* str) const
{
        using namespace mrpt;
        using namespace mrpt::poses;
        using namespace std;
        using namespace mrpt::utils;
        using namespace mrpt::math;

        stringstream ss;
        string header_sep(30, '=');

        ss << "Path properties: " << endl;
        ss << header_sep << endl << endl;

        ss << "- CPosePDFGaussianInf: "
           << (curr_pose_pdf.isInfType() ? "TRUE" : "FALSE") << endl;
        ss << "- Nodes list: \n\t< " << getSTLContainerAsString(nodes_traversed)
           << "\b\b>" << endl;

        ss << endl;
        ss << curr_pose_pdf << endl;
        ss << endl;

        CMatrixDouble33 mat;
        if (curr_pose_pdf.isInfType())
        {
                curr_pose_pdf.getInformationMatrix(mat);
        }
        else
        {
                curr_pose_pdf.getCovariance(mat);
        }
        ss << "Determinant: " << mat.det();

        *str = ss.str();
}
template <class GRAPH_T>
std::string TUncertaintyPath<GRAPH_T>::getAsString() const
{
        std::string s;
        this->getAsString(&s);
        return s;
}

template <class GRAPH_T>
const mrpt::utils::TNodeID& TUncertaintyPath<GRAPH_T>::getSource() const
{
        return nodes_traversed.at(0);
}
template <class GRAPH_T>
const mrpt::utils::TNodeID& TUncertaintyPath<GRAPH_T>::getDestination() const
{
        return nodes_traversed.back();
}

template <class GRAPH_T>
double TUncertaintyPath<GRAPH_T>::getDeterminant()
{
        using namespace mrpt;
        using namespace mrpt::math;
        using namespace mrpt::poses;
        using namespace std;

        // if determinant is up-to-date then return the cached version...
        if (determinant_is_updated) return determinant_cached;

        // update the cached version and return it.
        CMatrixDouble33 mat;
        if (curr_pose_pdf.isInfType())
        {
                curr_pose_pdf.getInformationMatrix(mat);
        }
        else
        {
                curr_pose_pdf.getCovariance(mat);
        }
        double determinant = mat.det();

        determinant_cached = determinant;
        determinant_is_updated = true;

        return determinant;
}

template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::hasLowerUncertaintyThan(
        const self_t& other) const
{
        using namespace mrpt;
        using namespace mrpt::math;
        using namespace mrpt::poses;
        using namespace std;

        ASSERTMSG_(
                (curr_pose_pdf.isInfType() && other.curr_pose_pdf.isInfType()) ||
                        (!curr_pose_pdf.isInfType() && !other.curr_pose_pdf.isInfType()),
                mrpt::format(
                        "Constraints of given paths don't have the same "
                        "representation of uncertainty"));

        // If we are talking about information form matrices, the *higher* the
        // determinant the more confident we are.
        // if we are talking about covariances then the *lower*.
        bool has_lower = false;
        if (curr_pose_pdf.isInfType())
        {
                has_lower = this->getDeterminant() > other.getDeterminant();
        }
        else
        {
                has_lower = this->getDeterminant() < other.getDeterminant();
        }

        return has_lower;
}
}
}  // end of namespaces

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