#include <TraversabilityGenerator3d.hpp>

Classes
struct	BodyHull

struct	PrefitEntry

struct	PrefitPatch

struct	YawCheckPatch

Public Types
typedef maps::grid::MLSMapSloped	MLGrid

Public Member Functions
	TraversabilityGenerator3d (const TraversabilityConfig &config)

virtual	~TraversabilityGenerator3d ()

void	clearTrMap ()

void	clearSoilMap ()

void	setInitialPatch (const Eigen::Affine3d &ground2Mls, double patchRadius)

void	setSoilType (SoilNode *node, SoilType soilType)

bool	addSoilNode (const SoilSample &sample)

double	gaussian2D (double x, double y, double meanX, double meanY, double sigmaX, double sigmaY)

virtual TravGenNode *	generateStartNode (const Eigen::Vector3d &startPos)

TravGenNode *	findMatchingTraversabilityPatchAt (maps::grid::Index idx, const double curHeight) const

SoilNode *	findMatchingSoilPatchAt (maps::grid::Index idx, const double curHeight) const

void	updateSoilInformation ()

void	expandAll (const std::vector< Eigen::Vector3d > &positions)

void	expandAll (const Eigen::Vector3d &startPos)

void	expandAll (const Eigen::Vector3d &startPos, const double expandDist)

void	expandAll (TravGenNode *startNode)

void	expandAll (TravGenNode *startNode, const double expandDist)

virtual bool	expandNode (TravGenNode *node)

void	setMLSGrid (std::shared_ptr< MLGrid > &grid)

int	getNumNodes () const

const maps::grid::TraversabilityMap3d< TravGenNode * > &	getTraversabilityMap () const

const maps::grid::TraversabilityMap3d< SoilNode * > &	getSoilMap () const

void	addConnectedPatches (SoilNode *node)

void	setConfig (const TraversabilityConfig &config)

Protected Types
enum class	NodeClassification : uint8_t { Unknown , PreexistingObstacle , Obstacle , Traversable }

enum class	NeighborRequest : uint8_t { Ok , Skip , Abort }

typedef MLGrid::CellType	Cell

typedef MLGrid::View	View

typedef View::CellType	ViewCell

typedef MLGrid::PatchType	Patch

typedef std::unordered_map< uint64_t, PrefitEntry >	PrefitCache

Protected Member Functions
Polyhedron_3	generatePolyhedron (const std::vector< Eigen::Vector3d > &points)

Polyhedron_3	createPolyhedronFromSurfacePatch (const maps::grid::SurfacePatch< maps::grid::MLSConfig::SLOPE > *p, const Eigen::Vector3d &position)

bool	computePlaneRansac (TravGenNode &node)

double	computeSlope (const Eigen::Hyperplane< double, int(3) > &plane) const

Eigen::Vector3d	computeSlopeDirection (const Eigen::Hyperplane< double, int(3) > &plane) const

bool	checkStepHeightAABB (TravGenNode *node)

bool	checkStepHeightOBB (TravGenNode *node)

BodyHull	buildBodyPolyhedron (const Eigen::Vector3d &nodePos, const Eigen::Hyperplane< double, 3 > &plane, double yaw)

Eigen::AlignedBox3d	rotationSafeSearchBox (const Eigen::Vector3d &nodePos) const

std::vector< YawCheckPatch >	collectYawCheckPatches (TravGenNode *node)

bool	checkCollisionForYaw (TravGenNode *node, double yaw, const std::vector< YawCheckPatch > &patches)

bool	computeSafeOrientations (TravGenNode *node)

bool	computeAllowedOrientations (TravGenNode *node)

bool	checkForFrontier (const TravGenNode *node)

NodeClassification	classifyNode (TravGenNode *node)

PrefitPatch	buildPatchNodeAt (const maps::grid::Index &idx, const double curHeight)

TravGenNode *	finishPatchNode (const PrefitPatch &prefit, const maps::grid::Index &idx)

