KFSLAMApp_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/KFSLAMApp.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>
#include <functional>

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

void generic_kf_slam_test(
    const std::string& ini_filename, const std::string& rawlog_filename,
    config_changer_t cfg_changer)
{
    using namespace std::string_literals;

    const auto ini_fil = mrpt::UNITTEST_BASEDIR +
                         "/share/mrpt/config_files/kf-slam/"s + ini_filename;
    EXPECT_TRUE(mrpt::system::fileExists(ini_fil));

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

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

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

        app.initialize(argc, argv);

        // Output 3D scenes to a temporary directory:
        app.params.write(
            "MappingApplication", "logOutput_dir",
            mrpt::system::getTempFileName() + "_dir"s);

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

        // Check results:
        EXPECT_LT(app.loc_error_wrt_gt, 0.1);
    }
    catch (const std::exception& e)
    {
        std::cerr << mrpt::exception_to_str(e);
        GTEST_FAIL();
    }
}

TEST(KFSLAMApp, EKF_SLAM_3D)
{
    generic_kf_slam_test(
        "EKF-SLAM_6D_test.ini", "kf-slam_6D_demo.rawlog",
        [](mrpt::config::CConfigFileBase& c) {
            using namespace std::string_literals;
            c.write("MappingApplication", "SHOW_3D_LIVE", false);
            c.write("MappingApplication", "SAVE_3D_SCENES", true);
        });
}

TEST(KFSLAMApp, EKF_SLAM_2D)
{
    generic_kf_slam_test(
        "EKF-SLAM_test_2d.ini", "kf-slam_demo.rawlog",
        [](mrpt::config::CConfigFileBase& c) {
            using namespace std::string_literals;
            c.write("MappingApplication", "SHOW_3D_LIVE", false);
            c.write("MappingApplication", "SAVE_3D_SCENES", true);
        });
}

TEST(KFSLAMApp, EKF_SLAM_3D_data_assoc_JCBB_Maha)
{
    generic_kf_slam_test(
        "EKF-SLAM_6D_test_datassoc.ini", "kf-slam_6D_demo_DA.rawlog",
        [](mrpt::config::CConfigFileBase& c) {
            using namespace std::string_literals;
            c.write("RangeBearingKFSLAM", "data_assoc_method", "assocJCBB");
            c.write("RangeBearingKFSLAM", "data_assoc_metric", "metricMaha");

            c.write("MappingApplication", "SHOW_3D_LIVE", false);
            c.write("MappingApplication", "SAVE_3D_SCENES", false);
        });
}

TEST(KFSLAMApp, EKF_SLAM_3D_data_assoc_NN_Maha)
{
    generic_kf_slam_test(
        "EKF-SLAM_6D_test_datassoc.ini", "kf-slam_6D_demo_DA.rawlog",
        [](mrpt::config::CConfigFileBase& c) {
            using namespace std::string_literals;
            c.write("RangeBearingKFSLAM", "data_assoc_method", "assocNN");
            c.write("RangeBearingKFSLAM", "data_assoc_metric", "metricMaha");

            c.write("MappingApplication", "SHOW_3D_LIVE", false);
            c.write("MappingApplication", "SAVE_3D_SCENES", false);
        });
}