TUserOptionsChecker_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    |
   +---------------------------------------------------------------------------+ */

// implementation of the TUserOptionsChecker class template methods
namespace mrpt { namespace graphslam { namespace apps {

template<class GRAPH_t>
TUserOptionsChecker<GRAPH_t>::TUserOptionsChecker():
        sep_header(40, '='),
        sep_subheader(20, '-')
{ }

template<class GRAPH_t>
TUserOptionsChecker<GRAPH_t>::~TUserOptionsChecker(){
        using namespace std;

        // release the instances holding the descriptions of the available
        // deciders/optimizers
        for (vector<TRegistrationDeciderProps*>::iterator
                        it = regs_descriptions.begin();
                        it != regs_descriptions.end();
                        ++it) {
                delete *it;
        }
        for (vector<TOptimizerProps*>::iterator
                        it = optimizers_descriptions.begin();
                        it != optimizers_descriptions.end();
                        ++it) {
                delete *it;
        }

}

template<class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::createDeciderOptimizerMappings() {
        MRPT_START;

        using namespace mrpt::graphslam::deciders;
        using namespace mrpt::graphslam::optimizers;
        using namespace mrpt::graphs;
        using namespace mrpt::poses;

        // node registration deciders
        node_regs_map["CEmptyNRD"] =
                &createNodeRegistrationDecider<CEmptyNRD<GRAPH_t> >;
        node_regs_map["CFixedIntervalsNRD"] =
                        &createNodeRegistrationDecider<CFixedIntervalsNRD<GRAPH_t> >;
        // edge registration deciders
        edge_regs_map["CEmptyERD"] =
                &createEdgeRegistrationDecider<CEmptyERD<GRAPH_t> >;
        // optimizers
        optimizers_map["CLevMarqGSO"] =
                &createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t> >;
        optimizers_map["CEmptyGSO"] =
                &createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t> >;

        // create the decider optimizer, specific to the GRAPH_T template type
        this->_createDeciderOptimizerMappings();


        MRPT_END;
}

template<class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::_createDeciderOptimizerMappings() {
}

// deciders/optpimizers specific to the 2D SLAM cases
template<>
inline void TUserOptionsChecker<mrpt::graphs::CNetworkOfPoses2DInf>::_createDeciderOptimizerMappings() {
        using namespace mrpt::graphs;

        node_regs_map["CICPCriteriaNRD"] =
                &createNodeRegistrationDecider<CICPCriteriaNRD<CNetworkOfPoses2DInf> >;
        edge_regs_map["CICPCriteriaERD"] =
                        &createEdgeRegistrationDecider<CICPCriteriaERD<CNetworkOfPoses2DInf> >;
        edge_regs_map["CLoopCloserERD"] =
                        &createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf> >;

}
template<>
inline void TUserOptionsChecker<mrpt::graphs::CNetworkOfPoses2DInf_NA>::_createDeciderOptimizerMappings() {
        using namespace mrpt::graphs;

        node_regs_map["CICPCriteriaNRD"] =
                &createNodeRegistrationDecider<CICPCriteriaNRD<CNetworkOfPoses2DInf_NA> >;
        edge_regs_map["CICPCriteriaERD"] =
                        &createEdgeRegistrationDecider<CICPCriteriaERD<CNetworkOfPoses2DInf_NA> >;
        edge_regs_map["CLoopCloserERD"] =
                        &createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf_NA> >;
}

// deciders/optpimizers specific to the 3D SLAM cases
template<>
inline void TUserOptionsChecker<mrpt::graphs::CNetworkOfPoses3DInf>::_createDeciderOptimizerMappings() {

}

template<class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::dumpRegistrarsToConsole(
                std::string reg_type/*="all"*/) const {
        MRPT_START;
        using namespace std;
        using  namespace mrpt;

        ASSERTMSG_((system::strCmpI(reg_type, "node") ||
                                system::strCmpI(reg_type, "edge") ||
                                system::strCmpI(reg_type, "all")),
                        format("Registrar string '%s' does not match a known registrar name.\n"
                                "Specify 'node' 'edge' or 'all'", reg_type.c_str()));


        if ( system::strCmpI(reg_type, "node") || system::strCmpI(reg_type, "edge") ) {

                cout << endl << "Available " << system::upperCase(reg_type) << " Registration Deciders: " << endl;
                cout << sep_header << endl;

                for (vector<TRegistrationDeciderProps*>::const_iterator
                                dec_it = regs_descriptions.begin();
                                dec_it != regs_descriptions.end(); ++dec_it) {
                        TRegistrationDeciderProps* dec = *dec_it;
                        if ( system::strCmpI(dec->type, reg_type) ) {
                                cout << dec->name << endl;
                                cout << sep_subheader << endl;
                                cout << "\t- " << "Description: " <<  dec->description << endl;
                                cout << "\t- " << "Rawlog Format: " <<  dec->rawlog_format << endl;
                                cout << "\t- " << "Observations that can be used: " << endl;
                                cout << "\t- " << "Multi-robot SLAM capable decider: " <<
                                        (dec->is_mr_slam_class? "TRUE": "FALSE") << endl;
                                cout << "\t- " << "SLAM Type: " << endl;
                                if (dec->is_slam_2d) {
                                        cout << "\t\t+ " << "2D" << endl;
                                }
                                if (dec->is_slam_3d) {
                                        cout << "\t\t+ " << "3D" << endl;
                                }
                                cout << endl;
                                for (vector<string>::const_iterator obs_it = dec->observations_used.begin();
                                                obs_it != dec->observations_used.end(); ++obs_it) {
                                        cout << "\t\t+ " << *obs_it << endl;
                                }
                        }
                }
        }
        else { // print both
                dumpRegistrarsToConsole("node");
                dumpRegistrarsToConsole("edge");
        }

        MRPT_END;
}

template<class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::dumpOptimizersToConsole() const {
        MRPT_START;

        using namespace std;

        cout << endl << "Available GraphSlam Optimizer classes: " << endl;
        cout << sep_header << endl;

        for (vector<TOptimizerProps*>::const_iterator opt_it = optimizers_descriptions.begin();
                        opt_it != optimizers_descriptions.end(); ++opt_it) {
                TOptimizerProps* opt = *opt_it;
                cout << opt->name << endl;
                cout << sep_subheader << endl;
                cout << "\t- " << "Description: " <<  opt->description << endl;

                cout << "\t- " << "Multi-robot SLAM capable optimizer: " <<
                        (opt->is_mr_slam_class? "TRUE": "FALSE");
                cout << "\t- " << "SLAM Type: " << endl;
                if (opt->is_slam_2d) {
                        cout << "\t\t+ " << "2D" << endl;
                }
                if (opt->is_slam_3d) {
                        cout << "\t\t+ " << "3D" << endl;
                }
        }
        MRPT_END;
}

template<class GRAPH_t>
bool TUserOptionsChecker<GRAPH_t>::checkRegistrationDeciderExists(
                std::string given_reg,
                std::string reg_type) const {
        MRPT_START;

        using namespace std;
        using  namespace mrpt;
        using namespace mrpt::poses;

        ASSERTMSG_((system::strCmpI(reg_type, "node") ||
                        system::strCmpI(reg_type, "edge")),
                        format("Registrar string \"%s\" does not match a known registrar name.\n"
                                "Specify 'node' or 'edge' ", reg_type.c_str()));
        bool found = false;

        for (vector<TRegistrationDeciderProps*>::const_iterator
                        dec_it = regs_descriptions.begin();
                        dec_it != regs_descriptions.end(); ++dec_it) {
                TRegistrationDeciderProps* dec = *dec_it;

                // TODO - check this
                // if decider is not suitable for the selected SLAM type, ignore.
                pose_t p;
                if ((!dec->is_slam_2d && IS_CLASS(&p, CPose2D)) ||
                                (!dec->is_slam_3d && IS_CLASS(&p, CPose3D))) {
                        continue;
                }

                if (system::strCmpI(dec->type, reg_type)) {
                        if (system::strCmpI(dec->name, given_reg)) {
                                found = true;
                                return found;
                        }
                }
        }

        return found;
        MRPT_END;
}

template<class GRAPH_t>
bool TUserOptionsChecker<GRAPH_t>::checkOptimizerExists(std::string given_opt) const {
        MRPT_START;
        using namespace std;
        using  namespace mrpt;
        using namespace mrpt::poses;

        bool found = false;

        for (vector<TOptimizerProps*>::const_iterator
                        opt_it = optimizers_descriptions.begin();
                        opt_it != optimizers_descriptions.end(); ++opt_it) {
                TOptimizerProps* opt = *opt_it;

                // if optimizer is not suitable for the selected SLAM type, ignore.
                pose_t p;
                if ((!opt->is_slam_2d && IS_CLASS(&p, CPose2D)) &&
                                (!opt->is_slam_3d && IS_CLASS(&p, CPose3D))) {
                        continue;
                }

                if ( system::strCmpI(opt->name, given_opt) ) {
                        found = true;
                        return found;
                }
        }

        return found;
        MRPT_END;
}

template<class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::populateDeciderOptimizerProperties() {
        MRPT_START;

        // reset the vectors - in case they contain any elements
        regs_descriptions.clear();
        optimizers_descriptions.clear();

        // registering the available deciders
        { // CFixedIntervalsNRD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CFixedIntervalsNRD";
                dec->description = "Register a new node if the distance from the previous node surpasses a predefined distance threshold. Uses odometry information for estimating the robot movement";
                dec->type = "Node";
                dec->rawlog_format = "Both";
                dec->observations_used.push_back("CActionRobotMovement2D - Format #1");
                dec->observations_used.push_back("CObservationOdometry - Format #2");
                dec->is_slam_2d = true;
                dec->is_slam_3d = true;

                regs_descriptions.push_back(dec);
        }
        { // CICPCriteriaNRD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CICPCriteriaNRD";
                dec->description = "Register a new node if the distance from the previous node surpasses a predefined distance threshold. Uses 2D/3D RangeScans alignment for estimating the robot movement";
                dec->type = "Node";
                dec->rawlog_format = "#2 - Observation-only";
                dec->observations_used.push_back("CObservation2DRangeScan - Format #2");
                dec->observations_used.push_back("CObservation3DRangeScan - Format #2");
                dec->is_slam_2d = true;

                regs_descriptions.push_back(dec);
        }
        { // CEmptyNRD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CEmptyNRD";
                dec->description = "Empty Decider - does nothing when its class methods are called";
                dec->type = "Node";
                dec->rawlog_format = "Both";
                dec->is_mr_slam_class = true;
                dec->is_slam_2d = true;
                dec->is_slam_3d = true;

                regs_descriptions.push_back(dec);
        }
        { // CICPCriteriaERD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CICPCriteriaERD";
                dec->description = "Register a new edge by aligning the provided 2D/3D RangeScans of 2 nodes. Uses the goodness of the ICP Alignment as the criterium for adding a new edge";
                dec->type = "Edge";
                dec->rawlog_format = "Both";
                dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2");
                dec->observations_used.push_back("CObservation3DRangeScan - Format #2");
                dec->is_slam_2d = true;

                regs_descriptions.push_back(dec);
        }
        { // CEmptyERD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CEmptyERD";
                dec->description = "Empty Decider - does nothing when its class methods are called";
                dec->type = "Edge";
                dec->rawlog_format = "Both";
                dec->is_mr_slam_class = true;
                dec->is_slam_2d = true;
                dec->is_slam_3d = true;

                regs_descriptions.push_back(dec);
        }
        { // CLoopCloserERD
                TRegistrationDeciderProps* dec = new TRegistrationDeciderProps;
                dec->name = "CLoopCloserERD";
                dec->description = "Partition the map and register *sets* of edges based on the Pairwise consistency matrix of each set.";
                dec->type = "Edge";
                dec->rawlog_format = "Both";
                dec->observations_used.push_back("CObservation2DRangeScan - Format #1, #2");
                dec->is_slam_2d = true;

                regs_descriptions.push_back(dec);
        }

        // registering the available optimizers
        { // CEmptyGSO
                TOptimizerProps* opt = new TOptimizerProps;
                opt->name = "CEmptyGSO";
                opt->description = "Empty Optimizer - does nothing when its class methods are called";
                opt->is_mr_slam_class = true;
                opt->is_slam_2d = true;
                opt->is_slam_3d = true;

                optimizers_descriptions.push_back(opt);
        }

        { // CLevMarqGSO
                TOptimizerProps* opt = new TOptimizerProps;
                opt->name = "CLevMarqGSO";
                opt->description = "Levenberg-Marqurdt non-linear graphSLAM solver";
                opt->is_mr_slam_class = true;
                opt->is_slam_2d = true;
                opt->is_slam_3d = true;

                optimizers_descriptions.push_back(opt);
        }

        MRPT_END
}



} } } // END OF NAMESPACES