10 #include <Eigen/Dense> 14 #include <tbb/task_scheduler_init.h> 25 : method_(
Method::RasterSerial), estimationRadius_(0.0), parallelizationEnabled_(false), threadCount_(1), gridMapResolution_(0.02) {}
33 std::string algorithm;
35 ROS_WARN(
"Could not find the parameter: `algorithm`. Setting to default value: 'area'.");
42 if (algorithm !=
"raster") {
45 ROS_WARN(
"Could not find the parameter: `radius`. Switching to raster method.");
51 ROS_WARN(
"Parameter `radius` is not positive. Switching to raster method.");
59 ROS_WARN(
"Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.");
67 ROS_WARN(
"Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.");
74 if (algorithm ==
"raster") {
96 std::string normalVectorPositiveAxis;
98 ROS_ERROR(
"Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
101 if (normalVectorPositiveAxis ==
"z") {
103 }
else if (normalVectorPositiveAxis ==
"y") {
105 }
else if (normalVectorPositiveAxis ==
"x") {
108 ROS_ERROR(
"The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
114 ROS_ERROR(
"Normal vectors filter did not find parameter `input_layer`.");
121 ROS_ERROR(
"Normal vectors filter did not find parameter `output_layers_prefix`.");
130 template <
typename T>
132 std::vector<std::string> normalVectorsLayers;
138 for (
const auto& layer : normalVectorsLayers) {
160 template <
typename T>
167 if (map.
isValid(*iterator, inputLayer)) {
168 const Index index(*iterator);
177 template <
typename T>
183 std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
185 TBBInitPtr.reset(
new tbb::task_scheduler_init(
threadCount_));
189 tbb::parallel_for(0, gridMapSize(0) * gridMapSize(1), [&](
int range) {
191 const Index index(range / gridMapSize(1), range % gridMapSize(1));
192 if (map.
isValid(index, inputLayer)) {
201 template <
typename T>
209 const double minAllowedEstimationRadius = 0.5 * map.
getResolution();
211 ROS_WARN(
"Estimation radius is smaller than allowed by the map resolution (%f < %f)",
estimationRadius_, minAllowedEstimationRadius);
217 Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero();
220 if (!map.
getPosition3(inputLayer, *circleIterator, point)) {
225 sumSquared.noalias() += point * point.transpose();
228 Vector3 unitaryNormalVector = Vector3::Zero();
230 ROS_DEBUG(
"Not enough points to establish normal direction (nPoints = %i)", static_cast<int>(nPoints));
231 unitaryNormalVector = Vector3::UnitZ();
234 const Eigen::Matrix3d covarianceMatrix = sumSquared / nPoints - mean * mean.transpose();
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);
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();
252 unitaryNormalVector = -unitaryNormalVector;
255 map.
at(outputLayersPrefix +
"x", index) = unitaryNormalVector.x();
256 map.
at(outputLayersPrefix +
"y", index) = unitaryNormalVector.y();
257 map.
at(outputLayersPrefix +
"z", index) = unitaryNormalVector.z();
260 template <
typename T>
270 const Index submapStartIndex(1, 1);
271 const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
275 const Index index(*iterator);
283 template <
typename T>
292 const Index submapStartIndex(1, 1);
293 const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2);
294 if (submapBufferSize(1)!=0) {
296 std::unique_ptr<tbb::task_scheduler_init> TBBInitPtr;
298 TBBInitPtr.reset(
new tbb::task_scheduler_init(
threadCount_));
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));
306 ROS_ERROR(
"Grid map size is too small for normal raster computation");
313 template <
typename T>
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);
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));
340 bool validConfiguration =
true;
342 switch (configurationDirX) {
347 topCell = centralCell;
354 bottomCell = centralCell;
359 validConfiguration =
false;
363 switch (configurationDirY) {
368 leftCell = centralCell;
375 rightCell = centralCell;
380 validConfiguration =
false;
383 if (validConfiguration) {
385 Vector3 normalVector = Vector3::Zero();
387 normalVector(0) = (bottomCell - topCell) / distanceX;
389 normalVector(1) = (rightCell - leftCell) / distanceY;
391 normalVector(2) = +1;
393 normalVector.normalize();
397 normalVector = -normalVector;
400 map.
at(outputLayersPrefix +
"x", index) = normalVector.x();
401 map.
at(outputLayersPrefix +
"y", index) = normalVector.y();
402 map.
at(outputLayersPrefix +
"z", index) = normalVector.z();
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.
void computeWithAreaParallel(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
bool parallelizationEnabled_
Parameter that specifies whether to parallelize or not.
bool isValid(const Index &index) const
Eigen::Vector3d Position3
bool configure() override
void computeWithRasterParallel(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
Eigen::Vector3d normalVectorPositiveAxis_
Normal vector positive axis.
double getResolution() const
float & at(const std::string &layer, const Index &index)
void computeWithAreaSerial(GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
~NormalVectorsFilter() override
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.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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)