RBPF_SLAM_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/RBPF_SLAM_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::RBPF_SLAM_App_Base&)>;

void generic_rbpf_slam_test(
    const std::string& ini_filename, const std::string& rawlog_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/rbpf-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::RBPF_SLAM_App_Rawlog app;
        app.setMinLoggingLevel(mrpt::system::LVL_WARN);

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

        app.initialize(argc, argv);

        app.params.write(
            "MappingApplication", "logOutput_dir",
            mrpt::system::getTempFileName() + "_dir"s);
        app.params.write(
            "MappingApplication", "SHOW_PROGRESS_IN_WINDOW", false);

#if !MRPT_HAS_OPENCV
        app.params.write("MappingApplication", "SAVE_3D_SCENE", false);
        app.params.write("MappingApplication", "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 auto tester_for_2006_01_21 = [](mrpt::apps::RBPF_SLAM_App_Base& o) {
    EXPECT_EQ(o.out_estimated_path.size(), 224U);
    const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
    const auto p_gt = mrpt::poses::CPose3D::FromString(
        "[3.4548 -18.0399 0.000000 -86.48 0.000000 0.000000]");

    EXPECT_LT(mrpt::poses::Lie::SE<3>::log(p - p_gt).norm(), 1.0)
        << "actual pose  =" << p.asString()
        << "\nexpected pose=" << p_gt.asString();
};

static auto tester_for_ROSLAM_demo = [](mrpt::apps::RBPF_SLAM_App_Base& o) {
    EXPECT_EQ(o.out_estimated_path.size(), 99U);
    const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
    const auto p_gt = mrpt::poses::CPose3D::FromString(
        "[1.938686 3.352273 0.000000 114.993417 0.000000 0.000000]");

    EXPECT_LT(mrpt::poses::Lie::SE<3>::log(p - p_gt).norm(), 1.0)
        << "actual pose  =" << p.asString()
        << "\nexpected pose=" << p_gt.asString();

    MRPT_TODO("Stricter unit tests: check for estimated landmark positions");
};

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling)
{
    using namespace std::string_literals;

    generic_rbpf_slam_test(
        "gridmapping_optimal_sampling.ini",
        "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
        [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_gridICP)
{
    using namespace std::string_literals;

    generic_rbpf_slam_test(
        "gridmapping_RBPF_grid_ICPbased_malaga.ini",
        "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
        [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_pointsICP)
{
    using namespace std::string_literals;

    generic_rbpf_slam_test(
        "gridmapping_RBPF_ICPbased_malaga.ini",
        "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
        [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_MC)
{
    using namespace std::string_literals;

    generic_rbpf_slam_test(
        "RO-SLAM_simulatedData_MC.ini", "RO-SLAM_demo.rawlog",
        [](mrpt::config::CConfigFileBase&) {}, tester_for_ROSLAM_demo);
}

TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_SOG)
{
    using namespace std::string_literals;

    generic_rbpf_slam_test(
        "RO-SLAM_simulatedData_SOG.ini", "RO-SLAM_demo.rawlog",
        [](mrpt::config::CConfigFileBase&) {}, tester_for_ROSLAM_demo);
}