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

/** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
 *
 *   The main entry points for a user are pushAction() and pushObservations().
 *Several parameters can be modified
 *   through m_options.
 *
 *  The mathematical models of this approach have been reported in:
 *      - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J.,
 *        "Towards a Unified Bayesian Approach to Hybrid Metric-Topological
 *SLAM",
 *          in  IEEE Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
 *      - ...
 *
 *  More information in the wiki page: https://www.mrpt.org/HMT-SLAM
 *
 */

#include "hmtslam-precomp.h"  // Precomp header

#include <mrpt/config/CConfigFile.h>
#include <mrpt/io/CFileStream.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/filesystem.h>

#include <mrpt/system/os.h>

using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::config;
using namespace mrpt::opengl;
using namespace mrpt::serialization;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CHMTSLAM, CSerializable, mrpt::hmtslam)

// Initialization of static members:
int64_t CHMTSLAM::m_nextAreaLabel = 0;
TPoseID CHMTSLAM::m_nextPoseID = 0;
THypothesisID CHMTSLAM::m_nextHypID = COMMON_TOPOLOG_HYP + 1;

/*---------------------------------------------------------------
                        Constructor
  ---------------------------------------------------------------*/
CHMTSLAM::CHMTSLAM()
{
    // Initialize data structures:
    // ----------------------------
    m_terminateThreads = false;
    m_terminationFlag_LSLAM = m_terminationFlag_TBI =
        m_terminationFlag_3D_viewer = false;

    // Create threads:
    // -----------------------
    m_hThread_LSLAM = std::thread(&CHMTSLAM::thread_LSLAM, this);
    m_hThread_TBI = std::thread(&CHMTSLAM::thread_TBI, this);
    m_hThread_3D_viewer = std::thread(&CHMTSLAM::thread_3D_viewer, this);

    // Other variables:
    m_LSLAM_method = nullptr;

    // Register default LC detectors:
    // --------------------------------
    registerLoopClosureDetector(
        "gridmaps", &CTopLCDetector_GridMatching::createNewInstance);
    registerLoopClosureDetector(
        "fabmap", &CTopLCDetector_FabMap::createNewInstance);

    // Prepare an empty map:
    initializeEmptyMap();
}

/*---------------------------------------------------------------
                        Destructor
  ---------------------------------------------------------------*/
CHMTSLAM::~CHMTSLAM()
{
    // Signal to threads that we are closing:
    // -------------------------------------------
    m_terminateThreads = true;

    // Wait for threads:
    // ----------------------------------
    MRPT_LOG_DEBUG("[CHMTSLAM::destructor] Waiting for threads end...\n");

    m_hThread_3D_viewer.join();
    m_hThread_LSLAM.join();
    m_hThread_TBI.join();

    MRPT_LOG_DEBUG("[CHMTSLAM::destructor] All threads finished.\n");

    // Save the resulting H.Map if logging
    // --------------------------------------
    if (!m_options.LOG_OUTPUT_DIR.empty())
    {
        try
        {
            /*          // Update the last area(s) in the HMAP:
                        updateHierarchicalMapFromRBPF();

                        // Save:
                        os::sprintf(auxFil,1000,"%s/hierarchicalMap.hmap",m_options.logOutputDirectory.c_str());

                        CFileStream     f(auxFil,fomWrite);
                        f << m_knownAreas;
                        */
        }
        catch (const std::exception& e)
        {
            MRPT_LOG_WARN(mrpt::format(
                "Ignoring exception at ~CHMTSLAM():\n%s", e.what()));
        }
        catch (...)
        {
            MRPT_LOG_WARN("Ignoring untyped exception at ~CHMTSLAM()");
        }
    }

    // Delete data structures:
    // ----------------------------------
    clearInputQueue();

    // Others:
    if (m_LSLAM_method)
    {
        delete m_LSLAM_method;
        m_LSLAM_method = nullptr;
    }

    // Delete TLC-detectors
    {
        std::lock_guard<std::mutex> lock(m_topLCdets_cs);

        // Clear old list:
        for (auto& m_topLCdet : m_topLCdets) delete m_topLCdet;
        m_topLCdets.clear();
    }
}

/*---------------------------------------------------------------
                        clearInputQueue
  ---------------------------------------------------------------*/
void CHMTSLAM::clearInputQueue()
{
    // Wait for critical section
    {
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);

        while (!m_inputQueue.empty())
        {
            // delete m_inputQueue.front();
            m_inputQueue.pop();
        };
    }
}

/*---------------------------------------------------------------
                        pushAction
  ---------------------------------------------------------------*/