bool	finalizeNode (TravGenNode node, NodeClassification classification, PrefitCache prefits)

NeighborRequest	computeNeighborRequest (const TravGenNode *node, const maps::grid::Index &idxS, maps::grid::Index &outIdx, double &outHeight) const

void	addConnectedPatches (TravGenNode *node)

void	addConnectedPatches (TravGenNode node, PrefitCache prefits)

bool	getConnectedPatch (const maps::grid::Index &idx, double height, const Patch *&patch)

TravGenNode *	createTraversabilityPatchAt (maps::grid::Index idx, const double curHeight)

SoilNode *	createSoilPatchAt (maps::grid::Index idx, const double curHeight)

void	inflateFrontiers ()

void	inflateObstacles ()

void	addInitialPatchToMLS ()

SoilNode *	generateStartSoilNode (const Eigen::Vector3d &startPos)

Static Protected Member Functions
static double	interpolate (double x, double x0, double y0, double x1, double y1)

Protected Attributes
std::shared_ptr< MLGrid >	mlsGrid

bool	addInitialPatch

Eigen::Affine3d	initialPatch2Mls

double	patchRadius

std::vector< TravGenNode * >	obstacleNodesGrowList

maps::grid::TraversabilityMap3d< TravGenNode * >	trMap

maps::grid::TraversabilityMap3d< SoilNode * >	soilMap

int	currentNodeId = 0

int	currentSoilNodeId = 0

std::vector< TravGenNode * >	frontierNodesGrowList

TraversabilityConfig	config

Member Typedef Documentation

◆ Cell

typedef MLGrid::CellType traversability_generator3d::TraversabilityGenerator3d::Cell

protected

◆ MLGrid

typedef maps::grid::MLSMapSloped traversability_generator3d::TraversabilityGenerator3d::MLGrid

◆ Patch

typedef MLGrid::PatchType traversability_generator3d::TraversabilityGenerator3d::Patch

protected

◆ PrefitCache

typedef std::unordered_map<uint64_t, PrefitEntry> traversability_generator3d::TraversabilityGenerator3d::PrefitCache

protected

◆ View

typedef MLGrid::View traversability_generator3d::TraversabilityGenerator3d::View

protected

◆ ViewCell

typedef View::CellType traversability_generator3d::TraversabilityGenerator3d::ViewCell

protected

Member Enumeration Documentation

◆ NeighborRequest

enum class traversability_generator3d::TraversabilityGenerator3d::NeighborRequest : uint8_t

strongprotected

Enumerator
Ok
Skip
Abort

◆ NodeClassification

enum class traversability_generator3d::TraversabilityGenerator3d::NodeClassification : uint8_t

strongprotected

Outcome of the read-only expansion checks; see classifyNode().

Enumerator
Unknown	node type is UNKNOWN: skip
PreexistingObstacle	node was already OBSTACLE: seed inflation only
Obstacle	failed slope / step-height / incline checks
Traversable	passed all checks

Constructor & Destructor Documentation

◆ TraversabilityGenerator3d()

traversability_generator3d::TraversabilityGenerator3d::TraversabilityGenerator3d ( const TraversabilityConfig & config )

◆ ~TraversabilityGenerator3d()

traversability_generator3d::TraversabilityGenerator3d::~TraversabilityGenerator3d ( )

virtual

Member Function Documentation

◆ addConnectedPatches() [1/3]

void traversability_generator3d::TraversabilityGenerator3d::addConnectedPatches ( SoilNode * node )

◆ addConnectedPatches() [2/3]

void traversability_generator3d::TraversabilityGenerator3d::addConnectedPatches ( TravGenNode * node )

protected

◆ addConnectedPatches() [3/3]

void traversability_generator3d::TraversabilityGenerator3d::addConnectedPatches	(	TravGenNode *	node,
		PrefitCache *	prefits
	)

protected

