NormalVectorsFilter.cpp
Go to the documentation of this file.
1 /*
2  * NormalVectorsFilter.cpp
3  *
4  * Created on: May 05, 2015
5  * Author: Peter Fankhauser, Martin Wermelinger
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 #include <Eigen/Dense>
11 #include <memory>
12 #include <stdexcept>
13 
14 #include <tbb/task_scheduler_init.h>
15 #include <tbb/tbb.h>
16 
18 
20 
21 namespace grid_map {
22 
23 template <typename T>
25  : method_(Method::RasterSerial), estimationRadius_(0.0), parallelizationEnabled_(false), threadCount_(1), gridMapResolution_(0.02) {}
26 
27 template <typename T>
29 
30 template <typename T>
32  // Read which algorithm is chosen: area or raster.
33  std::string algorithm;
34  if (!filters::FilterBase<T>::getParam(std::string("algorithm"), algorithm)) {
35  ROS_WARN("Could not find the parameter: `algorithm`. Setting to default value: 'area'.");
36  // Default value.
37  algorithm = "area";
38  }
39 
40  // Read parameters related to area algorithm only if needed, otherwise when using raster method
41  // on purpose it throws unwanted errors.
42  if (algorithm != "raster") {
43  // Read radius, if found, its value will be used for area method. If radius parameter is not found, raster method will be used.
44  if (!filters::FilterBase<T>::getParam(std::string("radius"), estimationRadius_)) {
45  ROS_WARN("Could not find the parameter: `radius`. Switching to raster method.");
46  algorithm = "raster";
47  }
48  ROS_DEBUG("Normal vectors estimation radius = %f", estimationRadius_);
49  // If radius not positive switch to raster method.
50  if (estimationRadius_ <= 0) {
51  ROS_WARN("Parameter `radius` is not positive. Switching to raster method.");
52  algorithm = "raster";
53  }
54  }
55 
56  // Read parallelization_enabled to decide whether parallelization has to be used, if parameter is not found an error is thrown and
57  // the false default value will be used.
58  if (!filters::FilterBase<T>::getParam(std::string("parallelization_enabled"), parallelizationEnabled_)) {
59  ROS_WARN("Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.");
61  }
62  ROS_DEBUG("Parallelization_enabled = %d", parallelizationEnabled_);
63 
64  // Read thread_number to set the number of threads to be used if parallelization is enebled,
65  // if parameter is not found an error is thrown and the default is to set it to automatic.
66  if (!filters::FilterBase<T>::getParam(std::string("thread_number"), threadCount_)) {
67  ROS_WARN("Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.");
68  threadCount_ = tbb::task_scheduler_init::automatic;
69  }
70  ROS_DEBUG("Thread_number = %d", threadCount_);
71 
72  // Set wanted method looking at algorithm and parallelization_enabled parameters.
73  // parallelization_enabled is used to select whether to use parallelization or not.
74  if (algorithm == "raster") {
75  // If parallelizationEnabled_=true, use the parallel method, otherwise serial.
78  ROS_DEBUG("Method RasterParallel");
79  } else {
81  ROS_DEBUG("Method RasterSerial");
82  }
83  } else {
84  // If parallelizationEnabled_=true, use the parallel method, otherwise serial.
87  ROS_DEBUG("Method AreaParallel");
88  } else {
90  ROS_DEBUG("Method AreaSerial");
91  }
92  ROS_DEBUG("estimationRadius_ = %f", estimationRadius_);
93  }
94 
95  // Read normal_vector_positive_axis, to define normal vector positive direction.
96  std::string normalVectorPositiveAxis;
97  if (!filters::FilterBase<T>::getParam(std::string("normal_vector_positive_axis"), normalVectorPositiveAxis)) {
98  ROS_ERROR("Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
99  return false;
100  }
101  if (normalVectorPositiveAxis == "z") {
102  normalVectorPositiveAxis_ = Vector3::UnitZ();
103  } else if (normalVectorPositiveAxis == "y") {
104  normalVectorPositiveAxis_ = Vector3::UnitY();
105  } else if (normalVectorPositiveAxis == "x") {
106  normalVectorPositiveAxis_ = Vector3::UnitX();
107  } else {
108  ROS_ERROR("The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
109  return false;
110  }
111 
112  // Read input_layer, to define input grid map layer.
113  if (!filters::FilterBase<T>::getParam(std::string("input_layer"), inputLayer_)) {
114  ROS_ERROR("Normal vectors filter did not find parameter `input_layer`.");
115  return false;
116  }
117  ROS_DEBUG("Normal vectors filter input layer is = %s.", inputLayer_.c_str());
118 
119  // Read output_layers_prefix, to define output grid map layers prefix.
120  if (!filters::FilterBase<T>::getParam(std::string("output_layers_prefix"), outputLayersPrefix_)) {
121  ROS_ERROR("Normal vectors filter did not find parameter `output_layers_prefix`.");
122  return false;
123  }
124  ROS_DEBUG("Normal vectors filter output_layer = %s.", outputLayersPrefix_.c_str());
125 
126  // If everything has been set up correctly
127  return true;
128 }
129 
130 template <typename T>
131 bool NormalVectorsFilter<T>::update(const T& mapIn, T& mapOut) {
132  std::vector<std::string> normalVectorsLayers;
133  normalVectorsLayers.push_back(outputLayersPrefix_ + "x");
134  normalVectorsLayers.push_back(outputLayersPrefix_ + "y");
135  normalVectorsLayers.push_back(outputLayersPrefix_ + "z");
136 
137  mapOut = mapIn;
138  for (const auto& layer : normalVectorsLayers) {
139  mapOut.add(layer);
140  }
141  switch (method_) {
142  case Method::AreaSerial:
144  break;
147  break;
150  break;
153  break;
154  }
155 
156  return true;
157 }
158 
159 // SVD Area based methods.
160 template <typename T>
161 void NormalVectorsFilter<T>::computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
162  const double start = ros::Time::now().toSec();
163 
164  // For each cell in submap.
165  for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
166  // Check if this is an empty cell (hole in the map).
167  if (map.isValid(*iterator, inputLayer)) {
168  const Index index(*iterator);
169  areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index);
170  }
171  }
172 
173  const double end = ros::Time::now().toSec();
174  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
175 }
176 
177 template <typename T>
178 void NormalVectorsFilter<T>::computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
179  const double start = ros::Time::now().toSec();
180  grid_map::Size gridMapSize = map.getSize();
181 
182  // Set number of thread to use for parallel programming.
183  std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
184  if (threadCount_ != -1) {
185  TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_));
186  }
187 
188  // Parallelized iteration through the map.
189  tbb::parallel_for(0, gridMapSize(0) * gridMapSize(1), [&](int range) {
190  // Recover Cell index from range iterator.
191  const Index index(range / gridMapSize(1), range % gridMapSize(1));
192  if (map.isValid(index, inputLayer)) {
193  areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index);
194  }
195  });
196 
197  const double end = ros::Time::now().toSec();
198  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
199 }
200 
201 template <typename T>
202 void NormalVectorsFilter<T>::areaSingleNormalComputation(GridMap& map, const std::string& inputLayer,
203  const std::string& outputLayersPrefix, const grid_map::Index& index) {
204  // Requested position (center) of circle in map.
205  Position center;
206  map.getPosition(index, center);
207 
208  // Prepare data computation. Check if area is bigger than cell.
209  const double minAllowedEstimationRadius = 0.5 * map.getResolution();
210  if (estimationRadius_ <= minAllowedEstimationRadius) {
211  ROS_WARN("Estimation radius is smaller than allowed by the map resolution (%f < %f)", estimationRadius_, minAllowedEstimationRadius);
212  }
213 
214  // Gather surrounding data.
215  size_t nPoints = 0;
216  Position3 sum = Position3::Zero();
217  Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero();
218  for (CircleIterator circleIterator(map, center, estimationRadius_); !circleIterator.isPastEnd(); ++circleIterator) {
219  Position3 point;
220  if (!map.getPosition3(inputLayer, *circleIterator, point)) {
221  continue;
222  }
223  nPoints++;
224  sum += point;
225  sumSquared.noalias() += point * point.transpose();
226  }
227 
228  Vector3 unitaryNormalVector = Vector3::Zero();
229  if (nPoints < 3) {
230  ROS_DEBUG("Not enough points to establish normal direction (nPoints = %i)", static_cast<int>(nPoints));
231  unitaryNormalVector = Vector3::UnitZ();
232  } else {
233  const Position3 mean = sum / nPoints;
234  const Eigen::Matrix3d covarianceMatrix = sumSquared / nPoints - mean * mean.transpose();
235 
236  // Compute Eigenvectors.
237  // Eigenvalues are ordered small to large.
238  // Worst case bound for zero eigenvalue from : https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
239  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
240  solver.computeDirect(covarianceMatrix, Eigen::DecompositionOptions::ComputeEigenvectors);
241  if (solver.eigenvalues()(1) > 1e-8) {
242  unitaryNormalVector = solver.eigenvectors().col(0);
243  } else { // If second eigenvalue is zero, the normal is not defined.
244  ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated.");
245  ROS_DEBUG("Expected cause: data is on a straight line (nPoints = %i)", static_cast<int>(nPoints));
246  unitaryNormalVector = Vector3::UnitZ();
247  }
248  }
249 
250  // Check direction of the normal vector and flip the sign towards the user defined direction.
251  if (unitaryNormalVector.dot(normalVectorPositiveAxis_) < 0.0) {
252  unitaryNormalVector = -unitaryNormalVector;
253  }
254 
255  map.at(outputLayersPrefix + "x", index) = unitaryNormalVector.x();
256  map.at(outputLayersPrefix + "y", index) = unitaryNormalVector.y();
257  map.at(outputLayersPrefix + "z", index) = unitaryNormalVector.z();
258 }
259 // Raster based methods.
260 template <typename T>
261 void NormalVectorsFilter<T>::computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
262  // Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
263  const double start = ros::Time::now().toSec();
264 
265  const grid_map::Size gridMapSize = map.getSize();
267  // Faster access to grid map values.
268  const grid_map::Matrix dataMap = map[inputLayer];
269  // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside.
270  const Index submapStartIndex(1, 1);
271  const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
272 
273  // For each cell in submap.
274  for (SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) {
275  const Index index(*iterator);
276  rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index);
277  }
278 
279  const double end = ros::Time::now().toSec();
280  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
281 }
282 
283 template <typename T>
284 void NormalVectorsFilter<T>::computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) {
285  const double start = ros::Time::now().toSec();
286 
287  const grid_map::Size gridMapSize = map.getSize();
289  // Faster access to grid map values if copy grid map layer into local matrix.
290  const grid_map::Matrix dataMap = map[inputLayer];
291  // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside.
292  const Index submapStartIndex(1, 1);
293  const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
294  if (submapBufferSize(1)!=0) {
295  // Set number of thread to use for parallel programming
296  std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
297  if (threadCount_ != -1) {
298  TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_));
299  }
300  // Parallelized iteration through the map.
301  tbb::parallel_for(0, submapBufferSize(0) * submapBufferSize(1), [&](int range) {
302  const Index index(range / submapBufferSize(1) + submapStartIndex(0), range % submapBufferSize(1) + submapStartIndex(1));
303  rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index);
304  });
305  } else {
306  ROS_ERROR("Grid map size is too small for normal raster computation");
307  }
308 
309  const double end = ros::Time::now().toSec();
310  ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start));
311 }
312 
313 template <typename T>
314 void NormalVectorsFilter<T>::rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix,
315  const grid_map::Matrix& dataMap, const grid_map::Index& index) {
316  // Inspiration for algorithm:
317  // http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
318  const double centralCell = dataMap(index(0), index(1));
319  double topCell = dataMap(index(0) - 1, index(1));
320  double rightCell = dataMap(index(0), index(1) + 1);
321  double bottomCell = dataMap(index(0) + 1, index(1));
322  double leftCell = dataMap(index(0), index(1) - 1);
323 
324  // Neighboring cells configuration checked in X and Y direction independently.
325  // Gridmap frame is defined rotated 90 degrees anticlockwise compared to direction of matrix if we take
326  // rows as Y coordinate and columns as X coordinate.
327  // In Y direction cell numbered as follows: left 0, center 1, right 2.
328  // In X direction cell numbered as follows: top 0, center 1, bottom 2.
329  // To find configuration we are in, multiply value of map.isValid of cell in question, True or False,
330  // by 2^(number of the cell).
331  // Each configuration will have a different number associated with it and then use a switch.
332 
333  const int configurationDirX = 1 * static_cast<int>(std::isfinite(topCell)) + 2 * static_cast<int>(std::isfinite(centralCell)) +
334  4 * static_cast<int>(std::isfinite(bottomCell));
335  const int configurationDirY = 1 * static_cast<int>(std::isfinite(leftCell)) + 2 * static_cast<int>(std::isfinite(centralCell)) +
336  4 * static_cast<int>(std::isfinite(rightCell));
337 
338  // If outer cell height value is missing use the central value, however the formula for the normal calculation
339  // has to take into account that the distance of the cells used for normal calculation is different.
340  bool validConfiguration = true;
341  double distanceX;
342  switch (configurationDirX) {
343  case 7: // All 3 cell height values are valid.
344  distanceX = 2 * gridMapResolution_; // Top and bottom cell centers are 2 cell resolution distant.
345  break;
346  case 6: // Top cell height value not valid.
347  topCell = centralCell;
348  distanceX = gridMapResolution_;
349  break;
350  case 5: // Central cell height value not valid. Not a problem.
351  distanceX = 2 * gridMapResolution_;
352  break;
353  case 3: // Bottom cell height value not valid.
354  bottomCell = centralCell;
355  distanceX = gridMapResolution_;
356  break;
357 
358  default: // More than 1 cell height values are not valid, normal vector will not be calculated in this location.
359  validConfiguration = false;
360  }
361 
362  double distanceY;
363  switch (configurationDirY) {
364  case 7: // All 3 cell height values are valid.
365  distanceY = 2 * gridMapResolution_; // Left and right cell centers are 2 call resolution distant.
366  break;
367  case 6: // Left cell height value not valid.
368  leftCell = centralCell;
369  distanceY = gridMapResolution_;
370  break;
371  case 5: // Central cell height value not valid. Not a problem.
372  distanceY = 2 * gridMapResolution_;
373  break;
374  case 3: // Right cell height value not valid.
375  rightCell = centralCell;
376  distanceY = gridMapResolution_;
377  break;
378 
379  default: // More than 1 cell height values are not valid, normal vector will not be calculated in this location.
380  validConfiguration = false;
381  }
382 
383  if (validConfiguration) {
384  // Normal vector initialization
385  Vector3 normalVector = Vector3::Zero();
386  // X DIRECTION
387  normalVector(0) = (bottomCell - topCell) / distanceX;
388  // Y DIRECTION
389  normalVector(1) = (rightCell - leftCell) / distanceY;
390  // Z DIRECTION
391  normalVector(2) = +1;
392 
393  normalVector.normalize();
394 
395  // Check direction of the normal vector and flip the sign towards the user defined direction.
396  if (normalVector.dot(normalVectorPositiveAxis_) < 0.0) {
397  normalVector = -normalVector;
398  }
399 
400  map.at(outputLayersPrefix + "x", index) = normalVector.x();
401  map.at(outputLayersPrefix + "y", index) = normalVector.y();
402  map.at(outputLayersPrefix + "z", index) = normalVector.z();
403  }
404 }
405 
406 } // namespace grid_map
407 
Eigen::Array2i Index
std::string outputLayersPrefix_
Output layer name.
void areaSingleNormalComputation(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix, const grid_map::Index &index)
bool getPosition(const Index &index, Position &position) const
#define ROS_DEBUG_THROTTLE(rate,...)
std::string inputLayer_
Input layer name.
Eigen::Array2i Size
Eigen::MatrixXf Matrix
void computeWithAreaParallel(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
#define ROS_WARN(...)
bool parallelizationEnabled_
Parameter that specifies whether to parallelize or not.
bool isValid(const Index &index) const
Eigen::Vector3d Position3
void computeWithRasterParallel(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
Eigen::Vector3d normalVectorPositiveAxis_
Normal vector positive axis.
double getResolution() const
Eigen::Vector2d Position
float & at(const std::string &layer, const Index &index)
void computeWithAreaSerial(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
void rasterSingleNormalComputation(GridMap &map, const std::string &outputLayersPrefix, const grid_map::Matrix &dataMap, const grid_map::Index &index)
bool update(const T &mapIn, T &mapOut) override
int threadCount_
Parameter that specifies the number of thread used.
double estimationRadius_
Radius of submap for normal vector estimation.
static Time now()
Eigen::Vector3d Vector3
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
double gridMapResolution_
Grid Map Resolution.
const Size & getSize() const
void computeWithRasterSerial(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
#define ROS_DEBUG(...)


grid_map_filters
Author(s): P├ęter Fankhauser , Martin Wermelinger
autogenerated on Fri Feb 19 2021 03:15:18