Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
grid_map::NormalVectorsFilter< T > Class Template Reference

#include <NormalVectorsFilter.hpp>

Inheritance diagram for grid_map::NormalVectorsFilter< T >:
Inheritance graph
[legend]

Public Member Functions

bool configure () override
 
 NormalVectorsFilter ()
 
bool update (const T &mapIn, T &mapOut) override
 
 ~NormalVectorsFilter () override
 
- Public Member Functions inherited from filters::FilterBase< T >
bool configure (const std::string &param_name, ros::NodeHandle node_handle=ros::NodeHandle())
 
bool configure (XmlRpc::XmlRpcValue &config)
 
 FilterBase ()
 
const std::string & getName ()
 
std::string getType ()
 
virtual ~FilterBase ()
 

Private Types

enum  Method { Method::AreaSerial, Method::AreaParallel, Method::RasterSerial, Method::RasterParallel }
 

Private Member Functions

void areaSingleNormalComputation (GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix, const grid_map::Index &index)
 
void computeWithAreaParallel (GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
 
void computeWithAreaSerial (GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
 
void computeWithRasterParallel (GridMap &map, const std::string &inputLayer, const std::string &outputLayersPrefix)
 
void computeWithRasterSerial (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)
 

Private Attributes

double estimationRadius_
 Radius of submap for normal vector estimation. More...
 
double gridMapResolution_
 Grid Map Resolution. More...
 
std::string inputLayer_
 Input layer name. More...
 
Method method_
 
Eigen::Vector3d normalVectorPositiveAxis_
 Normal vector positive axis. More...
 
std::string outputLayersPrefix_
 Output layer name. More...
 
bool parallelizationEnabled_
 Parameter that specifies whether to parallelize or not. More...
 
int threadCount_
 Parameter that specifies the number of thread used. More...
 

Additional Inherited Members

- Protected Member Functions inherited from filters::FilterBase< T >
bool getParam (const std::string &name, std::string &value)
 
bool getParam (const std::string &name, XmlRpc::XmlRpcValue &value)
 
bool getParam (const std::string &name, double &value)
 
bool getParam (const std::string &name, std::vector< double > &value)
 
bool getParam (const std::string &name, unsigned int &value)
 
bool getParam (const std::string &name, int &value)
 
bool getParam (const std::string &name, std::vector< std::string > &value)
 
bool getParam (const std::string &name, bool &value)
 
bool loadConfiguration (XmlRpc::XmlRpcValue &config)
 
- Protected Attributes inherited from filters::FilterBase< T >
bool configured_
 
std::string filter_name_
 
std::string filter_type_
 
string_map_t params_
 

Detailed Description

template<typename T>
class grid_map::NormalVectorsFilter< T >

Compute the normal vectors of a layer in a map.

Definition at line 23 of file NormalVectorsFilter.hpp.

Member Enumeration Documentation

template<typename T >
enum grid_map::NormalVectorsFilter::Method
strongprivate
Enumerator
AreaSerial 
AreaParallel 
RasterSerial 
RasterParallel 

Definition at line 165 of file NormalVectorsFilter.hpp.

Constructor & Destructor Documentation

template<typename T >
grid_map::NormalVectorsFilter< T >::NormalVectorsFilter ( )

Constructor

Definition at line 24 of file NormalVectorsFilter.cpp.

template<typename T >
grid_map::NormalVectorsFilter< T >::~NormalVectorsFilter ( )
overridedefault

Destructor.

Member Function Documentation

template<typename T >
void grid_map::NormalVectorsFilter< T >::areaSingleNormalComputation ( GridMap map,
const std::string &  inputLayer,
const std::string &  outputLayersPrefix,
const grid_map::Index index 
)
private

Estimate the normal vector at one point of the input layer, specified by the index parameter, by using points within a circle of specified radius.

The eigen decomposition of the covariance matrix (3x3) of all data points is used to establish the normal direction. Four cases can be identified when the eigenvalues are ordered in ascending order: 1) The data is in a cloud -> all eigenvalues are non-zero 2) The data is on a plane -> The first eigenvalue is zero 3) The data is on a line -> The first two eigenvalues are zero. 4) The data is in one point -> All eigenvalues are zero

Only case 1 & 2 provide enough information the establish a normal direction. The degenerate cases (3 or 4) are identified by checking if the second eigenvalue is zero.

The numerical threshold (1e-8) for the eigenvalue being zero is given by the accuracy of the decomposition, as reported by Eigen: https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html

Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.
indexIndex of point in the grid map for which this function calculates the normal vector.

Definition at line 202 of file NormalVectorsFilter.cpp.

template<typename T >
void grid_map::NormalVectorsFilter< T >::computeWithAreaParallel ( GridMap map,
const std::string &  inputLayer,
const std::string &  outputLayersPrefix 
)
private

Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function. This function makes use of the area method and is the parallel version of such normal vector computation using a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.

Definition at line 178 of file NormalVectorsFilter.cpp.

template<typename T >
void grid_map::NormalVectorsFilter< T >::computeWithAreaSerial ( GridMap map,
const std::string &  inputLayer,
const std::string &  outputLayersPrefix 
)
private

Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function. This function makes use of the area method and is the serial version of such normal vector computation using a simple for cycle to iterate over the whole map.

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.

Definition at line 161 of file NormalVectorsFilter.cpp.

