#include <NormalVectorsFilter.hpp>

Inheritance diagram for grid_map::NormalVectorsFilter< T >:

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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output layer name prefix.
index	Index 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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output 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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output 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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output 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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output 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

map	grid map containing the layer for which the normal vectors are computed for.
inputLayer	Layer the normal vector should be computed for.
outputLayersPrefix	Output layer name prefix.
dataMap	Matrix containing the input layer of the grid map in question.
index	Index 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

mapIn	grid map containing the layer for which the normal vectors are computed for.
mapOut	grid 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:

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

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

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

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