Class that provides several methods for running collision tests of a rigid models footprint with images. More...

#include <model/CollisionTest.h>

Public Member Functions
	CollisionTest (float cellSize=0.02f, uint32 segments=360)

	CollisionTest (const CollisionTest &other)

template<typename Reflector >
void	reflect (Reflector &r)

CollisionTest &	operator= (const CollisionTest &other)

bool	testCollision (const Img8U1 &map, const Point2i &p, float phi) const
	Tests collision of the enclosed footprint with a given map for rotation phi. More...

float	distanceToObstacle (const Img32F1 &distanceMap, const Point2i &p, float phi, Point2i *oClosestModelPoint=NULL) const
	Calculates the distance of the enclosed footprint to the nearest obstacle in the given map. More...

void	distanceTransform (const Img8U1 &map, Img32F1 &oDistanceMap) const
	Calculates a distance transformation on the given map. More...

void	distanceTransform (const Img8U1 &map, Img32F1 &oDistanceMap, Img< int32 > &oLabelMap) const
	Same as distanceTransform(const Img8U1&, Img32F1&) const, but also returns a label map which allows to determine the position of the obstacle in addition to its distance. More...

void	distanceTransformAndClearShape (const Img8U1 &map, Img32F1 &oDistanceMap, const Point2i &p, float phi) const
	Calculates a distance transformation on the given map and clears the rotated footprint at position p in the map. More...

void	distanceTransformAndClearShape (const Img8U1 &map, Img32F1 &oDistanceMap, Img< int32 > &oLabelMap, const Point2i &p, float phi) const
	Same as distanceTransformAndClearShape(const Img8U1&, Img32F1&, const Point2i&, float) const, but also returns a label map which allows to determine the position of the obstacle in addition to its distance. More...

const Img8U1 &	getShape (float phi) const
	Return the entry of the LUT that is closest to the representation of the enclosed footprint rotated about phi. More...

const Rect2i &	getShapeBB (float phi) const
	Return the bounding box for the shape for phi. More...

void	initialize (const Footprint &footprint)
	Initialize collision testing with the given footprint. More...

void	initialize (const Footprint &footprint, float cellSize)
	Initialize collision testing with the given footprint and cell size. More...

void	initialize (const Footprint &footprint, float cellSize, uint32 segments)
	Initialize collision testing with the given footprint, cell size and angular precision. More...

void	clearShape (Img8U1 &map, const Point2i &p, float phi, uint8 freeValue=0) const
	Clears the map region that is covered by the rotated footprint at position p in the given map. More...

void	decayShape (Img8U1 &map, const Point2i &p, float phi, float decayRate, float neutral=127.f, bool decayObstacles=true, bool decayFreespace=false, bool invertShape=false) const
	Decays the map region that is covered by the rotated footprint at position p in the given map. More...

Static Public Member Functions
static boost::optional< Point2i >	obstaclePosition (const Img32F1 &distanceMap, const Img< int32 > &labelMap, const Point2i &closestModelPoint, std::map< int, Point2i > *ioCache=NULL)
	Determine the position of the obstacle. More...

Protected Member Functions
int	calculateSegment (float phi) const

std::pair< Point2i, Rect2i >	calculateOffsetAndOverlap (const Size2i &map, const Point2i &p, float phi) const

Protected Attributes
float	mCellSize
	Resolution of the collision map [m]. More...

