CTopLCDetector_FabMap.cpp

Go to the documentation of this file.

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

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

#ifdef HTMSLAM_HAS_FABMAP
#include <FabMapLibInterface.h>
#define THE_FABMAP static_cast<fabmap::FabMapInstance*>(m_fabmap)
#endif

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::hmtslam;
using namespace std;

CTopLCDetector_FabMap::CTopLCDetector_FabMap(CHMTSLAM* hmtslam)
    : CTopLCDetectorBase(hmtslam), m_fabmap(nullptr)
{
#ifdef HTMSLAM_HAS_FABMAP
    // Use already loaded options:
    const CTopLCDetector_FabMap::TOptions* o =
        &m_hmtslam->m_options.TLC_fabmap_options;

    unsigned int nVocabSize;
    if (!fabmap::ParseOXV_PeekDimensions(
            o->vocab_path + o->vocabName + ".oxv", nVocabSize))
        THROW_EXCEPTION_FMT(
            "Error parsing vocabulary file: %s",
            std::string(o->vocab_path + o->vocabName + ".oxv").c_str())

    m_fabmap = new fabmap::FabMapInstance(
        o->vocab_path, o->vocabName, o->p_obs_given_exists, 0.1,
        o->p_at_new_place, nVocabSize, o->df_lik_smooth);

    // Restart msg:
    cout << "[CTopLCDetector_FabMap::constructor] Resetting FabMap" << endl;
    THE_FABMAP->hmtslam_restart();
#else
    THROW_EXCEPTION("Please, recompile MRPT with FabMap to use this class.");
#endif
}

CTopLCDetector_FabMap::~CTopLCDetector_FabMap()
{
#ifdef HTMSLAM_HAS_FABMAP
    delete THE_FABMAP;
    m_fabmap = nullptr;
#endif
}

/** This method must compute the topological observation model.
  * \param out_log_lik The output, a log-likelihood.
  * \return nullptr, or a PDF of the estimated translation between the two areas
 * (can be a multi-modal PDF).
  */
CPose3DPDF::Ptr CTopLCDetector_FabMap::computeTopologicalObservationModel(
    const THypothesisID& hypID, const CHMHMapNode::Ptr& currentArea,
    const CHMHMapNode::Ptr& refArea, double& out_log_lik)
{
    MRPT_UNUSED_PARAM(hypID);
    MRPT_UNUSED_PARAM(currentArea);
    MRPT_UNUSED_PARAM(refArea);
    MRPT_UNUSED_PARAM(out_log_lik);
    return CPose3DPDF::Ptr();
}

/** Hook method for being warned about the insertion of a new poses into the
 * maps.
  *  This should be independent of hypothesis IDs.
  */
void CTopLCDetector_FabMap::OnNewPose(
    const TPoseID& poseID, const CSensoryFrame* SF)
{
#ifdef HTMSLAM_HAS_FABMAP

    vector<string> lstObsImages;

    size_t n = 0;
    CObservationImage::Ptr obsIm;
    while ((obsIm = SF->getObservationByClass<CObservationImage>(n)))
    {
        string path;
        obsIm->image.getExternalStorageFileAbsolutePath(path);
        lstObsImages.push_back(path);
        n++;
    };

    if (lstObsImages.empty()) return;  // Not all poses must have images.

    cout << "[OnNewPose] Adding new pose: " << poseID
         << " # of images: " << lstObsImages.size() << endl;
    THE_FABMAP->hmtslam_addNewPose(poseID, lstObsImages);

#else
    MRPT_UNUSED_PARAM(poseID);
    MRPT_UNUSED_PARAM(SF);
#endif
}

// Initialization
CTopLCDetector_FabMap::TOptions::TOptions()
    : vocab_path("./vocab"),
      vocabName("vocab_name"),
      p_obs_given_exists(0.39),
      p_at_new_place(0.99),
      df_lik_smooth(0.99)
{
}

//  Load parameters from configuration source
void CTopLCDetector_FabMap::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
    MRPT_LOAD_CONFIG_VAR(vocab_path, string, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(vocabName, string, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(p_obs_given_exists, double, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(p_at_new_place, double, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(df_lik_smooth, double, iniFile, section);
}

//  This method must display clearly all the contents of the structure in
//  textual form, sending it to a CStream.
void CTopLCDetector_FabMap::TOptions::dumpToTextStream(std::ostream& out) const
{
    out << mrpt::format(
        "\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n");

    LOADABLEOPTS_DUMP_VAR(vocab_path, string)
    LOADABLEOPTS_DUMP_VAR(vocabName, string)
    LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
    LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
    LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
}