#include <SignedDistanceField.hpp>

Public Types
using	Derivative3 = Eigen::Vector3d

Public Member Functions
Derivative3	derivative (const Position3 &position) const noexcept

void	filterPoints (std::function< void(const Position3 &, float, const Derivative3 &)> func, size_t decimation=1) const

const std::string &	getFrameId () const noexcept

Time	getTime () const noexcept

	SignedDistanceField (const GridMap &gridMap, const std::string &elevationLayer, double minHeight, double maxHeight)

size_t	size () const noexcept

double	value (const Position3 &position) const noexcept

std::pair< double, Derivative3 >	valueAndDerivative (const Position3 &position) const noexcept

Private Types
using	node_data_t = std::array< float, 4 >
	Data structure to store together {signed distance value, derivative}. More...

Private Member Functions
void	computeLayerSdfandDeltaX (const Matrix &elevation, Matrix &currentLayer, Matrix &dxTranspose, Matrix &sdfTranspose, Matrix &tmp, Matrix &tmpTranspose, float height, float resolution, float minHeight, float maxHeight) const

void	computeSignedDistance (const Matrix &elevation)

void	emplacebackLayerData (const Matrix &signedDistance, const Matrix &dxTranspose, const Matrix &dy, const Matrix &dz)

Static Private Member Functions
static Derivative3	derivative (const node_data_t &nodeData) noexcept

static double	distance (const node_data_t &nodeData) noexcept

static float	distanceFloat (const node_data_t &nodeData) noexcept

Private Attributes
std::vector< node_data_t >	data_
	Object encoding the signed distance value and derivative in the grid. More...

std::string	frameId_
	Frame id of the grid map. More...

signed_distance_field::Gridmap3dLookup	gridmap3DLookup_
	Object encoding the 3D grid. More...

Time	timestamp_
	Timestamp of the grid map (nanoseconds). More...

Detailed Description

This class creates a dense 3D signed distance field grid for a given elevation map. The 3D grid uses the same resolution as the 2D map. i.e. all voxels are of size (resolution x resolution x resolution). The size of the 3D grid is the same as the map in XY direction. The size in Z direction is specified in the constructor.

During the creation of the class all values and derivatives are pre-computed in one go. This makes repeated lookups very cheap. Querying a point outside of the constructed grid will result in linear extrapolation.

The distance value and derivatives (dx,dy,dz) per voxel are stored next to each other in memory to support fast lookup during interpolation, where we need all 4 values simultaneously. The entire dense grid is stored as a flat vector, with the indexing outsourced to the Gridmap3dLookup class.

Definition at line 34 of file SignedDistanceField.hpp.

Member Typedef Documentation

◆ Derivative3

using grid_map::SignedDistanceField::Derivative3 = Eigen::Vector3d

Definition at line 37 of file SignedDistanceField.hpp.

◆ node_data_t

using grid_map::SignedDistanceField::node_data_t = std::array<float, 4>

private

Data structure to store together {signed distance value, derivative}.

Definition at line 117 of file SignedDistanceField.hpp.

Constructor & Destructor Documentation

◆ SignedDistanceField()

grid_map::SignedDistanceField::SignedDistanceField	(	const GridMap &	gridMap,
		const std::string &	elevationLayer,
		double	minHeight,
		double	maxHeight
	)

Create a signed distance field and its derivative for an elevation layer in the grid map.

Parameters

gridMap	: Input map to create the SDF for.
elevationLayer	: Name of the elevation layer.
minHeight	: Desired starting height of the 3D SDF grid.
maxHeight	: Desired ending height of the 3D SDF grid. (Will be rounded up to match the resolution)

Definition at line 25 of file SignedDistanceField.cpp.

Member Function Documentation

◆ computeLayerSdfandDeltaX()

void grid_map::SignedDistanceField::computeLayerSdfandDeltaX	(	const Matrix &	elevation,
		Matrix &	currentLayer,
		Matrix &	dxTranspose,
		Matrix &	sdfTranspose,
		Matrix &	tmp,
		Matrix &	tmpTranspose,
		float	height,
		float	resolution,
		float	minHeight,
		float	maxHeight
	)		const

private

Simultaneously compute the signed distance and derivative in x direction at a given height

Parameters

elevation	[in] : elevation data
currentLayer	[out] : signed distance values
dxTranspose	[out] : derivative in x direction (the matrix is transposed)
sdfTranspose	[tmp] : temporary variable to reuse between calls (allocated on first use)
tmp	[tmp] : temporary variable (allocated on first use)
tmpTranspose	[tmp] : temporary variable (allocated on first use)
height	[in] : height the signed distance is created for.
resolution	[in] : resolution of the map.
minHeight	[in] : smallest height value in the elevation data.
maxHeight	[in] : largest height value in the elevation data.

Definition at line 156 of file SignedDistanceField.cpp.

◆ computeSignedDistance()

void grid_map::SignedDistanceField::computeSignedDistance ( const Matrix & elevation )

private

