VoxelGrid.cpp
Go to the documentation of this file.
1 #include "VoxelGrid.h"
2 
3 #include <algorithm>
4 
5 #include <SimoxUtility/algorithm/string/string_tools.h>
6 
8 
9 #include "../utils.h"
10 
12 {
13 
14  bool
15  Voxel::isFree() const
16  {
17  return empty() || getTotal() == 0;
18  }
19 
20  std::size_t
22  {
23  std::size_t sum = 0;
24  for (const auto& [_, num] : *this)
25  {
26  sum += num;
27  }
28  return sum;
29  }
30 
31  std::map<Label, float>
33  {
34  const float total = static_cast<float>(getTotal());
35 
36  std::map<Label, float> density;
37  for (const auto& [label, num] : *this)
38  {
39  density[label] = total > 0.f ? num / total : 0.f;
40  ;
41  }
42  return density;
43  }
44 
45  Label
47  {
48  return getMax().first;
49  }
50 
51  std::pair<Label, std::size_t>
52  Voxel::getMax() const
53  {
54  return *std::max_element(begin(), end());
55  }
56 
57  bool
58  Voxel::operator==(const Voxel& rhs) const
59  {
60  // Collect labels to compare.
61  std::set<Label> labels;
62  for (const Voxel* voxel : {this, &rhs})
63  {
64  for (const auto& [label, num] : *voxel)
65  {
66  if (num > 0) // No need to compare entries with count 0.
67  {
68  labels.insert(label);
69  }
70  }
71  }
72  for (Label label : labels)
73  {
74  if ((*this).at(label) != rhs.at(label))
75  {
76  return false;
77  }
78  }
79  return true;
80  }
81 
82  bool
83  Voxel::operator!=(const Voxel& rhs) const
84  {
85  return !(*this == rhs);
86  }
87 
88  std::ostream&
89  operator<<(std::ostream& os, const Voxel& voxel)
90  {
91  os << "{ ";
92  std::vector<std::string> items;
93  for (const auto& [label, num] : voxel)
94  {
95  items.push_back(std::to_string(label) + ": " + std::to_string(num));
96  }
97  os << simox::alg::join(items, ", ") << " }";
98  return os;
99  }
100 
101  template <class PointT>
102  static void
103  addPointCloud(VoxelGrid& grid,
104  const pcl::PointCloud<PointT>& _pointCloud,
105  const Eigen::Matrix4f& pointCloudPose)
106  {
107  // Transform the point cloud in one go to avoid transformation of each
108  // point individually.
109  const pcl::PointCloud<PointT> pointCloud =
110  utils::transformPointCloudToGrid(_pointCloud, pointCloudPose, grid.getPose());
111 
112  // After transforming the point cloud, it is local.
113  const bool local = true;
114 
115  for (const PointT& point : pointCloud)
116  {
117  const Eigen::Vector3f vpoint(point.data);
118 
119  const Eigen::Vector3i indices = grid.getVoxelGridIndex(vpoint, local);
120  if (!grid.isInside(indices))
121  {
122  continue;
123  }
124 
125  auto& voxel = grid.getVoxel(indices);
126  voxel[static_cast<Label>(point.label)]++;
127  }
128  }
129 
130  void
131  VoxelGrid::addPointCloud(const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
132  const Eigen::Matrix4f& pointCloudPose)
133  {
134  LabelDensity::addPointCloud(*this, pointCloud, pointCloudPose);
135  }
136 
137  void
138  VoxelGrid::addPointCloud(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
139  const Eigen::Matrix4f& pointCloudPose)
140  {
141  LabelDensity::addPointCloud(*this, pointCloud, pointCloudPose);
142  }
143 
144  std::set<Label>
146  {
147  std::set<Label> ids;
148  for (const Voxel& voxel : *this)
149  {
150  for (const auto& item : voxel)
151  {
152  ids.insert(item.first);
153  }
154  }
155  return ids;
156  }
157 
158  void
159  VoxelGrid::reduceNoise(std::size_t minNumPoints)
160  {
161  for (auto& voxel : *this)
162  {
163  std::set<Label> remove;
164  for (const auto& [label, num] : voxel)
165  {
166  if (num < minNumPoints)
167  {
168  remove.insert(label);
169  }
170  }
171 
172  for (Label label : remove)
173  {
174  voxel.erase(voxel.find(label));
175  }
176  }
177  }
178 
180  makeJsonGetters(const VoxelGrid& grid)
181  {
183 
184  const std::set<Label> labels = grid.getUniqueLabels();
185 
186  for (Label label : labels)
187  {
188  getters[std::to_string(label)] = [label](const Voxel& voxel) -> nlohmann::json
189  {
190  const auto find = voxel.find(label);
191  return find != voxel.end() ? find->second : 0;
192  };
193  }
194 
195  return getters;
196  }
197 
198  static io::VoxelAttributeSetterMap<Voxel>
199  makeJsonSetters(const std::set<Label>& labels)
200  {
201  voxelgrid::io::VoxelAttributeSetterMap<Voxel> setters;
202  for (Label label : labels)
203  {
204  setters[std::to_string(label)] = [label](const nlohmann::json& j, Voxel& voxel)
205  { voxel[label] = j; };
206  }
207  return setters;
208  }
209 
210  static std::set<Label>
211  getLabelsFromKeys(const nlohmann::json& json)
212  {
213  // All numeric top-level keys are labels.
214 
215  std::set<Label> labels;
216  for (const auto& item : json.items())
217  {
218  if (item.key() == "structure")
219  {
220  continue;
221  }
222  try
223  {
224  const int key = std::stoi(item.key());
225  if (key >= 0)
226  {
227  labels.insert(static_cast<Label>(key));
228  }
229  }
230  catch (std::invalid_argument&)
231  {
232  // Ignore this key.
233  }
234  }
235 
236  return labels;
237  }
238 
239  void
240  VoxelGrid::writeJson(std::ostream& os) const
241  {
242  io::JsonIO::write<Voxel>(os, *this, makeJsonGetters(*this));
243  }
244 
245  void
246  VoxelGrid::writeJson(const std::filesystem::path& file) const
247  {
248  io::JsonIO::write<Voxel>(file, *this, makeJsonGetters(*this));
249  }
250 
251  void
252  VoxelGrid::readJson(std::istream& is)
253  {
254  // Since we do not know which labels are present, we have to take a peak at
255  // the JSON content.
256  const nlohmann::json json = io::JsonIO::readJson(is);
257  from_json(json, *this);
258  }
259 
260  void
261  VoxelGrid::readJson(const std::filesystem::path& file)
262  {
263  std::ifstream ifs(file);
264  readJson(ifs);
265  }
266 
267  void
268  to_json(nlohmann::json& json, const VoxelGrid& grid)
269  {
270  io::JsonIO::toJson(json, grid, makeJsonGetters(grid));
271  }
272 
273  void
274  from_json(const nlohmann::json& json, VoxelGrid& grid)
275  {
276  io::JsonIO::fromJson(json, grid, makeJsonSetters(getLabelsFromKeys(json)));
277  }
278 
279  void
280  VoxelGrid::toCsv(std::ostream& os, bool includeTotal) const
281  {
282  static const std::string sep = ",";
283 
284  const std::set<Label> labels = getUniqueLabels();
285 
286  // Write header.
287 
288  std::vector<std::string> strings;
289  // Index
290  strings.push_back("index");
291  // Counts
292  std::transform(labels.begin(),
293  labels.end(),
294  std::back_inserter(strings),
295  [](Label l) { return std::to_string(l); });
296 
297  if (includeTotal)
298  {
299  // Total
300  strings.push_back("total");
301  }
302 
303  os << simox::alg::join(strings, sep) << std::endl;
304 
305  // Write data.
306  for (std::size_t index = 0; index < numVoxels(); ++index)
307  {
308  const Voxel& voxel = getVoxel(index);
309  if (!voxel.empty())
310  {
311  strings.clear();
312 
313  // Index
314  strings.push_back(std::to_string(index));
315  // Counts
316  std::transform(labels.begin(),
317  labels.end(),
318  std::back_inserter(strings),
319  [&voxel](const Label id)
320  {
321  auto it = voxel.find(id);
322  return std::to_string(it != voxel.end() ? it->second : 0);
323  });
324 
325  // Total
326  if (includeTotal)
327  {
328  strings.push_back(std::to_string(voxel.getTotal()));
329  }
330 
331  // Write line.
332  os << simox::alg::join(strings, sep) << std::endl;
333  }
334  }
335  }
336 
337  void
338  VoxelGrid::toCsv(const std::filesystem::path& file, bool includeTotal) const
339  {
340  std::ofstream os(file);
341  toCsv(os, includeTotal);
342  }
343 
344 } // namespace visionx::voxelgrid::LabelDensity
VoxelGrid.h
visionx::voxelgrid::io::JsonIO::fromJson
static void fromJson(const nlohmann::json &j, VoxelGrid< VoxelT > &grid, const VoxelAttributeSetterMap< VoxelT > &attributeMap)
Deserialize a voxel grid from JSON.
Definition: JsonIO.h:295
visionx::voxelgrid::VoxelGrid
A 3D grid of voxels of type _VoxelT.
Definition: VoxelGrid.hpp:49
visionx::voxelgrid::io::JsonIO::toJson
static void toJson(nlohmann::json &j, const VoxelGrid< VoxelT > &grid, const VoxelAttributeGetterMap< VoxelT > &attributeMap)
Serialize a voxel grid to JSON.
Definition: JsonIO.h:246
index
uint8_t index
Definition: EtherCATFrame.h:59
visionx::voxelgrid::utils::transformPointCloudToGrid
pcl::PointCloud< PointT > transformPointCloudToGrid(const pcl::PointCloud< PointT > &pointCloud, const Eigen::Matrix4f &pointCloudPose, const Eigen::Matrix4f &gridPose)
Transform a point cloud to local grid coordinates.
Definition: point_cloud.h:32
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::RemoteGui::Client::Label
Definition: Widgets.h:31
visionx::voxelgrid::LabelDensity::Voxel::getMaxLabel
Label getMaxLabel() const
Get the label with most points.
Definition: VoxelGrid.cpp:46
visionx::voxelgrid::VoxelGrid::getPose
Eigen::Matrix4f getPose() const
Get the grid pose in the world frame.
Definition: VoxelGrid.hpp:363
visionx::voxelgrid::LabelDensity::operator<<
std::ostream & operator<<(std::ostream &os, const Voxel &voxel)
Definition: VoxelGrid.cpp:89
visionx::voxelgrid::LabelDensity::VoxelGrid::toCsv
void toCsv(std::ostream &os, bool includeTotal=false) const
Write the voxel data to a CSV file.
Definition: VoxelGrid.cpp:280
visionx::voxelgrid::io::VoxelAttributeGetterMap
std::map< std::string, VoxelAttributeGetter< VoxelT > > VoxelAttributeGetterMap
Map of attribute name to attribute getter.
Definition: JsonIO.h:27
visionx::voxelgrid::LabelDensity::VoxelGrid::reduceNoise
void reduceNoise(std::size_t minNumPoints)
Remove entries with number of points below minNumPoints.
Definition: VoxelGrid.cpp:159
visionx::voxelgrid::LabelDensity::VoxelGrid
Voxel grid storing the number of points per label in each voxel.
Definition: VoxelGrid.h:66
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:234
visionx::voxelgrid::LabelDensity::VoxelGrid::readJson
void readJson(std::istream &is)
Read this voxel grid from JSON.
Definition: VoxelGrid.cpp:252
visionx::voxelgrid::LabelDensity::VoxelGrid::writeJson
void writeJson(std::ostream &os) const
Write this voxel grid to JSON.
Definition: VoxelGrid.cpp:240
visionx::voxelgrid::VoxelGrid::getVoxel
VoxelT & getVoxel(std::size_t index)
Get the voxel with the given index.
Definition: VoxelGrid.hpp:102
visionx::voxelgrid::LabelDensity::Voxel::isFree
bool isFree() const
Indicate whether this voxel is free, i.e. it contains no points.
Definition: VoxelGrid.cpp:15
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:6
visionx::voxelgrid::VoxelGrid< VoxelT >::numVoxels
std::size_t numVoxels() const
Get the number of voxels in the grid.
Definition: VoxelGrid.hpp:175
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:717
visionx::voxelgrid::io::JsonIO::readJson
static nlohmann::json readJson(const std::string &filename)
Read JSON from file.
Definition: JsonIO.cpp:24
visionx::voxelgrid::LabelDensity::Voxel::getDensity
std::map< Label, float > getDensity() const
Get the relative frequency of labels, so that the sum is 1.0.
Definition: VoxelGrid.cpp:32
visionx::voxelgrid::LabelDensity::Voxel::getTotal
std::size_t getTotal() const
Get the total number of points in this voxel.
Definition: VoxelGrid.cpp:21
visionx::voxelgrid::VoxelGrid::getVoxelGridIndex
Eigen::Vector3i getVoxelGridIndex(size_t index) const
Get the grid index of the voxel with the given flat index.
Definition: VoxelGrid.hpp:253
visionx::voxelgrid::LabelDensity::Voxel::getMax
std::pair< Label, std::size_t > getMax() const
Get the label with most points and the number of points.
Definition: VoxelGrid.cpp:52
visionx::voxelgrid::LabelDensity::Voxel
Map of labels (object IDs) to number of points.
Definition: VoxelGrid.h:26
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
visionx::voxelgrid::LabelDensity::VoxelGrid::getUniqueLabels
std::set< Label > getUniqueLabels() const
Get the set of unique labels.
Definition: VoxelGrid.cpp:145
visionx::voxelgrid::LabelDensity::VoxelGrid::addPointCloud
void addPointCloud(const pcl::PointCloud< pcl::PointXYZL > &pointCloud, const Eigen::Matrix4f &pointCloudPose=Eigen::Matrix4f::Identity())
Add the point cloud.
Definition: VoxelGrid.cpp:131
visionx::voxelgrid::LabelDensity::from_json
void from_json(const nlohmann::json &json, VoxelGrid &grid)
Read the voxel grid from JSON.
Definition: VoxelGrid.cpp:274
visionx::voxelgrid::LabelDensity::Voxel::operator==
bool operator==(const Voxel &rhs) const
Equality operator.
Definition: VoxelGrid.cpp:58
visionx::voxelgrid::VoxelGrid::isInside
bool isInside(const Eigen::Vector3i &index) const
Indicate whether the given point is inside the voxel.
Definition: VoxelGrid.hpp:422
JsonIO.h
visionx::voxelgrid::LabelDensity::to_json
void to_json(nlohmann::json &json, const VoxelGrid &grid)
Write the voxel grid to JSON.
Definition: VoxelGrid.cpp:268
visionx::voxelgrid::LabelDensity
Definition: Visualizer.cpp:8
visionx::PointT
pcl::PointXYZRGBA PointT
Definition: MaskRCNNPointCloudObjectLocalizer.h:77
visionx::voxelgrid::LabelDensity::Voxel::operator!=
bool operator!=(const Voxel &rhs) const
Definition: VoxelGrid.cpp:83