addConnectedPatches() variant that consumes nodes pre-fitted by the parallel wave phase instead of fitting them inline.

◆ addInitialPatchToMLS()

void traversability_generator3d::TraversabilityGenerator3d::addInitialPatchToMLS ( )

protected

◆ addSoilNode()

bool traversability_generator3d::TraversabilityGenerator3d::addSoilNode ( const SoilSample & sample )

◆ buildBodyPolyhedron()

TraversabilityGenerator3d::BodyHull traversability_generator3d::TraversabilityGenerator3d::buildBodyPolyhedron	(	const Eigen::Vector3d &	nodePos,
		const Eigen::Hyperplane< double, 3 > &	plane,
		double	yaw
	)

protected

◆ buildPatchNodeAt()

TraversabilityGenerator3d::PrefitPatch traversability_generator3d::TraversabilityGenerator3d::buildPatchNodeAt	(	const maps::grid::Index &	idx,
		const double	curHeight
	)

protected

The read-only (fitting) half of createTraversabilityPatchAt(): candidate heights from the MLS plus the RANSAC plane fits. Safe to run concurrently for distinct cells.

◆ checkCollisionForYaw()

bool traversability_generator3d::TraversabilityGenerator3d::checkCollisionForYaw	(	TravGenNode *	node,
		double	yaw,
		const std::vector< YawCheckPatch > &	patches
	)

protected

Check if the robot at a specific yaw orientation collides with MLS patches (patches from collectYawCheckPatches() of the same node).

Returns: true if the yaw is collision-free (safe).

Check if the robot, rotated to a specific yaw angle, would collide with MLS patches at the given node position (patches pre-collected per node).

Returns: true if the yaw is collision-free (safe).

◆ checkForFrontier()

bool traversability_generator3d::TraversabilityGenerator3d::checkForFrontier ( const TravGenNode * node )

protected

◆ checkStepHeightAABB()

bool traversability_generator3d::TraversabilityGenerator3d::checkStepHeightAABB ( TravGenNode * node )

protected

Check if any of the patches around node that the robot might stand on is higher than stepHeight, i.e. would be inside the robot's body.

◆ checkStepHeightOBB()

bool traversability_generator3d::TraversabilityGenerator3d::checkStepHeightOBB ( TravGenNode * node )

protected

Check if any of the patches within the robot OBB collide with the robot.

◆ classifyNode()

TraversabilityGenerator3d::NodeClassification traversability_generator3d::TraversabilityGenerator3d::classifyNode ( TravGenNode * node )

protected

The read-only half of expandNode(): slope, step-height (AABB with OBB fallback) and incline-limit checks. Reads only the MLS/trMap geometry and writes only the node's OWN userData (allowedOrientations), so DISTINCT nodes may be classified concurrently from multiple threads.

◆ clearSoilMap()

void traversability_generator3d::TraversabilityGenerator3d::clearSoilMap ( )

◆ clearTrMap()

void traversability_generator3d::TraversabilityGenerator3d::clearTrMap ( )

◆ collectYawCheckPatches()

std::vector< TraversabilityGenerator3d::YawCheckPatch > traversability_generator3d::TraversabilityGenerator3d::collectYawCheckPatches ( TravGenNode * node )

protected

Build the patch polyhedra of the node's rotation-safe search window once; checkCollisionForYaw() reuses them for every sampled yaw.

◆ computeAllowedOrientations()

bool traversability_generator3d::TraversabilityGenerator3d::computeAllowedOrientations ( TravGenNode * node )

protected

Returns: false if no allowed orientation was found (e.g. due to extreme slope)

◆ computeNeighborRequest()

TraversabilityGenerator3d::NeighborRequest traversability_generator3d::TraversabilityGenerator3d::computeNeighborRequest	(	const TravGenNode *	node,
		const maps::grid::Index &	idxS,
		maps::grid::Index &	outIdx,
		double &	outHeight
	)		const