void CHMTSLAM::pushAction(const CActionCollection::Ptr& acts)
{
    if (m_terminateThreads)
    {
        // Discard it:
        // delete acts;
        return;
    }

    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        m_inputQueue.push(acts);
    }
}

/*---------------------------------------------------------------
                        pushObservations
  ---------------------------------------------------------------*/
void CHMTSLAM::pushObservations(const CSensoryFrame::Ptr& sf)
{
    if (m_terminateThreads)
    {
        // Discard it:
        // delete sf;
        return;
    }

    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        m_inputQueue.push(sf);
    }
}

/*---------------------------------------------------------------
                        pushObservation
  ---------------------------------------------------------------*/
void CHMTSLAM::pushObservation(const CObservation::Ptr& obs)
{
    if (m_terminateThreads)
    {  // Discard it:
        // delete obs;
        return;
    }

    // Add a CSensoryFrame with the obs:
    CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();
    sf->insert(
        obs);  // memory will be freed when deleting the SF in other thread

    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        m_inputQueue.push(sf);
    }
}

/*---------------------------------------------------------------
                        loadOptions
  ---------------------------------------------------------------*/
void CHMTSLAM::loadOptions(const mrpt::config::CConfigFileBase& cfg)
{
    m_options.loadFromConfigFile(cfg, "HMT-SLAM");

    m_options.defaultMapsInitializers.loadFromConfigFile(cfg, "MetricMaps");

    m_options.pf_options.loadFromConfigFile(cfg, "PARTICLE_FILTER");

    m_options.KLD_params.loadFromConfigFile(cfg, "KLD");

    m_options.AA_options.loadFromConfigFile(cfg, "GRAPH_CUT");

    // Topological Loop Closure detector options:
    m_options.TLC_grid_options.loadFromConfigFile(cfg, "TLC_GRIDMATCHING");
    m_options.TLC_fabmap_options.loadFromConfigFile(cfg, "TLC_FABMAP");

    m_options.dumpToConsole();
}

/*---------------------------------------------------------------
                        loadOptions
  ---------------------------------------------------------------*/
void CHMTSLAM::loadOptions(const std::string& configFile)
{
    ASSERT_(mrpt::system::fileExists(configFile));
    CConfigFile cfg(configFile);
    loadOptions(cfg);
}

/*---------------------------------------------------------------
                        TOptions
  ---------------------------------------------------------------*/
CHMTSLAM::TOptions::TOptions()
{
    LOG_OUTPUT_DIR = "";
    LOG_FREQUENCY = 1;

    SLAM_METHOD = lsmRBPF_2DLASER;

    SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
    SLAM_MIN_HEADING_BETWEEN_OBS = DEG2RAD(25.0f);

    MIN_ODOMETRY_STD_XY = 0;
    MIN_ODOMETRY_STD_PHI = 0;

    VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
    VIEW3D_AREA_SPHERES_RADIUS = 1.0f;

    random_seed = 1234;

    TLC_detectors.clear();

    stds_Q_no_odo.resize(3);
    stds_Q_no_odo[0] = stds_Q_no_odo[1] = 0.10f;
    stds_Q_no_odo[2] = DEG2RAD(4.0f);
}

/*---------------------------------------------------------------
                        loadFromConfigFile
  ---------------------------------------------------------------*/
void CHMTSLAM::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
    MRPT_LOAD_CONFIG_VAR(LOG_OUTPUT_DIR, string, source, section);
    MRPT_LOAD_CONFIG_VAR(LOG_FREQUENCY, int, source, section);

    MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(
        SLAM_METHOD, int, TLSlamMethod, source, section);

    MRPT_LOAD_CONFIG_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float, source, section);
    MRPT_LOAD_CONFIG_VAR_DEGREES(SLAM_MIN_HEADING_BETWEEN_OBS, source, section);

    MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY, float, source, section);
    MRPT_LOAD_CONFIG_VAR_DEGREES(MIN_ODOMETRY_STD_PHI, source, section);

    MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_HEIGHT, float, source, section);
    MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_RADIUS, float, source, section);

    MRPT_LOAD_CONFIG_VAR(random_seed, int, source, section);

    stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
    source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
    ASSERT_(stds_Q_no_odo.size() == 3);

    stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);

    std::string sTLC_detectors =
        source.read_string(section, "TLC_detectors", "", true);

    mrpt::system::tokenize(sTLC_detectors, ", ", TLC_detectors);

    std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;

    // load other sub-classes:
    AA_options.loadFromConfigFile(source, section);
}

/*---------------------------------------------------------------
                        dumpToTextStream
  ---------------------------------------------------------------*/
