CMetricMapBuilder.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 "slam-precomp.h"  // Precompiled headers

#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CMetricMapBuilder.h>
#include <mrpt/system/filesystem.h>

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

CMetricMapBuilder::CMetricMapBuilder()
    : mrpt::system::COutputLogger("CMetricMapBuilder"),
      options(this->m_min_verbosity_level)
{
    MRPT_LOG_DEBUG("CMetricMapBuilder ctor.");
}

CMetricMapBuilder::~CMetricMapBuilder()
{
    MRPT_LOG_DEBUG("CMetricMapBuilder dtor.");
}

/*---------------------------------------------------------------
                    clear
Clear all elements of the maps, and reset localization to (0,0,0deg).
  ---------------------------------------------------------------*/
void CMetricMapBuilder::clear()
{
    MRPT_LOG_DEBUG("CMetricMapBuilder::clear() called.");
    CSimpleMap dummyMap;
    CPosePDFGaussian dummyPose;

    // Initialize with an empty map and pose to (0,0,0)
    initialize(dummyMap, &dummyPose);
}

/*---------------------------------------------------------------
                    loadCurrentMapFromFile
  ---------------------------------------------------------------*/
void CMetricMapBuilder::loadCurrentMapFromFile(const std::string& fileName)
{
    CSimpleMap map;

    // New file??
    if (mrpt::system::fileExists(fileName))
    {
        MRPT_LOG_INFO_STREAM(
            "[CMetricMapBuilder::loadCurrentMapFromFile] Loading current map "
            "from '"
            << fileName << "' ..." << std::endl);
        CFileGZInputStream f(fileName);

        // Load from file:
        archiveFrom(f) >> map;
    }
    else
    {  // Is a new file, start with an empty map:
        MRPT_LOG_WARN_STREAM(
            "[CMetricMapBuilder::loadCurrentMapFromFile] Loading current map "
            "from '"
            << fileName << "' ..." << std::endl);
        map.clear();
    }

    // Initialize the map builder with this map
    initialize(map);
}

/*---------------------------------------------------------------
                    saveCurrentMapToFile
  ---------------------------------------------------------------*/
void CMetricMapBuilder::saveCurrentMapToFile(
    const std::string& fileName, bool compressGZ) const
{
    // get current map:
    CSimpleMap curmap;
    getCurrentlyBuiltMap(curmap);

    MRPT_LOG_INFO_STREAM(
        "[CMetricMapBuilder::saveCurrentMapToFile] Saving current map to '"
        << fileName << "' ..." << std::endl);

    // Save to file:
    if (compressGZ)
    {
        CFileGZOutputStream f(fileName);
        archiveFrom(f) << curmap;
    }
    else
    {
        CFileOutputStream f(fileName);
        archiveFrom(f) << curmap;
    }
}