CFixedIntervalsNRD_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 CFIXEDINTERVALSNRD_IMPL_H
#define CFIXEDINTERVALSNRD_IMPL_H

namespace mrpt
{
namespace graphslam
{
namespace deciders
{
// Ctors, Dtors
//////////////////////////////////////////////////////////////

template <class GRAPH_T>
CFixedIntervalsNRD<GRAPH_T>::CFixedIntervalsNRD()
{
        using namespace mrpt::utils;
        this->initializeLoggers("CFixedIntervalsNRD");

        this->logFmt(LVL_DEBUG, "IntervalsNRD: Initialized class object");
}
template <class GRAPH_T>
CFixedIntervalsNRD<GRAPH_T>::~CFixedIntervalsNRD()
{
}

// Member function implementations
//////////////////////////////////////////////////////////////

template <class GRAPH_T>
bool CFixedIntervalsNRD<GRAPH_T>::updateState(
        mrpt::obs::CActionCollection::Ptr action,
        mrpt::obs::CSensoryFrame::Ptr observations,
        mrpt::obs::CObservation::Ptr observation)
{
        MRPT_START;
        using namespace mrpt::obs;
        using namespace mrpt::math;
        using namespace mrpt::utils;
        using namespace mrpt::poses;

        // don't use the measurements in this implementation
        MRPT_UNUSED_PARAM(observations);

        if (observation)
        {  // FORMAT #2 - observation-only format
                m_observation_only_rawlog = true;

                if (IS_CLASS(observation, CObservationOdometry))
                {
                        CObservationOdometry::Ptr obs_odometry =
                                std::dynamic_pointer_cast<CObservationOdometry>(observation);
                        // not incremental - gives the absolute odometry reading
                        m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
                        this->logFmt(
                                LVL_DEBUG, "Current odometry-only pose: %s",
                                m_curr_odometry_only_pose.asString().c_str());

                        // I don't have any information about the covariane of the move in
                        // observation-only format
                        this->m_since_prev_node_PDF.mean =
                                m_curr_odometry_only_pose - m_last_odometry_only_pose;
                }
        }
        else
        {  // FORMAT #1 - action-observation format
                m_observation_only_rawlog = false;

                mrpt::poses::CPose3DPDFGaussian move_pdf;
                bool found = action->getFirstMovementEstimation(move_pdf);
                if (found)
                {
                        // update the relative PDF of the path since the LAST node was
                        // inserted
                        constraint_t incr_constraint;
                        incr_constraint.copyFrom(move_pdf);
                        this->m_since_prev_node_PDF += incr_constraint;
                }
        }  // ELSE - FORMAT #1

        bool registered = this->checkRegistrationCondition();

        if (registered)
        {
                if (m_observation_only_rawlog)
                {
                        // keep track of the odometry-only pose_t at the last inserted graph
                        // node
                        m_last_odometry_only_pose = m_curr_odometry_only_pose;
                }
        }

        return registered;

        MRPT_END;
}  // end of updateState

template <class GRAPH_T>
bool CFixedIntervalsNRD<GRAPH_T>::checkRegistrationCondition()
{
        MRPT_START;

        // check that a node has already been registered - if not, default to
        // (0,0,0)
        pose_t last_pose_inserted =
                this->m_prev_registered_nodeID != INVALID_NODEID
                        ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
                        : pose_t();

        // odometry criterion
        bool registered = false;

        if (this->checkRegistrationCondition(
                        last_pose_inserted, this->getCurrentRobotPosEstimation()))
        {
                registered = this->registerNewNodeAtEnd();
        }

        return registered;
        MRPT_END;
}  // end of checkRegistrationCondition

template <class GRAPH_T>
bool CFixedIntervalsNRD<GRAPH_T>::checkRegistrationCondition(
        const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
{
        using namespace mrpt::math;

        bool res = false;
        if ((p1.distanceTo(p2) > params.registration_max_distance) ||
                (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
        {
                res = true;
        }

        return res;
}  // end of checkRegistrationCondition2D

template <class GRAPH_T>
bool CFixedIntervalsNRD<GRAPH_T>::checkRegistrationCondition(
        const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
{
        using namespace mrpt::math;

        bool res = false;
        if ((p1.distanceTo(p2) > params.registration_max_distance) ||
                (fabs(wrapToPi(p1.roll() - p2.roll())) >
                 params.registration_max_angle) ||
                (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
                 params.registration_max_angle) ||
                (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
        {
                res = true;
        }

        return res;
}  // end of checkRegistrationCondition3D

template <class GRAPH_T>
void CFixedIntervalsNRD<GRAPH_T>::loadParams(const std::string& source_fname)
{
        MRPT_START;
        using namespace mrpt::utils;
        parent_t::loadParams(source_fname);

        params.loadFromConfigFileName(
                source_fname, "NodeRegistrationDeciderParameters");

        // set the logging level if given by the user
        CConfigFile source(source_fname);
        int min_verbosity_level = source.read_int(
                "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
        this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));

        this->logFmt(LVL_DEBUG, "Successfully loaded parameters.");
        MRPT_END;
}

template <class GRAPH_T>
void CFixedIntervalsNRD<GRAPH_T>::printParams() const
{
        MRPT_START;
        parent_t::printParams();
        params.dumpToConsole();

        MRPT_END;
}

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

        const std::string report_sep(2, '\n');
        const std::string header_sep(80, '#');

        // Report on graph
        stringstream class_props_ss;
        class_props_ss << "Strategy: "
                                   << "Fixed Odometry-based Intervals" << std::endl;
        class_props_ss << header_sep << std::endl;

        // time and output logging
        const std::string time_res = this->m_time_logger.getStatsAsText();
        const std::string output_res = this->getLogAsString();

        // merge the individual reports
        report_str->clear();
        parent_t::getDescriptiveReport(report_str);

        *report_str += class_props_ss.str();
        *report_str += report_sep;

        // configuration parameters
        *report_str += params.getAsString();
        *report_str += report_sep;

        // loggers results
        *report_str += time_res;
        *report_str += report_sep;

        *report_str += output_res;
        *report_str += report_sep;

        MRPT_END;
}

// TParams
//////////////////////////////////////////////////////////////
template <class GRAPH_T>
CFixedIntervalsNRD<GRAPH_T>::TParams::TParams()
{
}
template <class GRAPH_T>
CFixedIntervalsNRD<GRAPH_T>::TParams::~TParams()
{
}
template <class GRAPH_T>
void CFixedIntervalsNRD<GRAPH_T>::TParams::dumpToTextStream(
        mrpt::utils::CStream& out) const
{
        MRPT_START;
        out.printf("%s", this->getAsString().c_str());
        MRPT_END;
}
template <class GRAPH_T>
void CFixedIntervalsNRD<GRAPH_T>::TParams::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& source, const std::string& section)
{
        MRPT_START;
        using namespace mrpt::math;
        using namespace mrpt::utils;

        registration_max_distance = source.read_double(
                section, "registration_max_distance", 0.5 /* meter */, false);
        registration_max_angle = source.read_double(
                section, "registration_max_angle", 60 /* degrees */, false);
        registration_max_angle = DEG2RAD(registration_max_angle);

        MRPT_END;
}

template <class GRAPH_T>
void CFixedIntervalsNRD<GRAPH_T>::TParams::getAsString(
        std::string* params_out) const
{
        MRPT_START;
        using namespace mrpt::math;
        using namespace mrpt::utils;

        double max_angle_deg = RAD2DEG(registration_max_angle);
        params_out->clear();

        *params_out +=
                "------------------[ Fixed Intervals Node Registration "
                "]------------------\n";
        *params_out += mrpt::format(
                "Max distance for registration = %.2f m\n", registration_max_distance);
        *params_out += mrpt::format(
                "Max angle for registration    = %.2f deg\n", max_angle_deg);

        MRPT_END;
}
template <class GRAPH_T>
std::string CFixedIntervalsNRD<GRAPH_T>::TParams::getAsString() const
{
        MRPT_START;

        std::string str;
        this->getAsString(&str);
        return str;

        MRPT_END;
}
}
}
}  // end of namespaces

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