persistence.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "persistence.h"
 
#include <cmath>
#include <cstdint>
#include <filesystem>
#include <fstream>
#include <optional>
#include <string>
#include <vector>
 
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/hal/interface.h>
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
 
#include <SimoxUtility/json/eigen_conversion.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
 
#include <nlohmann/json.hpp>
 
namespace armarx::navigation::algorithms
{
 
    Costmap
    load(const std::filesystem::path& directory)
    {
        // load
        const auto filename = directory / "costmap.json";
        std::ifstream ifs(filename);
        ARMARX_VERBOSE << "Loading costmap info " << filename.string();
        const nlohmann::json j = nlohmann::json::parse(ifs);
 
        // params
        const auto& jParam = j.at("params");
 
        const Costmap::Parameters params{.binaryGrid = jParam.at("binary_grid"),
                                         .cellSize = jParam.at("cell_size")};
 
        // scene bounds
        const auto& jSceneBounds = j.at("scene_bounds");
 
        const std::vector<float> boundsMin = jSceneBounds.at("min");
        const std::vector<float> boundsMax = jSceneBounds.at("max");
 
        ARMARX_CHECK_EQUAL(boundsMin.size(), 2);
        ARMARX_CHECK_EQUAL(boundsMax.size(), 2);
 
        const SceneBounds sceneBounds{.min = {boundsMin.at(0), boundsMin.at(1)},
                                      .max = {boundsMax.at(0), boundsMax.at(1)}};
 
 
        // grid
        const std::string gridFilename = j.at("grid_filename");
        cv::Mat gridMat = cv::imread((directory / gridFilename).string(),
                                     cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
 
        Eigen::MatrixXf grid;
        cv::cv2eigen(gridMat, grid);
 
        // mask, if any
        std::optional<Costmap::Mask> optMask;
        if (not j.at("mask_filename").empty())
        {
            const std::string maskFilename = j.at("grid_filename");
            cv::Mat maskMat = cv::imread((directory / maskFilename).string(),
                                         cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
 
            Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> mask;
            cv::cv2eigen(maskMat, mask);
 
            optMask = mask.cast<bool>();
        }
 
        Eigen::Isometry2f origin = Eigen::Isometry2f::Identity();
        if (not j.at("origin").empty())
        {
            const Eigen::Matrix3f h = j.at("origin");
            origin.matrix() = h;
        }
 
        return {grid, params, sceneBounds, optMask, origin};
    }
 
    bool
    save(const Costmap& costmap, const std::filesystem::path& directory)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "Storing costmap in directory " << QUOTED(directory.string());
 
        if (not std::filesystem::exists(directory))
        {
            ARMARX_VERBOSE << "Creating directory " << QUOTED(directory.string());
            std::filesystem::create_directories(directory);
        }
 
        nlohmann::json j = {{"params",
                             {{"binary_grid", costmap.params().binaryGrid},
                              {"cell_size", costmap.params().cellSize}}},
                            {"scene_bounds",
                             {{"min",
                               std::vector<float>{costmap.getLocalSceneBounds().min.x(),
                                                  costmap.getLocalSceneBounds().min.y()}},
                              {"max",
                               std::vector<float>{costmap.getLocalSceneBounds().max.x(),
                                                  costmap.getLocalSceneBounds().max.y()}}}},
                            {"origin", costmap.origin().matrix()}};
 
        std::optional<cv::Mat1b> mask;
        if (costmap.getMask().has_value())
        {
            // mask, if any
            Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> maskI =
                costmap.getMask()->cast<std::uint8_t>();
            cv::Mat1b m;
            cv::eigen2cv(maskI, m);
            mask = m;
        }
 
        // grid
        {
            ARMARX_TRACE;
 
            cv::Mat1f grid;
            cv::eigen2cv(costmap.getGrid(), grid);
 
            const std::string gridFilename = "grid.exr";
            cv::imwrite((directory / gridFilename).string(), grid);
 
            j["grid_filename"] = gridFilename;
 
            // for debugging purpose, also save a png image
            const std::string gridDebuggingFilename = "grid";
 
            double min;
            double max;
            if (mask)
            {
                cv::minMaxIdx(grid, &min, &max, 0, 0, mask.value());
            }
            else
            {
                cv::minMaxIdx(grid, &min, &max);
            }
            cv::Mat adjMap;
            grid.convertTo(adjMap, CV_8UC1, 255 / (max - min), -255 * min / (max - min));
 
            cv::Mat colorsMap;
            applyColorMap(adjMap, colorsMap, cv::COLORMAP_JET);
            cv::Mat maskedColorsMap;
            if (mask)
            {
                colorsMap.copyTo(maskedColorsMap, mask.value());
            }
            else
            {
                maskedColorsMap = colorsMap;
            }
            cv::imwrite((directory / (gridDebuggingFilename + ".png")).string(), maskedColorsMap);
 
            ARMARX_TRACE;
 
            // mask if any
            if (mask)
            {
                const std::string maskFilename = "mask.ppm";
                cv::imwrite((directory / maskFilename).string(), mask.value());
 
                j["mask_filename"] = maskFilename;
            }
        }
 
        // save
        std::ofstream of(directory / "costmap.json");
        of << j;
 
        return true;
    }
 
 
} // namespace armarx::navigation::algorithms