template<typename T >
void grid_map::NormalVectorsFilter< T >::computeWithRasterParallel ( GridMap map,
const std::string &  inputLayer,
const std::string &  outputLayersPrefix 
)
private

Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function. This function makes use of the raster method and is the parallel version of such normal vector computation using a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.

Definition at line 284 of file NormalVectorsFilter.cpp.

template<typename T >
void grid_map::NormalVectorsFilter< T >::computeWithRasterSerial ( GridMap map,
const std::string &  inputLayer,
const std::string &  outputLayersPrefix 
)
private

Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function. This function makes use of the raster method and is the serial version of such normal vector computation using a simple for cycle to iterate over the whole map.

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.

Definition at line 261 of file NormalVectorsFilter.cpp.

template<typename T >
bool grid_map::NormalVectorsFilter< T >::configure ( )
overridevirtual

Configures the filter from parameters on the Parameter Server. This comprehend in order: 1) algorithm, used for choosing between area and raster algorithm (default=area) 2)If algorithm is not raster estimationRadius_ is read an a basic check on its sign is made. If something is wrong it switches to raster algorithm. 3) parallelization_enabled, used for choosing between serial and parallel algorithm (default=false) 4) thread_count, used to set the number of threads to be used in parallelization is enabled (default=auto) The parallelization_enabled and algorithm parameters allow to choose between the 4 different methods { AreaSerial, AreaParallel, RasterSerial, RasterParallel } 4) normal_vector_positive_axis, used to define the upward positive direction for the normals 5) input_layer, defines in which layer of the grid map lie the information needed (usually elevation or elevation_filtered) 6) output_layers_prefix, defines the prefix for the new 3 layers (x,y,z) that will define the normal vectors Those parameters have to be written in a .yaml file that will be processed as a sequence of filters. An example can be seen in grid_map_demos/config/normal_filter_comparison.yaml.

Implements filters::FilterBase< T >.

Definition at line 31 of file NormalVectorsFilter.cpp.

template<typename T >
void grid_map::NormalVectorsFilter< T >::rasterSingleNormalComputation ( GridMap map,
const std::string &  outputLayersPrefix,
const grid_map::Matrix dataMap,
const grid_map::Index index 
)
private

Estimate the normal vector at one point of the input layer, specified by the index parameter, by using neighboring points around the point for which the normal vector is being calculated.

The neighboring cells are used to linearly approximate the first derivative in both directions.

|T| ^ |L|C|R| | x Axis |B| | <----—| Y Axis

Those values are then used to reconstruct the normal vector in the point of interest. This algorithm doesn't make use of the central cell if all the neighboring point heights are present. However, thanks to that, it can accommodate missing point heights, up to one in each direction, by using exactly the central height value. In this case, the resulting approximation, will suffer a loss of quality depending on the local surface shape. This implementation skips the outermost values in order to avoid checks on the validity of height values and therefore have a faster algorithm.

Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml

Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"

Parameters
mapgrid map containing the layer for which the normal vectors are computed for.
inputLayerLayer the normal vector should be computed for.
outputLayersPrefixOutput layer name prefix.
dataMapMatrix containing the input layer of the grid map in question.
indexIndex of point in the grid map for which this function calculates the normal vector.

Definition at line 314 of file NormalVectorsFilter.cpp.

template<typename T >
bool grid_map::NormalVectorsFilter< T >::update ( const T &  mapIn,
T &  mapOut 
)
overridevirtual

Compute the normal vectors of a layer in a map and saves it as additional grid map layer.

Parameters
mapIngrid map containing the layer for which the normal vectors are computed for.
mapOutgrid map containing mapIn and the new layers for the normal vectors.

Implements filters::FilterBase< T >.

Definition at line 131 of file NormalVectorsFilter.cpp.

Member Data Documentation

template<typename T >
double grid_map::NormalVectorsFilter< T >::estimationRadius_
private

Radius of submap for normal vector estimation.

Definition at line 170 of file NormalVectorsFilter.hpp.

template<typename T >
double grid_map::NormalVectorsFilter< T >::gridMapResolution_
private

Grid Map Resolution.

Definition at line 188 of file NormalVectorsFilter.hpp.

template<typename T >
std::string grid_map::NormalVectorsFilter< T >::inputLayer_
private

Input layer name.

Definition at line 182 of file NormalVectorsFilter.hpp.

template<typename T >
Method grid_map::NormalVectorsFilter< T >::method_
private

Definition at line 167 of file NormalVectorsFilter.hpp.

template<typename T >
Eigen::Vector3d grid_map::NormalVectorsFilter< T >::normalVectorPositiveAxis_
private

Normal vector positive axis.

Definition at line 179 of file NormalVectorsFilter.hpp.

template<typename T >
std::string grid_map::NormalVectorsFilter< T >::outputLayersPrefix_
private

Output layer name.

Definition at line 185 of file NormalVectorsFilter.hpp.

template<typename T >
bool grid_map::NormalVectorsFilter< T >::parallelizationEnabled_
private

Parameter that specifies whether to parallelize or not.

Definition at line 173 of file NormalVectorsFilter.hpp.

template<typename T >
int grid_map::NormalVectorsFilter< T >::threadCount_
private

Parameter that specifies the number of thread used.

Definition at line 176 of file NormalVectorsFilter.hpp.


The documentation for this class was generated from the following files:


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