heuristicParams.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 |
   +------------------------------------------------------------------------+ */

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */

#ifndef __HEURISTICPARAMS_H
#define __HEURISTICPARAMS_H

#include <mrpt/utils/CConfigFile.h>
#include <mrpt/system/filesystem.h>

namespace mrpt
{
namespace pbmap
{
/** A class used to define the heuristic parameters and thresholds used to match
 * sets of planes.
 *  These parameters are loaded from a configuration file.
 *
 * \ingroup mrpt_pbmap_grp
 */
struct config_heuristics
{
        // [global]
        /*! Global parameter to indicate the path to previous PbMaps used for place
         * recognition*/
        std::string path_prev_pbmaps;

        /*! Global parameter indicating the minimum required plane matches for place
         * recognition*/
        unsigned min_planes_recognition;  // Minimum number of planes to accept a
        // match between two neighborhoods of
        // planes
        bool use_structure;  // Use inferred knowledge
        bool use_completeness;  // Use inferred knowledge

        // [unary]
        /*! Unary constraint threshold to limit the difference of distances between
         * two pairs camera-plane (to be used in odometry, i.e. when the two cameras
         * are nearby */
        float dist_d;
        float angle;

        float color_threshold;
        float intensity_threshold;
        float hue_threshold;
        //    float colorDev_threshold;
        float area_threshold, area_threshold_inv;
        float area_full_threshold, area_full_threshold_inv;
        float area_half_threshold,
                area_half_threshold_inv;  // Constraint with "half confidence": one
        // plana is fully observed
        float elongation_threshold, elongation_threshold_inv;  // Unary constraint:
        int graph_mode;  // This var selects the condition to create edges in the
        // graph, either proximity of planar patches or
        // co-visibility in a single frame

        // [binary]
        /*! Binary constraint threshold to limit the difference of distances between
         * the centers of two pair of planes. This threashold should be loose if the
         * plane boundaries are not known, which is the most common situation */
        float dist_threshold, dist_threshold_inv;
        float angle_threshold;  // Binary constraint: the angle between the normals
        // of two pair of planes
        float height_threshold;  // Binary constraint: Height from one plane wrt a
        // neighbor (in meters)
        float height_threshold_parallel;  // Binary constraint: Height from one
        // plane wrt a neighbor (in meters) for
        // complete marked planes
        float cos_angle_parallel;

        /*! Load the PbMap registration thresholds from an .ini file */
        void load_params(const std::string& config_file_name)
        {
                ASSERT_FILE_EXISTS_(config_file_name)
                mrpt::utils::CConfigFile config_file(config_file_name);

                // global
                path_prev_pbmaps =
                        config_file.read_string("global", "path_prev_pbmaps", "", false);
                //  std::cout << path_prev_pbmaps << std::endl;
                use_structure = config_file.read_bool("global", "use_structure", true);
                use_completeness =
                        config_file.read_bool("global", "use_completeness", true);
                min_planes_recognition =
                        config_file.read_int("global", "min_planes_recognition", 4);
                graph_mode = config_file.read_int("global", "graph_mode", 1);

                // Unary constraints
                dist_d = config_file.read_float("unary", "dist_d", 0.5);
                angle = config_file.read_float("unary", "angle", 50);
                angle = cos(angle * 3.14159 / 180);

                color_threshold =
                        config_file.read_float("unary", "color_threshold", 0.09);
                intensity_threshold =
                        config_file.read_float("unary", "intensity_threshold", 175.0);
                hue_threshold = config_file.read_float("unary", "hue_threshold", 0.3);
                //      colorDev_threshold =
                //      config_file.read_float("unary","colorDev_threshold",0.005);
                area_threshold = config_file.read_float("unary", "area_threshold", 3.0);
                area_threshold_inv = 1 / area_threshold;
                area_full_threshold =
                        config_file.read_float("unary", "area_full_threshold", 1.6);
                area_full_threshold_inv = 1 / area_full_threshold;
                area_half_threshold =
                        config_file.read_float("unary", "area_half_threshold", 2.0);
                area_half_threshold_inv = 1 / area_half_threshold;
                elongation_threshold =
                        config_file.read_float("unary", "elongation_threshold", 2.9);
                elongation_threshold_inv = 1 / elongation_threshold;

                // Binary constraints
                dist_threshold =
                        config_file.read_float("binary", "dist_threshold", 2.0);
                dist_threshold_inv = 1 / dist_threshold;
                angle_threshold =
                        config_file.read_float("binary", "angle_threshold", 7.0);
                height_threshold =
                        config_file.read_float("binary", "height_threshold", 0.2);
                height_threshold_parallel =
                        config_file.read_float("binary", "height_threshold_parallel", 0.2);
                cos_angle_parallel =
                        config_file.read_float("binary", "cos_angle_parallel", 0.985);
        };

        /*! Print the threshold for registration */
        void print_params()
        {
                using std::cout;
                using std::endl;

                cout << "Unary thresholds:\n";
                cout << "dist_d " << dist_d << endl;
                cout << "angle " << angle << endl;
                cout << "color_threshold " << color_threshold << endl;
                cout << "intensity_threshold " << intensity_threshold << endl;
                cout << "hue_threshold " << hue_threshold << endl;
                cout << "area_threshold " << area_threshold << endl;
                cout << "elongation_threshold " << elongation_threshold << endl;

                cout << "Binary thresholds:\n";
                cout << "angle_threshold " << angle_threshold << endl;
                cout << "angle " << angle << endl;
                cout << "height_threshold " << height_threshold << endl;
        };
};
}
}  // End of namespaces

#endif