protected

The neighbor projection addConnectedPatches() performs before matching or creating a neighbor node in direction idxS: output cell and height. Abort mirrors the historical early-return of addConnectedPatches() on an inconsistent plane intersection (remaining neighbors are not processed).

◆ computePlaneRansac()

bool traversability_generator3d::TraversabilityGenerator3d::computePlaneRansac ( TravGenNode & node )

protected

◆ computeSafeOrientations()

bool traversability_generator3d::TraversabilityGenerator3d::computeSafeOrientations ( TravGenNode * node )

protected

Sample exactly config.numYawSamples yaws over the full circle [0,360deg) (footprint offset applied to every check), and if any are collision-free, fill node->allowedOrientations with a wedge (width = sampling step) per safe yaw.

Returns: true if at least one safe orientation was found.

◆ computeSlope()

double traversability_generator3d::TraversabilityGenerator3d::computeSlope ( const Eigen::Hyperplane< double, int(3) > & plane ) const

protected

◆ computeSlopeDirection()

Eigen::Vector3d traversability_generator3d::TraversabilityGenerator3d::computeSlopeDirection ( const Eigen::Hyperplane< double, int(3) > & plane ) const

protected

The vector of maximum slope on a plane is the projection of (0,0,1) onto the plane. (0,0,1) is the steepest vector possible in the global frame, thus by projecting it onto the plane we get the steepest vector possible on that plane.

◆ createPolyhedronFromSurfacePatch()

Polyhedron_3 traversability_generator3d::TraversabilityGenerator3d::createPolyhedronFromSurfacePatch	(	const maps::grid::SurfacePatch< maps::grid::MLSConfig::SLOPE > *	p,
		const Eigen::Vector3d &	position
	)

protected

◆ createSoilPatchAt()

SoilNode * traversability_generator3d::TraversabilityGenerator3d::createSoilPatchAt	(	maps::grid::Index	idx,
		const double	curHeight
	)

protected

◆ createTraversabilityPatchAt()

TravGenNode * traversability_generator3d::TraversabilityGenerator3d::createTraversabilityPatchAt	(	maps::grid::Index	idx,
		const double	curHeight
	)

protected

◆ expandAll() [1/5]

void traversability_generator3d::TraversabilityGenerator3d::expandAll ( const Eigen::Vector3d & startPos )

◆ expandAll() [2/5]

void traversability_generator3d::TraversabilityGenerator3d::expandAll	(	const Eigen::Vector3d &	startPos,
		const double	expandDist
	)

Expands the map starting at startPos. Expansion will stop if a distance of expandDist is reached. I.e. this will expand all nodes in a circle of radius expandDist around startPos.

◆ expandAll() [3/5]

void traversability_generator3d::TraversabilityGenerator3d::expandAll ( const std::vector< Eigen::Vector3d > & positions )

Expand the map starting from all given positions

◆ expandAll() [4/5]

void traversability_generator3d::TraversabilityGenerator3d::expandAll ( TravGenNode * startNode )

◆ expandAll() [5/5]

void traversability_generator3d::TraversabilityGenerator3d::expandAll	(	TravGenNode *	startNode,
		const double	expandDist
	)

Parameters

expandDist How far should the map be expanded? If negative the whole map will be expanded.

◆ expandNode()

bool traversability_generator3d::TraversabilityGenerator3d::expandNode ( TravGenNode * node )

virtual

◆ finalizeNode()

bool traversability_generator3d::TraversabilityGenerator3d::finalizeNode	(	TravGenNode *	node,
		NodeClassification	classification,
		PrefitCache *	prefits
	)

protected

The mutation half of expandNode(): applies a classification (soil node, expanded flag, type, grow lists, neighbor growth, frontier check). expandNode() = classifyNode() + finalizeNode(); the wave loop calls it with the pre-computed classification and the wave's pre-fit cache. Must run single-threaded.

Returns: true if the node became TRAVERSABLE.

