CTopLCDetector_GridMatching.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 "hmtslam-precomp.h"  // Precomp header

#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>

using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace mrpt::poses;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::serialization;
using namespace mrpt::maps;

CTopLCDetector_GridMatching::CTopLCDetector_GridMatching(CHMTSLAM* hmtslam)
    : CTopLCDetectorBase(hmtslam)
{
}

CTopLCDetector_GridMatching::~CTopLCDetector_GridMatching() = default;
/** 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_GridMatching::computeTopologicalObservationModel(
    const THypothesisID& hypID, const CHMHMapNode::Ptr& currentArea,
    const CHMHMapNode::Ptr& refArea, double& out_log_lik)
{
    // ------------------------------------------------------------------------------
    // Compute potential map transformations between the areas "areaID" and
    // "a->first"
    //  For this, we use the grid-to-grid matching method described in the TRO
    //  paper...
    // ------------------------------------------------------------------------------
    out_log_lik = 0;  // Nothing to modify, for now.

    CGridMapAligner gridAligner;
    CGridMapAligner::TReturnInfo info;
    const CPose2D initEstimate(
        0, 0, 0);  // It's actually ignored by the grid-matching

    // Use already loaded options:
    const CTopLCDetector_GridMatching::TOptions& o =
        m_hmtslam->m_options.TLC_grid_options;
    gridAligner.options = o.matchingOptions;

    CMultiMetricMap::Ptr hMapCur =
        currentArea->m_annotations.getAs<CMultiMetricMap>(
            NODE_ANNOTATION_METRIC_MAPS, hypID, false);
    CMultiMetricMap::Ptr hMapRef =
        refArea->m_annotations.getAs<CMultiMetricMap>(
            NODE_ANNOTATION_METRIC_MAPS, hypID, false);

    ASSERT_(hMapRef->countMapsByClass<COccupancyGridMap2D>() >= 1);
    ASSERT_(hMapCur->countMapsByClass<COccupancyGridMap2D>() >= 1);

    gridAligner.options.dumpToConsole();

    // Do the map align:
    CPosePDF::Ptr alignRes = gridAligner.Align(
        hMapCur.get(),  // "ref" as seen from "cur"...The order is critical!!!
        hMapRef.get(), initEstimate, info);

#if 0
    {
        hMapCur->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map1.png", info.landmarks_map1.get());
        hMapRef->m_gridMaps[0]->saveAsBitmapFileWithLandmarks("map2.png", info.landmarks_map2.get());
    }
#endif

    // Transform the 2D SOG into a 3D SOG:
    CPose3DPDF::Ptr res = CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*alignRes));

// --------------------
// Debug output:
// --------------------
#if 1
    const std::string dbg_dir =
        m_hmtslam->m_options.LOG_OUTPUT_DIR + "/TBI_TESTS";
    if (!m_hmtslam->m_options.LOG_OUTPUT_DIR.empty())
    {
        mrpt::system::createDirectory(dbg_dir);
        static int cnt = 0;
        ++cnt;
        const std::string filStat =
            dbg_dir + format(
                          "/state_%05i_test_%i_%i.hmtslam", cnt,
                          (int)currentArea->getID(), (int)refArea->getID());
        const std::string filRes =
            dbg_dir + format(
                          "/state_%05i_test_%i_%i_result.txt", cnt,
                          (int)currentArea->getID(), (int)refArea->getID());

        m_hmtslam->logFmt(
            mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
            filStat.c_str());
        CFileGZOutputStream f(filStat);
        auto arch = mrpt::serialization::archiveFrom(f);
        this->m_hmtslam->saveState(arch);

        m_hmtslam->logFmt(
            mrpt::system::LVL_DEBUG, "[TLCD_gridmatch] DEBUG: Saving %s\n",
            filRes.c_str());
        CFileOutputStream f_res(filRes);
        f_res.printf(
            "# SOG modes: %i\n",
            (int)std::dynamic_pointer_cast<CPosePDFSOG>(alignRes)->size());
        f_res.printf("ICP goodness: ");
        f_res.printf_vector("%f ", info.icp_goodness_all_sog_modes);
        f_res.printf("\n");
    }
#endif

    return res;
}

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

// Initialization
CTopLCDetector_GridMatching::TOptions::TOptions() : matchingOptions() {}
//  Load parameters from configuration source
void CTopLCDetector_GridMatching::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
    matchingOptions.loadFromConfigFile(source, section);
}

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