uint32	mSegments
	Size of the collision LUT (360° is divided in #segments) More...

std::vector< Img8U1 >	mShapeLUT
	Look up table for faster collision checking. More...

std::vector< Rect2i >	mShapeLUTBB
	Bounding box for each precalculated shape. More...

int	mShapeDimension
	Each LUT entry has this dimension (e.g. is a dim x dim image) More...

int	mObstacleThreshold
	Occupancy grid values above this threshold are counted as obstacles. More...

float	mSegmentStep
	Denotes a step in rad between each LUT entry (2pi / mSegments) More...

Detailed Description

Class that provides several methods for running collision tests of a rigid models footprint with images.

For optimization a LUT is build for a provided footprint. After initializing the LUT collision checks can be done for an arbitrary rotation of the footprint. This class is used for collision detection and avoidance in navigation tasks.

Note: Images and maps (e.g. GridMap) have different coordinate systems. When using this class with images note that the upper left corner is 0,0 and the lower right corner is xsize, ysize. The phi parameter used in the methods of this class rotates the shape clockwise starting with 0° at 3 o'clock. Maps are internally images where y is flipped so the upper left corner is 0, ysize and the lower right is xsize, 0. So phi rotates counterclockwise starting with 0° at 3 o'clock. (This is handled automatically so no need to worry)

Constructor & Destructor Documentation

◆ CollisionTest() [1/2]

CollisionTest	(	float	cellSize = `0.02f`,
		uint32	segments = `360`
	)

inline

◆ CollisionTest() [2/2]

CollisionTest ( const CollisionTest & other )

inline

Member Function Documentation

◆ reflect()

void reflect ( Reflector & r )

inline

◆ operator=()

CollisionTest& operator= ( const CollisionTest & other )

inline

◆ testCollision()

bool testCollision	(	const Img8U1 &	map,
		const Point2i &	p,
		float	phi
	)		const

Tests collision of the enclosed footprint with a given map for rotation phi.

Parameters

map	The map in which the collision check is done
p	The position of the model in the map
phi	The orientation of the model in the map

Returns: true if the footprint rotated about phi, at position p collides with an obstacle in the map (footprint contains 255 at a pixel where the map contains a value > mObstacleThreshold)

◆ distanceToObstacle()

float distanceToObstacle	(	const Img32F1 &	distanceMap,
		const Point2i &	p,
		float	phi,
		Point2i *	oClosestModelPoint = `NULL`
	)		const

Calculates the distance of the enclosed footprint to the nearest obstacle in the given map.

Parameters

distanceMap	The distance transformed map in which the collision check is done
p	The position of the model in the map
phi	The orientation of the model in the map
oClosestModelPoint	Optionally returns the position of the point on the model with closest obstacle proximity. Can be used to find the actual obstacle position using obstaclePosition.

Returns: distance in pixel between the shape pixel in the footprint rotated about phi, at position p that is closest to an obstacle in the map.

◆ obstaclePosition()

static boost::optional<Point2i> obstaclePosition	(	const Img32F1 &	distanceMap,
		const Img< int32 > &	labelMap,
		const Point2i &	closestModelPoint,
		std::map< int, Point2i > *	ioCache = `NULL`
	)

static

Determine the position of the obstacle.

Parameters

distanceMap	The distance transformed map in which the collision check was done
labelMap	The label map returned by the distance transformation
closestModelPoint	The position on the model with closest obstacle proximity (returned by distanceToObstacle())
ioCache	Optionally, obstacle points corresponding to labels can be cached for speedup. The cache is associated with the map (not shape) and therefore held outside. It is queried and updated inside this function. Cache must be cleared when distance transformation is recomputed.

Returns: Position of the obstacle in the map (or empty if there are no obstacles in the map at all)

Exceptions

XRuntime Obstacle should be there but position was not found in map

◆ distanceTransform() [1/2]

void distanceTransform	(	const Img8U1 &	map,
		Img32F1 &	oDistanceMap
	)		const

Calculates a distance transformation on the given map.

In the distance transformed map each pixel contains the distance value to the closest obstacle (denoted by values > mObstacleThreshold in the map). This allows fast retrieval of distances between the robots footprint (a shape in the LUT) and an obstacle by using the distanceToObstacle() method.

Parameters

map	The map to transform
oDistanceMap	The resulting distance transformed map

◆ distanceTransform() [2/2]

void distanceTransform	(	const Img8U1 &	map,
		Img32F1 &	oDistanceMap,
		Img< int32 > &	oLabelMap
	)		const

Same as distanceTransform(const Img8U1&, Img32F1&) const, but also returns a label map which allows to determine the position of the obstacle in addition to its distance.

Parameters

map	The map to transform
oDistanceMap	The resulting distance transformed map
oLabelMap	The resulting label map

Exceptions

XNotImplemented Not implemented (available OpenCV version is < 2.4)

◆ distanceTransformAndClearShape() [1/2]

void distanceTransformAndClearShape	(	const Img8U1 &	map,
		Img32F1 &	oDistanceMap,
		const Point2i &	p,
		float	phi
	)		const

Calculates a distance transformation on the given map and clears the rotated footprint at position p in the map.

In the distance transformed map each pixel contains the distance value to the closest obstacle (denoted by values > mObstacleThreshold in the map). This allows fast retrieval of distances between the robots footprint (a shape in the LUT) and an obstacle by using the distanceToObstacle() method.

Parameters

map	The map to transform
oDistanceMap	The resulting distance transformed map
p	The position of the model in the map
phi	The orientation of the model in the map

◆ distanceTransformAndClearShape() [2/2]

void distanceTransformAndClearShape	(	const Img8U1 &	map,
		Img32F1 &	oDistanceMap,
		Img< int32 > &	oLabelMap,
		const Point2i &	p,
		float	phi
	)		const

Same as distanceTransformAndClearShape(const Img8U1&, Img32F1&, const Point2i&, float) const, but also returns a label map which allows to determine the position of the obstacle in addition to its distance.

Parameters

map	The map to transform
oDistanceMap	The resulting distance transformed map
oLabelMap	The resulting label map
p	The position of the model in the map
phi	The orientation of the model in the map

Exceptions

XNotImplemented Not implemented (available OpenCV version is < 2.4)

◆ getShape()

const Img8U1& getShape ( float phi ) const

Return the entry of the LUT that is closest to the representation of the enclosed footprint rotated about phi.

◆ getShapeBB()

const Rect2i& getShapeBB ( float phi ) const

Return the bounding box for the shape for phi.

◆ initialize() [1/3]

void initialize ( const Footprint & footprint )

Initialize collision testing with the given footprint.

(Re)builds the internal LUT

Parameters

footprint The new footprint to use.

◆ initialize() [2/3]

void initialize	(	const Footprint &	footprint,
		float	cellSize
	)

Initialize collision testing with the given footprint and cell size.

(Re)builds the internal LUT

Parameters

footprint	The new footprint to use.
cellSize	The cell size of the maps that are later used for collision test (spatial precision)

◆ initialize() [3/3]

void initialize	(	const Footprint &	footprint,
		float	cellSize,
		uint32	segments
	)

Initialize collision testing with the given footprint, cell size and angular precision.

(Re)builds the internal LUT

Parameters

footprint	The new footprint to use.
cellSize	The cell size of the maps that are later used for collision test (spatial precision)
segments	The number of rotation segments, that are used when building the LUT. Each segment represents a certain orientation. The higher the number of segments, the higher is the angular precision in the collision tests.

◆ clearShape()

void clearShape	(	Img8U1 &	map,
		const Point2i &	p,
		float	phi,
		uint8	freeValue = `0`
	)		const

Clears the map region that is covered by the rotated footprint at position p in the given map.

Parameters

map	The map to clear
p	The position of the model in the map
phi	Orientation of the model in the map
freeValue	The value that is used to clear the map (default: 0)

◆ decayShape()

void decayShape	(	Img8U1 &	map,
		const Point2i &	p,
		float	phi,
		float	decayRate,
		float	neutral = `127.f`,
		bool	decayObstacles = `true`,
		bool	decayFreespace = `false`,
		bool	invertShape = `false`
	)		const

Decays the map region that is covered by the rotated footprint at position p in the given map.

Parameters

map	The map to decay
p	The position of the model in the map
phi	Orientation of the model in the map
decayRate	The rate by which the cell values reduce their distance to the neutral value (0.0 = values unchanged, 1.0 = set to neutral value)
neutral	The neutral value that all cell values will move towards (default: 127.0)
decayObstacles	Decay cell values that are greater than neutral? (default: true)
decayFreespace	Decay cell values that are less than neutral? (default: false)
invertShape	if true, instead of decaying everything in the footprint, decay everything NOT in the footprint

◆ calculateSegment()

int calculateSegment ( float phi ) const

protected

◆ calculateOffsetAndOverlap()

std::pair<Point2i, Rect2i> calculateOffsetAndOverlap	(	const Size2i &	map,
		const Point2i &	p,
		float	phi
	)		const

protected

Member Data Documentation

◆ mCellSize

float mCellSize

protected

Resolution of the collision map [m].

◆ mSegments

uint32 mSegments

protected

Size of the collision LUT (360° is divided in #segments)

◆ mShapeLUT

std::vector<Img8U1> mShapeLUT

protected

Look up table for faster collision checking.

◆ mShapeLUTBB

std::vector<Rect2i> mShapeLUTBB

protected

Bounding box for each precalculated shape.

◆ mShapeDimension

int mShapeDimension

protected

Each LUT entry has this dimension (e.g. is a dim x dim image)

◆ mObstacleThreshold

int mObstacleThreshold

protected

Occupancy grid values above this threshold are counted as obstacles.

◆ mSegmentStep

float mSegmentStep

protected

Denotes a step in rad between each LUT entry (2pi / mSegments)

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

include/model/CollisionTest.h

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CollisionTest() [1/2]

◆ CollisionTest() [2/2]

Member Function Documentation

◆ reflect()

◆ operator=()

◆ testCollision()

◆ distanceToObstacle()

◆ obstaclePosition()

◆ distanceTransform() [1/2]

◆ distanceTransform() [2/2]

◆ distanceTransformAndClearShape() [1/2]

◆ distanceTransformAndClearShape() [2/2]

◆ getShape()

◆ getShapeBB()

◆ initialize() [1/3]

◆ initialize() [2/3]

◆ initialize() [3/3]

◆ clearShape()

◆ decayShape()

◆ calculateSegment()

◆ calculateOffsetAndOverlap()

Member Data Documentation

◆ mCellSize

◆ mSegments

◆ mShapeLUT

◆ mShapeLUTBB

◆ mShapeDimension

◆ mObstacleThreshold

◆ mSegmentStep