TUncertaintyPath_impl.h

Go to the documentation of this file.

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

// Implementattion file for TUncertaintyPath struct

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

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
    auto init_path_mat = CMatrixDouble33::Identity();

    // 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::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to) const
{
    ASSERTDEBMSG_(
        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()));
    ASSERTDEBMSG_(
        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
    ASSERTDEBMSG_(
        other.nodes_traversed.begin()[0] == this->nodes_traversed.rbegin()[0],
        "\"other\" instance must start from the nodeID that this "
        "TUncertaintyPath has ended.");
    ASSERTDEBMSG_(
        other.nodes_traversed.size(),
        "\"other\" instance doesn't have an initialized list of traversed "
        "nodes");
    ASSERTDEBMSG_(
        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::graphs::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::config::CConfigFileBase& source, const std::string& section)
{
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::dumpToTextStream(std::ostream& out) const
{
    out << mrpt::format("%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::math;
    using namespace mrpt::containers;

    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::graphs::TNodeID& TUncertaintyPath<GRAPH_T>::getSource() const
{
    return nodes_traversed.at(0);
}
template <class GRAPH_T>
const mrpt::graphs::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;

    ASSERTDEBMSG_(
        (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;
}
}  // namespace mrpt::graphslam