◆ findMatchingSoilPatchAt()

SoilNode * traversability_generator3d::TraversabilityGenerator3d::findMatchingSoilPatchAt	(	maps::grid::Index	idx,
		const double	curHeight
	)		const

◆ findMatchingTraversabilityPatchAt()

TravGenNode * traversability_generator3d::TraversabilityGenerator3d::findMatchingTraversabilityPatchAt	(	maps::grid::Index	idx,
		const double	curHeight
	)		const

◆ finishPatchNode()

TravGenNode * traversability_generator3d::TraversabilityGenerator3d::finishPatchNode	(	const PrefitPatch &	prefit,
		const maps::grid::Index &	idx
	)

protected

Serial completion of buildPatchNodeAt(): assigns the node id and inserts the node into trMap. Must run single-threaded.

◆ gaussian2D()

double traversability_generator3d::TraversabilityGenerator3d::gaussian2D	(	double	x,
		double	y,
		double	meanX,
		double	meanY,
		double	sigmaX,
		double	sigmaY
	)

◆ generatePolyhedron()

Polyhedron_3 traversability_generator3d::TraversabilityGenerator3d::generatePolyhedron ( const std::vector< Eigen::Vector3d > & points )

protected

◆ generateStartNode()

TravGenNode * traversability_generator3d::TraversabilityGenerator3d::generateStartNode ( const Eigen::Vector3d & startPos )

virtual

◆ generateStartSoilNode()

SoilNode * traversability_generator3d::TraversabilityGenerator3d::generateStartSoilNode ( const Eigen::Vector3d & startPos )

protected

◆ getConnectedPatch()

bool traversability_generator3d::TraversabilityGenerator3d::getConnectedPatch	(	const maps::grid::Index &	idx,
		double	height,
		const Patch *&	patch
	)

protected

◆ getNumNodes()

int traversability_generator3d::TraversabilityGenerator3d::getNumNodes ( ) const

Returns the number of nodes after expansion

◆ getSoilMap()

const maps::grid::TraversabilityMap3d< SoilNode * > & traversability_generator3d::TraversabilityGenerator3d::getSoilMap ( ) const

◆ getTraversabilityMap()

const maps::grid::TraversabilityMap3d< TravGenNode * > & traversability_generator3d::TraversabilityGenerator3d::getTraversabilityMap ( ) const

◆ inflateFrontiers()

void traversability_generator3d::TraversabilityGenerator3d::inflateFrontiers ( )

protected

◆ inflateObstacles()

void traversability_generator3d::TraversabilityGenerator3d::inflateObstacles ( )

protected

◆ interpolate()

double traversability_generator3d::TraversabilityGenerator3d::interpolate	(	double	x,
		double	x0,
		double	y0,
		double	x1,
		double	y1
	)

staticprotected

◆ rotationSafeSearchBox()

Eigen::AlignedBox3d traversability_generator3d::TraversabilityGenerator3d::rotationSafeSearchBox ( const Eigen::Vector3d & nodePos ) const

protected

The rotation-safe MLS search window around a node (covers the footprint at every yaw), shared by collectYawCheckPatches() and checkCollisionForYaw().

◆ setConfig()

void traversability_generator3d::TraversabilityGenerator3d::setConfig ( const TraversabilityConfig & config )

◆ setInitialPatch()

void traversability_generator3d::TraversabilityGenerator3d::setInitialPatch	(	const Eigen::Affine3d &	ground2Mls,
		double	patchRadius
	)

◆ setMLSGrid()

void traversability_generator3d::TraversabilityGenerator3d::setMLSGrid ( std::shared_ptr< MLGrid > & grid )

◆ setSoilType()

void traversability_generator3d::TraversabilityGenerator3d::setSoilType	(	SoilNode *	node,
		SoilType	soilType
	)

◆ updateSoilInformation()

void traversability_generator3d::TraversabilityGenerator3d::updateSoilInformation ( )

