test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/random.h>
#include <mrpt/system/filesystem.h>
#include <iostream>
#include <map>

using namespace mrpt::bayes;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace std;

double MIN_LOG_WEIG = -1.0;

unsigned int N_TESTS = 500;
int N_PARTICLES = 100;

// For batch experiment:
CVectorDouble min_log_ws;
map<string, CVectorDouble> results;

// vectorToTextFile( out_indxs, #ALGOR, true, true); /* By rows, append */

#define TEST_RESAMPLING(ALGOR)                                            \
    mrpt::system::deleteFile(#ALGOR);                                     \
    /*printf(#ALGOR);*/                                                   \
    /*printf("\n");*/                                                     \
    ERR_MEANs.clear();                                                    \
    ERR_STDs.clear();                                                     \
    for (size_t i = 0; i < N_TESTS; i++)                                  \
    {                                                                     \
        mrpt::random::getRandomGenerator().drawUniformVector(             \
            log_ws, MIN_LOG_WEIG, 0.0);                                   \
        CParticleFilterCapable::log2linearWeights(log_ws, lin_ws);        \
        CParticleFilterCapable::computeResampling(                        \
            CParticleFilter::ALGOR, log_ws, out_indxs);                   \
        hist_parts = mrpt::math::histogram(out_indxs, 0, M - 1, M, true); \
        vector<double> errs_hist = lin_ws - hist_parts;                   \
        ERR_MEANs.push_back(mrpt::math::mean(errs_hist));                 \
        ERR_STDs.push_back(mrpt::math::stddev(errs_hist));                \
    }                                                                     \
    printf("%s: ERR_MEAN %e\n", #ALGOR, mrpt::math::mean(ERR_MEANs));     \
    printf("%s: ERR_STD %f\n", #ALGOR, mrpt::math::mean(ERR_STDs));       \
    results[#ALGOR].push_back(mrpt::math::mean(ERR_STDs));

// ------------------------------------------------------
//                  TestResampling
// ------------------------------------------------------
void TestResampling()
{
    vector<double> log_ws;
    std::vector<size_t> out_indxs;

    const size_t M = N_PARTICLES;

    log_ws.resize(M);
    // vectorToTextFile( log_ws, "log_ws.txt");

    // Compute normalized linear weights:
    vector<double> lin_ws;
    vector<double> hist_parts;
    vector<double> ERR_MEANs;
    vector<double> ERR_STDs;

    // prMultinomial
    TEST_RESAMPLING(prMultinomial)
    // prResidual
    TEST_RESAMPLING(prResidual)
    // prStratified
    TEST_RESAMPLING(prStratified)
    // prSystematic
    TEST_RESAMPLING(prSystematic)
}

void TestBatch()
{
    for (double LL = -2; LL <= 2.01; LL += 0.08)
    {
        double L = pow(10.0, LL);

        min_log_ws.push_back(L);
        printf("MIN_LOG_W=%f\n", L);

        MIN_LOG_WEIG = L;
        TestResampling();
    }

    // Save results to files:
    CVectorDouble R;

    vectorToTextFile(min_log_ws, "min_log_ws.txt");

    R = results["prMultinomial"];
    vectorToTextFile(R, "prMultinomial.txt");
    R = results["prResidual"];
    vectorToTextFile(R, "prResidual.txt");
    R = results["prStratified"];
    vectorToTextFile(R, "prStratified.txt");
    R = results["prSystematic"];
    vectorToTextFile(R, "prSystematic.txt");
}

// ------------------------------------------------------
//                        MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
    try
    {
        getRandomGenerator().randomize();

        if (argc > 1) N_PARTICLES = atoi(argv[1]);

        // TestResampling();
        TestBatch();

        return 0;
    }
    catch (exception& e)
    {
        std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
    catch (...)
    {
        cerr << "Untyped excepcion!!";
        return -1;
    }
}