30 #include <opencv2/core.hpp>
31 #include <opencv2/core/eigen.hpp>
32 #include <opencv2/core/mat.hpp>
33 #include <opencv2/imgcodecs.hpp>
34 #include <opencv2/imgproc.hpp>
35 #include <opencv2/opencv.hpp>
37 #include <SimoxUtility/json/json.hpp>
49 load(
const std::filesystem::path& directory)
52 const auto filename = directory /
"costmap.json";
55 const nlohmann::json j = nlohmann::json::parse(ifs);
58 const auto& jParam = j.at(
"params");
61 .cellSize = jParam.at(
"cell_size")};
64 const auto& jSceneBounds = j.at(
"scene_bounds");
66 const std::vector<float> boundsMin = jSceneBounds.at(
"min");
67 const std::vector<float> boundsMax = jSceneBounds.at(
"max");
72 const SceneBounds sceneBounds{.
min = {boundsMin.at(0), boundsMin.at(1)},
73 .max = {boundsMax.at(0), boundsMax.at(1)}};
77 const std::string gridFilename = j.at(
"grid_filename");
78 cv::Mat gridMat = cv::imread((directory / gridFilename).
string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
84 std::optional<Costmap::Mask> optMask;
85 if (not j.at(
"mask_filename").empty())
87 const std::string maskFilename = j.at(
"grid_filename");
88 cv::Mat maskMat = cv::imread((directory / maskFilename).
string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
93 optMask = mask.cast<
bool>();
96 return {grid, params, sceneBounds, optMask};
101 save(
const Costmap& costmap,
const std::filesystem::path& directory)
105 ARMARX_INFO <<
"Storing costmap in directory `" << directory.string() <<
"`";
107 if (not std::filesystem::exists(directory))
109 ARMARX_VERBOSE <<
"Creating directory `" << directory.string() <<
"`";
110 std::filesystem::create_directories(directory);
126 std::optional<cv::Mat1b> mask;
127 if (costmap.
getMask().has_value())
131 costmap.
getMask()->cast<std::uint8_t>();
144 const std::string gridFilename =
"grid.exr";
145 cv::imwrite((directory / gridFilename).
string(), grid);
147 j[
"grid_filename"] = gridFilename;
150 const std::string gridDebuggingFilename =
"grid";
156 cv::minMaxIdx(grid, &
min, &
max, 0, 0, mask.value());
160 cv::minMaxIdx(grid, &
min, &
max);
166 applyColorMap(adjMap, colorsMap, cv::COLORMAP_JET);
167 cv::Mat maskedColorsMap;
170 colorsMap.copyTo(maskedColorsMap, mask.value());
174 maskedColorsMap = colorsMap;
176 cv::imwrite((directory / (gridDebuggingFilename +
".png")).
string(), maskedColorsMap);
183 const std::string maskFilename =
"mask.ppm";
184 cv::imwrite((directory / maskFilename).
string(), mask.value());
186 j[
"mask_filename"] = maskFilename;
191 std::ofstream of(directory /
"costmap.json");