void CHMTSLAM::TOptions::dumpToTextStream(std::ostream& out) const
{
    out << "\n----------- [CHMTSLAM::TOptions] ------------ \n\n";

    LOADABLEOPTS_DUMP_VAR(LOG_OUTPUT_DIR, string);
    LOADABLEOPTS_DUMP_VAR(LOG_FREQUENCY, int);

    LOADABLEOPTS_DUMP_VAR(SLAM_METHOD, int);

    LOADABLEOPTS_DUMP_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float);
    LOADABLEOPTS_DUMP_VAR_DEG(SLAM_MIN_HEADING_BETWEEN_OBS);

    LOADABLEOPTS_DUMP_VAR(MIN_ODOMETRY_STD_XY, float);
    LOADABLEOPTS_DUMP_VAR_DEG(MIN_ODOMETRY_STD_PHI);

    LOADABLEOPTS_DUMP_VAR(random_seed, int);

    AA_options.dumpToTextStream(out);
    pf_options.dumpToTextStream(out);
    KLD_params.dumpToTextStream(out);
    defaultMapsInitializers.dumpToTextStream(out);
    TLC_grid_options.dumpToTextStream(out);
    TLC_fabmap_options.dumpToTextStream(out);
}

/*---------------------------------------------------------------
                    isInputQueueEmpty
  ---------------------------------------------------------------*/
bool CHMTSLAM::isInputQueueEmpty()
{
    bool res;

    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        res = m_inputQueue.empty();
    }
    return res;
}

/*---------------------------------------------------------------
                    inputQueueSize
  ---------------------------------------------------------------*/
size_t CHMTSLAM::inputQueueSize()
{
    size_t res;
    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        res = m_inputQueue.size();
    }
    return res;
}

/*---------------------------------------------------------------
                    getNextObjectFromInputQueue
  ---------------------------------------------------------------*/
CSerializable::Ptr CHMTSLAM::getNextObjectFromInputQueue()
{
    CSerializable::Ptr obj;

    {  // Wait for critical section
        std::lock_guard<std::mutex> lock(m_inputQueue_cs);
        if (!m_inputQueue.empty())
        {
            obj = m_inputQueue.front();
            m_inputQueue.pop();
        }
    }
    return obj;
}

/*---------------------------------------------------------------
                        initializeEmptyMap
  ---------------------------------------------------------------*/
void CHMTSLAM::initializeEmptyMap()
{
    THypothesisIDSet LMH_hyps;
    THypothesisID newHypothID = generateHypothesisID();

    LMH_hyps.insert(COMMON_TOPOLOG_HYP);
    LMH_hyps.insert(newHypothID);

    // ------------------------------------
    // CLEAR HIERARCHICAL MAP
    // ------------------------------------
    CHMHMapNode::TNodeID firstAreaID;
    {
        std::lock_guard<std::mutex> lock(m_map_cs);

        // Initialize hierarchical structures:
        // -----------------------------------------------------
        m_map.clear();

        // Create a single node for the starting area:
        CHMHMapNode::Ptr firstArea = std::make_shared<CHMHMapNode>(&m_map);
        firstAreaID = firstArea->getID();

        firstArea->m_hypotheses = LMH_hyps;
        CMultiMetricMap::Ptr emptyMap =
            CMultiMetricMap::Create(m_options.defaultMapsInitializers);

        firstArea->m_nodeType = "Area";
        firstArea->m_label = generateUniqueAreaLabel();
        firstArea->m_annotations.set(
            NODE_ANNOTATION_METRIC_MAPS, emptyMap, newHypothID);
        firstArea->m_annotations.setElemental(
            NODE_ANNOTATION_REF_POSEID, POSEID_INVALID, newHypothID);
    }  // end of lock m_map_cs

    // ------------------------------------
    // CLEAR LIST OF HYPOTHESES
    // ------------------------------------
    {
        std::lock_guard<std::mutex> lock(m_LMHs_cs);

        // Add to the list:
        m_LMHs.clear();
        CLocalMetricHypothesis& newLMH = m_LMHs[newHypothID];
        newLMH.m_parent = this;

        newLMH.m_currentRobotPose =
            POSEID_INVALID;  // Special case: map is empty
        newLMH.m_log_w = 0;
        newLMH.m_ID = newHypothID;

        newLMH.m_neighbors.clear();
        newLMH.m_neighbors.insert(firstAreaID);

        newLMH.clearRobotPoses();
    }  // end of cs

    // ------------------------------------------
    //   Create the local SLAM algorithm object
    // -----------------------------------------
    switch (m_options.SLAM_METHOD)
    {
        case lsmRBPF_2DLASER:
        {
            if (m_LSLAM_method) delete m_LSLAM_method;
            m_LSLAM_method = new CLSLAM_RBPF_2DLASER(this);
        }
        break;
        default:
            THROW_EXCEPTION_FMT(
                "Invalid selection for LSLAM method: %i",
                (int)m_options.SLAM_METHOD);
    };

    // ------------------------------------
    //  Topological LC detectors:
    // ------------------------------------
    {
        std::lock_guard<std::mutex> lock(m_topLCdets_cs);

        // Clear old list:
        for (auto& m_topLCdet : m_topLCdets) delete m_topLCdet;
        m_topLCdets.clear();

        // Create new list:
        //  1: Occupancy Grid matching.
        //  2: Cummins' image matching.
        for (auto d = m_options.TLC_detectors.begin();
             d != m_options.TLC_detectors.end(); ++d)
            m_topLCdets.push_back(loopClosureDetector_factory(*d));
    }

    // ------------------------------------
    //  Other variables:
    // ------------------------------------
    m_LSLAM_queue.clear();

    // ------------------------------------
    // Delete log files:
    // ------------------------------------
    if (!m_options.LOG_OUTPUT_DIR.empty())
    {
        mrpt::system::deleteFilesInDirectory(m_options.LOG_OUTPUT_DIR);
        mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR);
        mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_txt");
        mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_3D");
        mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/LSLAM_3D");
        mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/ASSO");
        mrpt::system::createDirectory(
            m_options.LOG_OUTPUT_DIR + "/HMTSLAM_state");
    }
}

