CMonteCarloLocalization2D_unittest.cpp

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

#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CConfigFile.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/random.h>
#include <gtest/gtest.h>

#include <mrpt/bayes/CParticleFilter_impl.h>

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace std;

// Defined in tests/test_main.cpp
namespace mrpt
{
namespace utils
{
extern std::string MRPT_GLOBAL_UNITTEST_SRC_DIR;
}
}  // namespace mrpt

void run_test_pf_localization(CPose2D& meanPose, CMatrixDouble33& cov)
{
        // ------------------------------------------------------
        // The code below is a simplification of the program "pf-localization"
        // ------------------------------------------------------
        const string ini_fil =
                MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/montecarlo_test1.ini");
        if (!mrpt::system::fileExists(ini_fil))
        {
                cerr << "WARNING: Skipping test due to missing file: " << ini_fil
                         << "\n";
                return;
        }

        CConfigFile iniFile(ini_fil);
        vector_int particles_count;  // Number of initial particles (if size>1, run
        // the experiments N times)

        // Load configuration:
        // -----------------------------------------
        string iniSectionName("LocalizationExperiment");

        // Mandatory entries:
        iniFile.read_vector(
                iniSectionName, "particles_count", vector_int(1, 0), particles_count,
                /*Fail if not found*/ true);
        string RAWLOG_FILE = iniFile.read_string(
                iniSectionName, "rawlog_file", "", /*Fail if not found*/ true);

        RAWLOG_FILE = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/") + RAWLOG_FILE;

        // Non-mandatory entries:
        string MAP_FILE = iniFile.read_string(iniSectionName, "map_file", "");

        MAP_FILE = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/") + MAP_FILE;

        size_t rawlog_offset = iniFile.read_int(iniSectionName, "rawlog_offset", 0);
        int NUM_REPS = iniFile.read_int(iniSectionName, "experimentRepetitions", 1);

        // PF-algorithm Options:
        // ---------------------------
        CParticleFilter::TParticleFilterOptions pfOptions;
        pfOptions.loadFromConfigFile(iniFile, "PF_options");

        // PDF Options:
        // ------------------
        TMonteCarloLocalizationParams pdfPredictionOptions;
        pdfPredictionOptions.KLD_params.loadFromConfigFile(iniFile, "KLD_options");

        // Metric map options:
        // -----------------------------
        TSetOfMetricMapInitializers mapList;
        mapList.loadFromConfigFile(iniFile, "MetricMap");

        // --------------------------------------------------------------------
        //                                              EXPERIMENT PREPARATION
        // --------------------------------------------------------------------
        CTicTac tictac, tictacGlobal;
        CSimpleMap simpleMap;
        CRawlog rawlog;
        size_t rawlogEntry, rawlogEntries;
        CParticleFilter::TParticleFilterStats PF_stats;

        // Load the set of metric maps to consider in the experiments:
        CMultiMetricMap metricMap;
        metricMap.setListOfMaps(&mapList);

        getRandomGenerator().randomize();

        // Load the map (if any):
        // -------------------------
        if (MAP_FILE.size())
        {
                ASSERT_(fileExists(MAP_FILE));

                // Detect file extension:
                // -----------------------------
                string mapExt = lowerCase(extractFileExtension(
                        MAP_FILE, true));  // Ignore possible .gz extensions

                if (!mapExt.compare("simplemap"))
                {
                        // It's a ".simplemap":
                        // -------------------------
                        CFileGZInputStream(MAP_FILE.c_str()) >> simpleMap;

                        ASSERT_(simpleMap.size() > 0);

                        // Build metric map:
                        // ------------------------------
                        metricMap.loadFromProbabilisticPosesAndObservations(simpleMap);
                }
                else if (!mapExt.compare("gridmap"))
                {
                        // It's a ".gridmap":
                        // -------------------------
                        ASSERT_(metricMap.m_gridMaps.size() == 1);
                        CFileGZInputStream(MAP_FILE) >> (*metricMap.m_gridMaps[0]);
                }
                else
                {
                        THROW_EXCEPTION_FMT(
                                "Map file has unknown extension: '%s'", mapExt.c_str());
                }
        }

        // --------------------------
        // Load the rawlog:
        // --------------------------
        rawlog.loadFromRawLogFile(RAWLOG_FILE);
        rawlogEntries = rawlog.size();

        for (vector_int::iterator itNum = particles_count.begin();
                 itNum != particles_count.end(); ++itNum)
        {
                int PARTICLE_COUNT = *itNum;

                // Global stats for all the experiment loops:
                vector<double> covergenceErrors;
                covergenceErrors.reserve(NUM_REPS);
                // --------------------------------------------------------------------
                //                                      EXPERIMENT REPETITIONS LOOP
                // --------------------------------------------------------------------
                tictacGlobal.Tic();
                for (int repetition = 0; repetition < NUM_REPS; repetition++)
                {
                        int M = PARTICLE_COUNT;
                        CMonteCarloLocalization2D pdf(M);

                        // PDF Options:
                        pdf.options = pdfPredictionOptions;

                        pdf.options.metricMap = &metricMap;

                        // Create the PF object:
                        CParticleFilter PF;
                        PF.m_options = pfOptions;

                        size_t step = 0;
                        rawlogEntry = 0;

                        // Initialize the PDF:
                        // -----------------------------
                        tictac.Tic();
                        if (!iniFile.read_bool(
                                        iniSectionName, "init_PDF_mode", false,
                                        /*Fail if not found*/ true))
                                pdf.resetUniformFreeSpace(
                                        metricMap.m_gridMaps[0].get(), 0.7f, PARTICLE_COUNT,
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_min_x", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_max_x", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_min_y", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_max_y", 0, true),
                                        DEG2RAD(iniFile.read_float(
                                                iniSectionName, "init_PDF_min_phi_deg", -180)),
                                        DEG2RAD(iniFile.read_float(
                                                iniSectionName, "init_PDF_max_phi_deg", 180)));
                        else
                                pdf.m_poseParticles.resetUniform(
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_min_x", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_max_x", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_min_y", 0, true),
                                        iniFile.read_float(
                                                iniSectionName, "init_PDF_max_y", 0, true),
                                        DEG2RAD(iniFile.read_float(
                                                iniSectionName, "init_PDF_min_phi_deg", -180)),
                                        DEG2RAD(iniFile.read_float(
                                                iniSectionName, "init_PDF_max_phi_deg", 180)),
                                        PARTICLE_COUNT);

                        // -----------------------------
                        //              Particle filter
                        // -----------------------------
                        CActionCollection::Ptr action;
                        CSensoryFrame::Ptr observations;
                        bool end = false;

                        // TTimeStamp cur_obs_timestamp;

                        while (rawlogEntry < (rawlogEntries - 1) && !end)
                        {
                                // Load pose change from the rawlog:
                                // ----------------------------------------
                                if (!rawlog.getActionObservationPair(
                                                action, observations, rawlogEntry))
                                        THROW_EXCEPTION("End of rawlog");

                                CPose2D expectedPose;  // Ground truth

                                //                              if (observations->size()>0)
                                //                                      cur_obs_timestamp =
                                // observations->getObservationByIndex(0)->timestamp;

                                if (step >= rawlog_offset)
                                {
                                        // Do not execute the PF at "step=0", to let the initial PDF
                                        // to be
                                        //   reflected in the logs.
                                        if (step > rawlog_offset)
                                        {
                                                // ----------------------------------------
                                                // RUN ONE STEP OF THE PARTICLE FILTER:
                                                // ----------------------------------------
                                                tictac.Tic();

                                                pdf.executeOn(
                                                        PF,
                                                        action.get(),  // Action
                                                        observations.get(),  // Obs.
                                                        &PF_stats,  // Output statistics
                                                        pfOptions.PF_algorithm
                                                        );
                                        }

                                        pdf.m_poseParticles.getCovarianceAndMean(cov, meanPose);
                                        // cout << meanPose << " cov trace: "  << cov.trace() <<
                                        // endl;

                                }  // end if rawlog_offset

                                step++;

                        };  // while rawlogEntries
                }  // for repetitions
        }  // end of loop for different # of particles
}

// TEST =================
TEST(MonteCarlo2D, RunSampleDataset)
{
#if MRPT_IS_BIG_ENDIAN
        MRPT_TODO("Debug this issue in big endian platforms")
        return;  // Skip this test for now
#endif

        // Actual ending point:
        const CPose2D GT_endpose(15.904, -10.010, DEG2RAD(4.93));

        // Placeholder for results:
        CPose2D meanPose;
        CMatrixDouble33 cov;

        // Invoke test:
        // Give it 3 opportunities, since it might fail once for bad luck, or even
        // twice in an extreme bad luck:
        for (int op = 0; op < 3; op++)
        {
                run_test_pf_localization(meanPose, cov);

                const double final_pf_cov_trace = cov.trace();
                const CPose2D final_pf_pose = meanPose;

                bool pass1 = (final_pf_pose - GT_endpose).norm() < 0.10;
                bool pass2 = final_pf_cov_trace < 0.01;

                if (pass1 && pass2) return;  // OK!

                // else: give it another try...
                cout << "\n*Warning: Test failed. Will give it another chance, since "
                                "after all it's nondeterministic!\n";
        }

        FAIL() << "Failed to converge after 3 opportunities!!" << endl;
}