Implementation of the signed distance field computation in this class.

Parameters

elevation

Definition at line 80 of file SignedDistanceField.cpp.

◆ derivative() [1/2]

SignedDistanceField::Derivative3 grid_map::SignedDistanceField::derivative ( const Position3 & position ) const

noexcept

Get the signed distance derivative at a 3D position.

Parameters

position : 3D position in the frame of the gridmap.

Returns: derivative of the signed distance field.

Definition at line 66 of file SignedDistanceField.cpp.

◆ derivative() [2/2]

static Derivative3 grid_map::SignedDistanceField::derivative ( const node_data_t & nodeData )

inlinestaticprivatenoexcept

Helper function to extract the sdf derivative

Definition at line 126 of file SignedDistanceField.hpp.

◆ distance()

static double grid_map::SignedDistanceField::distance ( const node_data_t & nodeData )

inlinestaticprivatenoexcept

Helper function to extract the sdf value

Definition at line 120 of file SignedDistanceField.hpp.

◆ distanceFloat()

static float grid_map::SignedDistanceField::distanceFloat ( const node_data_t & nodeData )

inlinestaticprivatenoexcept

Helper function to extract the sdf value as float

Definition at line 123 of file SignedDistanceField.hpp.

◆ emplacebackLayerData()

void grid_map::SignedDistanceField::emplacebackLayerData	(	const Matrix &	signedDistance,
		const Matrix &	dxTranspose,
		const Matrix &	dy,
		const Matrix &	dz
	)

private

Add the computed signed distance values and derivatives at a particular height to the grid. Adds the layer to the back of data_

Parameters

signedDistance	: signed distance values.
dxTranspose	: x components of the derivative (matrix is transposed).
dy	: y components of the derivative.
dz	: z components of the derivative.

Definition at line 165 of file SignedDistanceField.cpp.

◆ filterPoints()

void grid_map::SignedDistanceField::filterPoints	(	std::function< void(const Position3 &, float, const Derivative3 &)>	func,
		size_t	decimation = `1`
	)		const

Calls a function on each point in the signed distance field. The points are processed in the order they are stored in memory.

Parameters

func	: function taking the node position, signed distance value, and signed distance derivative.
decimation	: specifies how many points are returned. 1: all points, 2: every second point, etc.

Definition at line 186 of file SignedDistanceField.cpp.

◆ getFrameId()

const std::string & grid_map::SignedDistanceField::getFrameId ( ) const

noexcept

Definition at line 178 of file SignedDistanceField.cpp.

◆ getTime()

Time grid_map::SignedDistanceField::getTime ( ) const

noexcept

Definition at line 182 of file SignedDistanceField.cpp.

◆ size()

size_t grid_map::SignedDistanceField::size ( ) const

noexcept

Definition at line 174 of file SignedDistanceField.cpp.

◆ value()

double grid_map::SignedDistanceField::value ( const Position3 & position ) const

noexcept

Get the signed distance value at a 3D position.

Parameters

position : 3D position in the frame of the gridmap.

Returns: signed distance to the elevation surface.

Definition at line 58 of file SignedDistanceField.cpp.

◆ valueAndDerivative()

std::pair< double, SignedDistanceField::Derivative3 > grid_map::SignedDistanceField::valueAndDerivative ( const Position3 & position ) const

noexcept

Get both the signed distance value and derivative at a 3D position.

Parameters

position : 3D position in the frame of the gridmap.

Returns: {value, derivative} of the signed distance field.

Definition at line 72 of file SignedDistanceField.cpp.

Member Data Documentation

◆ data_

std::vector<node_data_t> grid_map::SignedDistanceField::data_

private

Object encoding the signed distance value and derivative in the grid.

Definition at line 132 of file SignedDistanceField.hpp.

◆ frameId_

std::string grid_map::SignedDistanceField::frameId_

private

Frame id of the grid map.

Definition at line 135 of file SignedDistanceField.hpp.

◆ gridmap3DLookup_

signed_distance_field::Gridmap3dLookup grid_map::SignedDistanceField::gridmap3DLookup_

private

Object encoding the 3D grid.

Definition at line 129 of file SignedDistanceField.hpp.

◆ timestamp_

Time grid_map::SignedDistanceField::timestamp_

private

Timestamp of the grid map (nanoseconds).

Definition at line 138 of file SignedDistanceField.hpp.

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

Public Types

Public Member Functions

Private Types

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ Derivative3

◆ node_data_t

Constructor & Destructor Documentation

◆ SignedDistanceField()

Member Function Documentation

◆ computeLayerSdfandDeltaX()

◆ computeSignedDistance()

◆ derivative() [1/2]

◆ derivative() [2/2]

◆ distance()

◆ distanceFloat()

◆ emplacebackLayerData()

◆ filterPoints()

◆ getFrameId()

◆ getTime()

◆ size()

◆ value()

◆ valueAndDerivative()

Member Data Documentation

◆ data_

◆ frameId_

◆ gridmap3DLookup_

◆ timestamp_