persistence.cpp
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #include "persistence.h"
23 
24 #include <cmath>
25 #include <cstddef>
26 #include <cstdint>
27 #include <filesystem>
28 #include <fstream>
29 
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>
36 
37 #include <SimoxUtility/json/json.hpp>
38 
41 
44 
46 {
47 
48  Costmap
49  load(const std::filesystem::path& directory)
50  {
51  // load
52  const auto filename = directory / "costmap.json";
53  std::ifstream ifs(filename);
54  ARMARX_VERBOSE << "Loading costmap info " << filename.string();
55  const nlohmann::json j = nlohmann::json::parse(ifs);
56 
57  // params
58  const auto& jParam = j.at("params");
59 
60  const Costmap::Parameters params{.binaryGrid = jParam.at("binary_grid"),
61  .cellSize = jParam.at("cell_size")};
62 
63  // scene bounds
64  const auto& jSceneBounds = j.at("scene_bounds");
65 
66  const std::vector<float> boundsMin = jSceneBounds.at("min");
67  const std::vector<float> boundsMax = jSceneBounds.at("max");
68 
69  ARMARX_CHECK_EQUAL(boundsMin.size(), 2);
70  ARMARX_CHECK_EQUAL(boundsMax.size(), 2);
71 
72  const SceneBounds sceneBounds{.min = {boundsMin.at(0), boundsMin.at(1)},
73  .max = {boundsMax.at(0), boundsMax.at(1)}};
74 
75 
76  // grid
77  const std::string gridFilename = j.at("grid_filename");
78  cv::Mat gridMat = cv::imread((directory / gridFilename).string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
79 
80  Eigen::MatrixXf grid;
81  cv::cv2eigen(gridMat, grid);
82 
83  // mask, if any
84  std::optional<Costmap::Mask> optMask;
85  if (not j.at("mask_filename").empty())
86  {
87  const std::string maskFilename = j.at("grid_filename");
88  cv::Mat maskMat = cv::imread((directory / maskFilename).string(), cv::IMREAD_ANYCOLOR | cv::IMREAD_ANYDEPTH);
89 
91  cv::cv2eigen(maskMat, mask);
92 
93  optMask = mask.cast<bool>();
94  }
95 
96  return {grid, params, sceneBounds, optMask};
97  }
98 
99 
100  bool
101  save(const Costmap& costmap, const std::filesystem::path& directory)
102  {
103  ARMARX_TRACE;
104 
105  ARMARX_INFO << "Storing costmap in directory `" << directory.string() << "`";
106 
107  if (not std::filesystem::exists(directory))
108  {
109  ARMARX_VERBOSE << "Creating directory `" << directory.string() << "`";
110  std::filesystem::create_directories(directory);
111  }
112 
113  nlohmann::json j = {
114  { "params", {
115  {"binary_grid", costmap.params().binaryGrid },
116  {"cell_size", costmap.params().cellSize }
117  }},
118  {"scene_bounds", {
119  {"min", std::vector<float>{costmap.getLocalSceneBounds().min.x(),
120  costmap.getLocalSceneBounds().min.y()} },
121  {"max", std::vector<float>{costmap.getLocalSceneBounds().max.x(),
122  costmap.getLocalSceneBounds().max.y()}}
123  }}
124  };
125 
126  std::optional<cv::Mat1b> mask;
127  if (costmap.getMask().has_value())
128  {
129  // mask, if any
131  costmap.getMask()->cast<std::uint8_t>();
132  cv::Mat1b m;
133  cv::eigen2cv(maskI, m);
134  mask = m;
135  }
136 
137  // grid
138  {
139  ARMARX_TRACE;
140 
141  cv::Mat1f grid;
142  cv::eigen2cv(costmap.getGrid(), grid);
143 
144  const std::string gridFilename = "grid.exr";
145  cv::imwrite((directory / gridFilename).string(), grid);
146 
147  j["grid_filename"] = gridFilename;
148 
149  // for debugging purpose, also save a png image
150  const std::string gridDebuggingFilename = "grid";
151 
152  double min;
153  double max;
154  if (mask)
155  {
156  cv::minMaxIdx(grid, &min, &max, 0, 0, mask.value());
157  }
158  else
159  {
160  cv::minMaxIdx(grid, &min, &max);
161  }
162  cv::Mat adjMap;
163  grid.convertTo(adjMap,CV_8UC1, 255 / (max-min), -255*min/(max-min));
164 
165  cv::Mat colorsMap;
166  applyColorMap(adjMap, colorsMap, cv::COLORMAP_JET);
167  cv::Mat maskedColorsMap;
168  if (mask)
169  {
170  colorsMap.copyTo(maskedColorsMap, mask.value());
171  }
172  else
173  {
174  maskedColorsMap = colorsMap;
175  }
176  cv::imwrite((directory / (gridDebuggingFilename + ".png")).string(), maskedColorsMap);
177 
178  ARMARX_TRACE;
179 
180  // mask if any
181  if (mask)
182  {
183  const std::string maskFilename = "mask.ppm";
184  cv::imwrite((directory / maskFilename).string(), mask.value());
185 
186  j["mask_filename"] = maskFilename;
187  }
188  }
189 
190  // save
191  std::ofstream of(directory / "costmap.json");
192  of << j;
193 
194  return true;
195  }
196 
197 
198 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::Costmap::getMask
const std::optional< Mask > & getMask() const noexcept
Definition: Costmap.cpp:392
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::navigation::algorithms::load
Costmap load(const std::filesystem::path &directory)
Definition: persistence.cpp:49
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
persistence.h
armarx::navigation::algorithms::Costmap::getLocalSceneBounds
const SceneBounds & getLocalSceneBounds() const noexcept
Definition: Costmap.cpp:126
Costmap.h
armarx::navigation::algorithms::save
bool save(const Costmap &costmap, const std::filesystem::path &directory)
Definition: persistence.cpp:101
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::navigation::algorithms::Costmap::Parameters::binaryGrid
bool binaryGrid
Definition: Costmap.h:24
armarx::navigation::algorithms::Costmap::params
const Parameters & params() const noexcept
Definition: Costmap.cpp:230
armarx::navigation::algorithms::Costmap::getGrid
const Grid & getGrid() const
Definition: Costmap.cpp:236
armarx::conversions::cv2eigen
Eigen::Vector2f cv2eigen(const cv::Point2f &pt)
Definition: opencv_eigen.h:47
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::navigation::algorithms::SceneBounds
Definition: types.h:29
ExpressionException.h
armarx::navigation::algorithms::SceneBounds::min
Eigen::Vector2f min
Definition: types.h:31
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
armarx::navigation::algorithms::Costmap::Parameters::cellSize
float cellSize
How big each cell is in the uniform grid.
Definition: Costmap.h:27
armarx::navigation::algorithms::Costmap::Parameters
Definition: Costmap.h:21
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
Logging.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
types.h
armarx::navigation::algorithms::SceneBounds::max
Eigen::Vector2f max
Definition: types.h:32
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13
armarx::conversions::eigen2cv
cv::Point2f eigen2cv(const Eigen::Vector2f &pt)
Definition: opencv_eigen.h:30