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::CActionCollectionPtr action,
                mrpt::obs::CSensoryFramePtr observations,
                mrpt::obs::CObservationPtr 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.present()) { // FORMAT #2 - observation-only format
                m_observation_only_rawlog = true;

                if (IS_CLASS(observation, CObservationOdometry)) {

                        CObservationOdometryPtr obs_odometry =
                                static_cast<CObservationOdometryPtr>(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 */