Member Data Documentation

◆ addInitialPatch

bool traversability_generator3d::TraversabilityGenerator3d::addInitialPatch

protected

◆ config

TraversabilityConfig traversability_generator3d::TraversabilityGenerator3d::config

protected

◆ currentNodeId

int traversability_generator3d::TraversabilityGenerator3d::currentNodeId = 0

protected

◆ currentSoilNodeId

int traversability_generator3d::TraversabilityGenerator3d::currentSoilNodeId = 0

protected

◆ frontierNodesGrowList

std::vector<TravGenNode *> traversability_generator3d::TraversabilityGenerator3d::frontierNodesGrowList

protected

◆ initialPatch2Mls

Eigen::Affine3d traversability_generator3d::TraversabilityGenerator3d::initialPatch2Mls

protected

◆ mlsGrid

std::shared_ptr<MLGrid > traversability_generator3d::TraversabilityGenerator3d::mlsGrid

protected

◆ obstacleNodesGrowList

std::vector<TravGenNode*> traversability_generator3d::TraversabilityGenerator3d::obstacleNodesGrowList

protected

◆ patchRadius

double traversability_generator3d::TraversabilityGenerator3d::patchRadius

protected

◆ soilMap

maps::grid::TraversabilityMap3d<SoilNode*> traversability_generator3d::TraversabilityGenerator3d::soilMap

protected

◆ trMap

maps::grid::TraversabilityMap3d<TravGenNode*> traversability_generator3d::TraversabilityGenerator3d::trMap

protected

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

Classes

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Member Typedef Documentation

◆ Cell

◆ MLGrid

◆ Patch

◆ PrefitCache

◆ View

◆ ViewCell

Member Enumeration Documentation

◆ NeighborRequest

◆ NodeClassification

Constructor & Destructor Documentation

◆ TraversabilityGenerator3d()

◆ ~TraversabilityGenerator3d()

Member Function Documentation

◆ addConnectedPatches() [1/3]

◆ addConnectedPatches() [2/3]

◆ addConnectedPatches() [3/3]

◆ addInitialPatchToMLS()

◆ addSoilNode()

◆ buildBodyPolyhedron()

◆ buildPatchNodeAt()

◆ checkCollisionForYaw()

◆ checkForFrontier()

◆ checkStepHeightAABB()

◆ checkStepHeightOBB()

◆ classifyNode()

◆ clearSoilMap()

◆ clearTrMap()

◆ collectYawCheckPatches()

◆ computeAllowedOrientations()

◆ computeNeighborRequest()

◆ computePlaneRansac()

◆ computeSafeOrientations()

◆ computeSlope()

◆ computeSlopeDirection()

◆ createPolyhedronFromSurfacePatch()

◆ createSoilPatchAt()

◆ createTraversabilityPatchAt()

◆ expandAll() [1/5]

◆ expandAll() [2/5]

◆ expandAll() [3/5]

◆ expandAll() [4/5]

◆ expandAll() [5/5]

◆ expandNode()

◆ finalizeNode()

◆ findMatchingSoilPatchAt()

◆ findMatchingTraversabilityPatchAt()

◆ finishPatchNode()

◆ gaussian2D()

◆ generatePolyhedron()

◆ generateStartNode()

◆ generateStartSoilNode()

◆ getConnectedPatch()

◆ getNumNodes()

◆ getSoilMap()

◆ getTraversabilityMap()

◆ inflateFrontiers()

◆ inflateObstacles()

◆ interpolate()

◆ rotationSafeSearchBox()

◆ setConfig()

◆ setInitialPatch()

◆ setMLSGrid()

◆ setSoilType()

◆ updateSoilInformation()

Member Data Documentation

◆ addInitialPatch

◆ config

◆ currentNodeId

◆ currentSoilNodeId

◆ frontierNodesGrowList

◆ initialPatch2Mls

◆ mlsGrid