/*---------------------------------------------------------------
                        generateUniqueAreaLabel
  ---------------------------------------------------------------*/
std::string CHMTSLAM::generateUniqueAreaLabel()
{
    return format("%li", (long int)(m_nextAreaLabel++));
}

/*---------------------------------------------------------------
                        generatePoseID
  ---------------------------------------------------------------*/
TPoseID CHMTSLAM::generatePoseID() { return m_nextPoseID++; }
/*---------------------------------------------------------------
                        generateHypothesisID
  ---------------------------------------------------------------*/
THypothesisID CHMTSLAM::generateHypothesisID() { return m_nextHypID++; }
/*---------------------------------------------------------------
                        getAs3DScene
  ---------------------------------------------------------------*/
void CHMTSLAM::getAs3DScene([[maybe_unused]] COpenGLScene& scene3D) {}

/*---------------------------------------------------------------
                        abortedDueToErrors
  ---------------------------------------------------------------*/
bool CHMTSLAM::abortedDueToErrors()
{
    return m_terminationFlag_LSLAM || m_terminationFlag_TBI ||
           m_terminationFlag_3D_viewer;
}

/*---------------------------------------------------------------
                        registerLoopClosureDetector
  ---------------------------------------------------------------*/
void CHMTSLAM::registerLoopClosureDetector(
    const std::string& name, CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*))
{
    m_registeredLCDetectors[name] = ptrCreateObject;
}

/*---------------------------------------------------------------
                loopClosureDetector_factory
  ---------------------------------------------------------------*/
CTopLCDetectorBase* CHMTSLAM::loopClosureDetector_factory(
    const std::string& name)
{
    MRPT_START
    auto it = m_registeredLCDetectors.find(name);
    if (it == m_registeredLCDetectors.end())
        THROW_EXCEPTION_FMT(
            "Invalid value for TLC_detectors: %s", name.c_str());
    return it->second(this);
    MRPT_END
}

bool CHMTSLAM::saveState(CArchive& out) const
{
    try
    {
        out << *this;
        return true;
    }
    catch (...)
    {
        return false;
    }
}

bool CHMTSLAM::loadState(CArchive& in)
{
    try
    {
        in >> *this;
        return true;
    }
    catch (...)
    {
        return false;
    }
}

/*---------------------------------------------------------------
                    readFromStream
  ---------------------------------------------------------------*/
void CHMTSLAM::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            // Acquire all critical sections before!
            // -------------------------------------------
            std::lock_guard<std::mutex> lock_map(m_map_cs);

            // Data:
            in >> m_nextAreaLabel >> m_nextPoseID >> m_nextHypID;
            // The HMT-MAP:
            in >> m_map;
            // The LMHs:
            in >> m_LMHs;
            // Save options??? Better allow changing them...
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

uint8_t CHMTSLAM::serializeGetVersion() const { return 0; }
void CHMTSLAM::serializeTo(mrpt::serialization::CArchive& out) const
{
    // Acquire all critical sections before!
    // -------------------------------------------
    std::lock_guard<std::mutex> lock_map(m_map_cs);

    // Data:
    out << m_nextAreaLabel << m_nextPoseID << m_nextHypID;
    // The HMT-MAP:
    out << m_map;
    // The LMHs:
    out << m_LMHs;
    // Save options??? Better allow changing them...
}