MonteCarloLocalization_App_unittest.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 <gtest/gtest.h>
#include <mrpt/apps/MonteCarloLocalization_App.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>
#include <iostream>

using config_changer_t = std::function<void(mrpt::config::CConfigFileBase&)>;
using post_tester_t =
    std::function<void(mrpt::apps::MonteCarloLocalization_Base&)>;

using MCL = mrpt::apps::MonteCarloLocalization_Rawlog;

void generic_pf_test(
    const std::string& ini_filename, const std::string& rawlog_filename,
    const std::string& map_filename, config_changer_t cfg_changer,
    post_tester_t post_tester)
{
    using namespace std::string_literals;

    const auto ini_fil = mrpt::UNITTEST_BASEDIR +
                         "/share/mrpt/config_files/pf-localization/"s +
                         ini_filename;
    ASSERT_FILE_EXISTS_(ini_fil);

    const auto rawlog_fil =
        mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + rawlog_filename;
    ASSERT_FILE_EXISTS_(rawlog_fil);

    const auto map_fil =
        mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + map_filename;
    ASSERT_FILE_EXISTS_(map_fil);

    try
    {
        MCL app;
        app.setMinLoggingLevel(mrpt::system::LVL_ERROR);

        const char* argv[] = {"pf-localization-slam", ini_fil.c_str(),
                              rawlog_fil.c_str()};
        const int argc = sizeof(argv) / sizeof(argv[0]);

        app.initialize(argc, argv);

        app.params.write(
            MCL::sect, "logOutput_dir",
            mrpt::system::getTempFileName() + "_dir"s);
        app.params.write(MCL::sect, "SHOW_PROGRESS_3D_REAL_TIME", false);

        app.params.write(MCL::sect, "map_file", map_fil);

        app.fill_out_estimated_path = true;
        app.allow_quit_on_esc_key = false;

#if !MRPT_HAS_OPENCV
        app.params.write(MCL::sect, "3DSceneFrequency", -1);
        app.params.write(MCL::sect, "LOG_FREQUENCY", 0);
#endif

        cfg_changer(app.params);
        app.run();

        // Check results:
        post_tester(app);
    }
    catch (const std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e);
        GTEST_FAIL();
    }
}

static bool tester_result_ok = true;

static auto tester_for_localization_demo =
    [](mrpt::apps::MonteCarloLocalization_Base& o) {
        EXPECT_EQ(o.out_estimated_path.size(), 37U);
        if (o.out_estimated_path.empty()) return;

        const mrpt::math::TPose3D p = o.out_estimated_path.rbegin()->second;
        const auto p_gt = mrpt::math::TPose3D::FromString(
            "[15.89 -10.0 0.000000 4.8 0.000000 0.000000]");

        const auto p_err = mrpt::poses::CPose3D(p_gt - p);
        const double err = mrpt::poses::Lie::SE<3>::log(p_err).norm();

        if (err < 0.5)
        {
            tester_result_ok = true;
        }
        else
        {
            tester_result_ok = false;
            std::cerr << "Final pose mismatch (will retry N times):\n"
                         "Expected: "
                      << p_gt.asString()
                      << "\n"
                         "Actual  : "
                      << p.asString() << "\n";
        }
    };

TEST(MonteCarloLocalization_Rawlog, RunForSampleDataset_2D)
{
    using namespace std::string_literals;
    for (int tries = 0; tries < 5; tries++)
    {
        generic_pf_test(
            "localization_demo.ini", "localization_demo.rawlog",
            "localization_demo.simplemap.gz",
            [](mrpt::config::CConfigFileBase& cfg) {
                // Use 2D:
                cfg.write(MCL::sect, "use_3D_poses", false);
            },
            tester_for_localization_demo);

        if (tester_result_ok) break;
    }
}

TEST(MonteCarloLocalization_Rawlog, RunForSampleDataset_3D)
{
    using namespace std::string_literals;
    for (int tries = 0; tries < 5; tries++)
    {
        generic_pf_test(
            "localization_demo.ini", "localization_demo.rawlog",
            "localization_demo.simplemap.gz",
            [](mrpt::config::CConfigFileBase& cfg) {
                // Use 3D:
                cfg.write(MCL::sect, "use_3D_poses", true);
                // 3D requires init in a fixed volume:
                cfg.write(MCL::sect, "init_PDF_mode", 1);
            },
            tester_for_localization_demo);

        if (tester_result_ok) break;
    }
}