Data Types

The values that flow between the agent, its layers and its plugins: states, nodes, edges, paths and their gain bookkeeping, and the episode’s history.

Robot State

class RobotState

#include <open_apero_core/state/robot_state.h>

Represents the full robot state including base and sensor poses at a point in time.

Stores the base frame and sensor frame positions and orientations in the world frame, along with a timestamp. Provides coordinate transformations and comparison utilities.

Public Functions

const Eigen::Quaterniond &getBaseOrientation() const

Get the base frame orientation in world coordinates.

Returns:

Reference to the base orientation quaternion.

const Eigen::Vector3d &getBasePosition() const

Get the base frame position in world coordinates.

Returns:

Reference to the base position vector.

const Transform3d &getBaseTransform() const

Get the base frame transform in world coordinates.

Returns:

Const reference to the base transform.

const Eigen::VectorXd &getJointPosition() const

Get the joint positions.

Returns:

Const reference to the joint position vector. Empty if unset.

Eigen::Vector3d getSensorEulerAnglesZyx() const

Get sensor Euler angles in ZYX order.

Returns:

Euler angles as (z, y, x) vector.

const Eigen::Quaterniond &getSensorOrientation() const

Get the sensor frame orientation in world coordinates.

Returns:

Reference to the sensor orientation quaternion.

const Eigen::Vector3d &getSensorPosition() const

Get the sensor frame position in world coordinates.

Returns:

Reference to the sensor position vector.

const Transform3d &getSensorTransform() const

Get the sensor frame transform in world coordinates.

Returns:

Const reference to the sensor transform.

double getSensorYaw() const

Get the sensor yaw angle.

Returns:

Yaw angle in radians.

double getTimeFromStart() const

Get the time offset from trajectory start.

Returns:

Time from start in seconds.

bool isSameSensorOrientation(const RobotState &other, double orientation_dot_epsilon = kEpsilon) const

Check if sensor orientation matches another state.

Parameters:

• other – The other robot state to compare.

• orientation_dot_epsilon – Quaternion dot-product tolerance.

Returns:

True if sensor orientations are the same.

bool isSameSensorPose(const RobotState &other, double position_epsilon = kEpsilon, double orientation_dot_epsilon = kEpsilon) const

Check if sensor pose (position and orientation) matches another state.

Parameters:

• other – The other robot state to compare.

• position_epsilon – Position tolerance in meters.

• orientation_dot_epsilon – Quaternion dot-product tolerance.

Returns:

True if sensor poses are the same.

bool isSameSensorPosition(const RobotState &other, double position_epsilon = kEpsilon) const

Check if sensor position matches another state.

Parameters:

• other – The other robot state to compare.

• position_epsilon – Position tolerance in meters.

Returns:

True if sensor positions are the same.

bool isSensorPoseReached(const RobotState &current, double pos_threshold, double angle_threshold) const

Check if the sensor pose has been reached given position and angle thresholds.

Symmetric in the two states, so either may be the target (Agent calls it on the current state with the target waypoint as current). A threshold below kEpsilon skips that check.

Parameters:

• current – The other robot state.

• pos_threshold – Position tolerance in meters.

• angle_threshold – Orientation tolerance in radians.

Returns:

True if the sensor pose is within the specified thresholds.

RobotState &operator=(const RobotState&) = default

RobotState()

Default constructor.

RobotState(const RobotState&) = default

RobotState(double time_from_start, const Transform3d &base_transform, const Transform3d &sensor_transform)

Construct a RobotState with full pose specification.

Parameters:

• time_from_start – Time offset from trajectory start.

• base_transform – Base frame transform in world coordinates.

• sensor_transform – Sensor frame transform in world coordinates.

void setBaseOrientation(const Eigen::Quaterniond &base_orientation)

Set the base frame orientation.

Parameters:

base_orientation – Base orientation in world coordinates.

void setBasePosition(const Eigen::Vector3d &base_position)

Set the base frame position.

Parameters:

base_position – Base position in world coordinates.

void setBaseTransform(const Transform3d &base_transform)

Set the base frame transform in world coordinates.

Parameters:

base_transform – Base transform in world coordinates.

void setJointPosition(const Eigen::VectorXd &joint_position)

Set the joint positions.

Parameters:

joint_position – Joint position vector of arbitrary dimension.

void setSensorOrientation(const Eigen::Quaterniond &sensor_orientation)

Set the sensor frame orientation.

Parameters:

sensor_orientation – Sensor orientation in world coordinates.

void setSensorPosition(const Eigen::Vector3d &sensor_position)

Set the sensor frame position.

Parameters:

sensor_position – Sensor position in world coordinates.

void setSensorTransform(const Transform3d &sensor_transform)

Set the sensor frame transform in world coordinates.

Parameters:

sensor_transform – Sensor transform in world coordinates.

void setTimeFromStart(double time_from_start)

Set the time offset from trajectory start.

Parameters:

time_from_start – Time from start in seconds.

std::string toString() const

Get a string representation of this robot state.

Returns:

Formatted string with position and orientation information.

~RobotState() = default

using open_apero::RobotTrajectory = std::vector<RobotState, Eigen::aligned_allocator<RobotState>>

A sequence of robot states forming a trajectory, with Eigen-aligned allocation.

struct Transform3d

#include <open_apero_core/geometry/transform3d.h>

A rigid 3D transform consisting of a position and orientation.

Public Functions

inline Transform3d inverse() const

Return the inverse of this transform.

inline bool isPoseReached(const Transform3d &target, double pos_threshold, double angle_threshold) const

Check if the target pose has been reached from this transform within thresholds. A threshold near zero disables that check dimension.

inline bool isSameOrientation(const Transform3d &other, double orientation_dot_epsilon = kEpsilon) const

Check if orientation matches another transform (handles q == -q equivalence).

inline bool isSamePose(const Transform3d &other, double position_epsilon = kEpsilon, double orientation_dot_epsilon = kEpsilon) const

Check if both position and orientation match another transform.

inline bool isSamePosition(const Transform3d &other, double position_epsilon = kEpsilon) const

Check if position matches another transform.

inline Eigen::Quaterniond operator*(const Eigen::Quaterniond &quat) const

Transform an orientation from this frame to the parent frame.

inline Eigen::Vector3d operator*(const Eigen::Vector3d &point) const

Transform a point from this frame to the parent frame.

inline Transform3d operator*(const Transform3d &other) const

Transform another transform from this frame to the parent frame.

Public Members

Eigen::Quaterniond orientation

Eigen::Vector3d position

Nodes

struct Node

#include <open_apero_core/graph/node.h>

Core planning node containing robot state, gain, traversability, and metadata.

Nodes are the fundamental building block of the exploration graph/tree. Each node holds a robot state, information gain anchors, traversability status, and an optional info payload for evaluator-specific data.

Subclassed by open_apero::GraphNode, open_apero::TreeNode

Public Functions

template<class T>
inline T *as()

Downcast to a derived node type.

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

template<class T>
inline const T *as() const

Downcast to a derived node type (const version).

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

inline double getSingleNodeGain() const

Compute the total gain from all gain anchors attached to this node.

Returns:

The sum of all anchor gains.

inline void markObserved()

Spend this viewpoint: mark it observed, clear is_at_frontier, gain anchors and info.

Evaluator::onNodeReached() calls it on arrival; a plugin may also call it for a viewpoint it knows to be observed already. is_terminating is left untouched; the evaluator that set it owns it.

Node() = default

inline Node(const Node &other)

Copy constructor. Performs a deep copy of the info payload.

Parameters:

other – The node to copy from.

inline Node &operator=(const Node &other)

Copy assignment operator. Performs a deep copy of the info payload.

Parameters:

other – The node to assign from.

Returns:

Reference to this node.

inline virtual std::unique_ptr<Node> shallowCopy()

Create a shallow copy of the node (excludes info payload).

Returns:

A unique pointer to the shallow-copied node.

inline virtual ~Node()

Public Members

std::vector<Eigen::Quaterniond> candidate_orientations

Candidate orientations that are collision-free at this node.

GainAnchorPtrVector gain_anchors

Gain anchors associated with this node.

std::unique_ptr<NodeInfo> info

Optional evaluator-specific information (e.g., visible voxels).

bool is_at_frontier = false

Whether this node lies at a frontier boundary.

std::vector<bool> is_candidate_traversable

Whether each candidate orientation is traversable.

bool is_gain_suppressed = false

Whether the path planner’s gain suppression zeroed this node’s gain for the current search.

Rewritten before every search (suppressNonMaximumClusters); the gain anchors stay untouched.

bool is_intermediate = false

Whether this is an intermediate node (not a decision point).

bool is_observed = false

Whether this viewpoint has already been observed this episode.

Set through markObserved() and never cleared; the visit itself is recorded by HistoryData. Evaluator::evaluate() and Evaluator::updateEvaluation() skip an observed node, so it is never evaluated again.

bool is_terminating = false

Reaching this node ends the episode (goal reached); set by the layer or evaluator that owns it.

bool is_traversable = false

Whether this node is traversable. Defaults to false: a node never assigned is pruned on the next maintenance pass.

std::size_t most_recent_path_traversed_edge_from_final_node_to_this = 0

std::size_t most_recent_path_visited_this = 0

PathLedger entries: the most recent path that visited this node, and the most recent one that traversed the edge from that path’s final node to this node.

RobotState robot_state

The robot state (position and orientation) at this node.

SamplingRole sampling_role = SamplingRole::kDefault

Which population the generator sampled this node for. Provenance, not derived state: nothing but the generator assigns it.

struct NodeInfo

#include <open_apero_core/graph/node.h>

Base class for evaluator-specific information attached to a node.

Subclasses carry domain-specific data (e.g., visible voxels, yaw plans). Supports deep copying and polymorphic downcasting.

Subclassed by open_apero::SimulatedSensorInfo, open_apero::YawPlanningInfo

Public Functions

template<class T>
inline T *as()

Downcast to a derived type.

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

template<class T>
inline const T *as() const

Downcast to a derived type (const version).

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

inline virtual std::unique_ptr<NodeInfo> deepCopy() const

Create a deep copy of this info object.

Returns:

A unique pointer to the cloned NodeInfo.

inline virtual ~NodeInfo()

struct SimulatedSensorInfo : public open_apero::NodeInfo

#include <open_apero_core/graph/node.h>

NodeInfo subclass storing simulated sensor results (visible voxels).

Public Functions

inline virtual std::unique_ptr<NodeInfo> deepCopy() const override

Create a deep copy of this simulated sensor info.

Returns:

A unique pointer to the cloned info.

inline virtual ~SimulatedSensorInfo()

Public Members

VoxelInfoMap visible_voxels

Map from visible voxel positions to their occupancy and gain info.

struct VoxelInfo

#include <open_apero_core/graph/node.h>

Combined occupancy and gain information for a single voxel.

Public Members

double gain = 0.0

Occupancy occupancy = Occupancy::kUnknown

struct YawPlanningInfo : public open_apero::NodeInfo

#include <open_apero_core/graph/node.h>

NodeInfo subclass for yaw planning, storing orientation candidates and selection state.

Public Functions

inline virtual std::unique_ptr<NodeInfo> deepCopy() const override

Deep copy that preserves the orientation nodes and their dynamic types.

Each orientation is cloned with its virtual shallowCopy() plus a deep copy of its info, so the clone shares nothing mutable; owned tree children are not cloned.

virtual ~YawPlanningInfo() = default

Public Members

int active_orientation = -1

Index of the currently active orientation. -1 = none selected yet; an indeterminate value here would index orientations out of bounds.

std::vector<std::unique_ptr<Node>> orientations

All oriented node versions at this position.

std::vector<bool> visited_orientation

Boolean mask indicating which orientations have been visited.

enum class open_apero::SamplingRole : std::uint8_t

Which sampling population a node belongs to.

Provenance owned by the generator, which may update it; evaluators never change it. Generators that sample a single population leave it at kDefault.

Values:

enumerator kDefault

Population-unaware generator: evaluate with everything the evaluator has.

enumerator kGain

Sampled for information gain; frontier status is not evaluated.

enumerator kFrontier

Sampled around a map frontier; only frontier status is evaluated.

using open_apero::NodeClusterPtrs = std::vector<Node*>

A cluster of node pointers sharing the same spatial location.

using open_apero::VoxelOccupancyMap = std::unordered_map<Eigen::Vector3d, Occupancy, Vector3dHash, Vector3dEqual>

Map from 3D voxel position to its occupancy state (as discovered during raycasting).

struct GraphNode : public open_apero::Node

#include <open_apero_core/graph/graph_node.h>

A node in the exploration graph, extending Node with a reachability flag.

Public Functions

virtual std::unique_ptr<Node> shallowCopy() override

Create a shallow copy of this graph node.

Returns:

A unique pointer to the copied node.

Public Members

bool connect_with_start_node = false

Whether this node is reachable from the start node in the current graph.

struct TreeNode : public open_apero::Node

#include <open_apero_core/graph/tree_node.h>

A node in the exploration tree, with parent/child links and an associated edge.

Provides recursive tree traversal methods (children, leaves, subtree).

Public Functions

void getChildren(std::vector<TreeNode*> *result)

Collect pointers to all immediate children of this node.

Parameters:

result – Vector to which child pointers are appended.

void getLeaves(std::vector<TreeNode*> *result)

Recursively collect pointers to all leaf nodes (nodes with no children).

Parameters:

result – Vector to which leaf node pointers are appended.

void getTree(std::vector<TreeNode*> *result)

Recursively collect pointers to all nodes in the subtree rooted at this node.

Parameters:

result – Vector to which node pointers are appended.

void getTree(std::vector<TreeNode*> *result, int maxdepth)

Recursively collect subtree node pointers up to a maximum depth.

Parameters:

• result – Vector to which node pointers are appended.

• maxdepth – Maximum recursion depth.

virtual std::unique_ptr<Node> shallowCopy() override

Create a shallow copy of this tree node (excludes children and info).

The copy keeps the edge and the parent pointer, so it names a parent that does not own it.

Returns:

A unique pointer to the shallow-copied node.

TreeNode *spawnChild()

Create a new child node owned by this node and return a pointer to it.

Returns:

Pointer to the newly created child node.

Public Members

std::vector<std::unique_ptr<TreeNode>> children

Owned child nodes.

Edge edge

Edge connecting this node to its parent.

TreeNode *parent = nullptr

Pointer to the parent node. Nullptr for the root node.

Edges and Graphs

struct Edge

#include <open_apero_core/graph/edge.h>

Stores edge data including trajectory, cost, and traversability information.

An edge connects two nodes and carries the robot trajectory between them, the associated traversal cost, and traversability status.

Public Members

double cost = 0.0

Traversal cost associated with this edge.

bool is_backtrack = false

Produced by a Backtracker; backtrackers do not record such edges (Backtracker::recordExecutedEdge), and a platform may exempt them from its own pre-flight checks.

bool is_considered = false

Whether the next path planner considers this edge. Excluded edges are visualized as semi-transparent.

bool is_traversable = false

Whether this edge is traversable. Defaults to false: an edge never assigned is pruned on the next maintenance pass.

RobotTrajectory trajectory

All trajectory points along the edge.

using open_apero::GraphType = boost::adjacency_list<boost::listS, boost::listS, boost::bidirectionalS, std::shared_ptr<GraphNode>, std::shared_ptr<Edge>>

Boost bidirectional adjacency list graph with GraphNode vertices and Edge edges.

Edge *open_apero::getEdge(Node *source, Node *target, const GraphType *graph = nullptr)

Get the edge between two nodes in the graph.

With a null graph both nodes must be TreeNodes, source the parent of target (throws otherwise), and the child’s own edge is returned.

Parameters:

• source – Pointer to the source node.

• target – Pointer to the target node.

• graph – Pointer to the graph (optional).

Returns:

Pointer to the edge, or nullptr if not found.

NodeId open_apero::getNodeId(const Node *node, const GraphType *graph)

Look up the vertex descriptor for a given node in the graph (linear scan).

Parameters:

• node – Pointer to the node to look up.

• graph – Pointer to the graph to search.

Returns:

The vertex descriptor, or kInvalidNodeId if either pointer is null or the node is not in the graph.

std::vector<Node*> open_apero::runBfs(Node *start_node, const GraphType *graph)

Run breadth-first search from a start node and return all reachable nodes.

Parameters:

• start_node – Pointer to the BFS starting node.

• graph – Pointer to the graph to traverse.

Returns:

Vector of pointers to all nodes reachable from start_node.

Paths and Gain Bookkeeping

struct PathStep

#include <open_apero_core/graph/path.h>

A node of a path and the step before it; paths share the steps they have in common.

Public Members

Node *node

const PathStep *parent

class PathSteps

#include <open_apero_core/graph/path.h>

Owns every step a search builds; a step stays valid as long as its owner does.

Public Functions

inline const PathStep *chain(const PathNodes &nodes)

The last step of a path through nodes in order.

inline void discard()

Drop the step extend() returned last, which no path holds.

inline const PathStep *extend(const PathStep *parent, Node *node)

The step node appended after parent; nullptr starts a path.

struct Path

#include <open_apero_core/graph/path.h>

A candidate path: its ratio, gain and cost, and its last step.

Comparable with operator> for the search heaps. Copying a path copies no nodes.

Public Functions

inline bool isValid() const

inline bool operator>(const Path &other) const

Compare paths by ratio, then gain, then cost (lower cost is better).

Parameters:

other – The path to compare against.

Returns:

True if this path is strictly greater (better) than the other.

inline Path(double r, double g, double c, const PathStep *l = nullptr)

inline PathNodes toNodes() const

The nodes from the first to the last.

Public Members

double cost

double gain

const PathStep *last

nullptr for a path holding no nodes.

double ratio

class PathLedger

#include <open_apero_core/graph/path.h>

Holds one path at a time, as entries on its nodes and anchors: the most recent path that visited them.

load() replaces the previous path and gives the new one a fresh id. The entries live on the objects, so one search runs at a time.

Public Functions

inline void load(const Path &path)

Load the path: its nodes, their anchors (none of a gain-suppressed node) and the edges it traversed from its final node. Replaces the previously loaded path.

inline double marginalGain(const Node *node, bool is_gain_correlated) const

What node is worth to the loaded path.

0 for a visited or gain-suppressed node. Uncorrelated: the node’s own gain. Correlated: the gains of its anchors the path has not visited.

inline double pathGain(const PathNodes &nodes, bool is_gain_correlated)

The gain of a path through nodes, loading them in order: what each node adds to the nodes before it; the first adds nothing.

inline PathLedger()

inline bool revisitsEdge(const Node *node) const

Whether the loaded path already traversed the edge from its final node to node.

struct GainAnchor

#include <open_apero_core/gain/gain_anchor.h>

Base class for gain anchors that associate an information gain value with a spatial entity.

Derived classes can extend this to carry additional domain-specific data. Each anchor has a unique ID derived from its memory address by default.

Subclassed by open_apero::evaluator::PointsEvaluator::PointSphere

Public Functions

template<class T>
inline T *as()

Downcast to a derived type.

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

template<class T>
inline const T *as() const

Downcast to a derived type (const version).

Template Parameters:

T – The target derived type.

Returns:

Pointer to the derived type, or nullptr if the cast fails.

inline explicit GainAnchor(double gain)

Construct a GainAnchor with the given gain value.

Parameters:

gain – The information gain value.

inline virtual size_t getUniqueId() const

Get a unique identifier for this anchor.

PathLedger does not use it: a path collects each anchor object once, so two objects sharing an id count twice.

Returns:

A unique ID based on the object’s memory address.

virtual ~GainAnchor() = default

Public Members

double gain

The information gain value.

std::size_t most_recent_path_collected_this = 0

PathLedger entry: the most recent path that collected this anchor’s gain.

class GainCostTable

#include <open_apero_core/gain/gain_cost_table.h>

Optimistic gain-vs-cost frontier (monotone non-decreasing; optionally concave).

Each observed path contributes a (cost, gain) knot; dominated knots are dropped, so gain strictly increases with cost. With concavity enabled the knots are reduced to the upper concave hull of {(0,0)} and the knots. Episodic: reset() clears it.

Public Functions

inline bool empty() const

Whether the frontier currently holds any knot.

inline explicit GainCostTable(bool concave = false)

Construct a frontier with optional concave-envelope reduction (off unless requested; PathPlanner sets it from its concave_envelope key, default true).

void insert(const Path &path)

Insert a candidate path’s (cost, gain) into the frontier.

Skips the candidate if it is dominated (a cheaper-or-equal knot is at least as good), removes any existing knots it dominates, and — when concavity is enabled — restores the upper concave hull. Non-positive cost or gain is ignored (covered by the implicit origin).

inline const std::vector<Point> &points() const

The retained frontier knots, ordered by increasing cost (origin (0,0) is implicit).

inline void reset()

Clear all knots; call at episode reset.

inline void setConcave(bool on)

Enable/disable the optional concave-envelope reduction.

double valueAt(double remaining_cost) const

Optimistic gain reachable for a given remaining cost.

Parameters:

remaining_cost – Remaining cost budget (clamped to >= 0).

Returns:

Linear interpolation within the observed range (anchored at the origin); beyond the last knot, linear extrapolation along the last segment’s slope (floored at 0), not a flat clamp to the best observed gain. Returns 0 when the frontier is empty.

struct Point

#include <open_apero_core/gain/gain_cost_table.h>

One frontier knot: a cost and its best gain. No path is stored; paths are only valid for the planning iteration that produced them.

Public Members

double cost

double gain

History

class HistoryData

#include <open_apero_core/state/history_data.h>

The episode’s record of executed motion and of the poses it observed from.

Two independent records: the traveled trajectory (visited-node endpoints, dense path, accumulated cost) and the observed viewpoints, every sensor pose the map integrated a frame from, exposed as getObservedViewpoints().

Public Functions

void addObservedPose(const Transform3d &sensor_pose)

Record a sensor pose the map has integrated a frame from.

Fed from the map’s own report of the frames it consumed, not from what the planner commanded, so it means “the scene was seen from here”. Stored in getObservedViewpoints() unless it is the same pose as the last one stored there.

Parameters:

sensor_pose – Sensor-frame pose of an integrated observation.

void addVisitedEdge(const Edge &edge)

Add a visited trajectory and its associated cost to the history.

Parameters:

edge – The edge representing the visited trajectory and its associated cost.

inline ObservedViewpoints &getObservedViewpoints()

The same record, for configuring its thresholds.

Returns:

Reference to the observed viewpoints.

inline const ObservedViewpoints &getObservedViewpoints() const

The episode’s one record of the sensor poses it observed from, oldest first, fed by addObservedPose().

Append-only between resetEpisode() calls, whatever its thresholds; consumers may hold a count of entries read.

Returns:

Const reference to the observed viewpoints.

void getVisualization(VisualizationMarkers *markers, double total_cost_budget = std::numeric_limits<double>::infinity())

Generate visualization markers for the history data.

Parameters:

• markers – Pointer to the marker collection to populate.

• total_cost_budget – Maximum cost budget for coloring the visualization.

HistoryData() = default

void resetEpisode()

Reset all stored history data. The observed-viewpoint thresholds stay.

inline int size() const

Get the number of stored past nodes.

Returns:

The count of past nodes.

class ObservedViewpoints

#include <open_apero_core/state/observed_viewpoints.h>

Whether a pose was already observed from: within a position and a geodesic angle threshold of a stored viewpoint.

Every added pose is stored, whatever the thresholds; the thresholds decide only the answers. While enabled, a pose is observed exactly when some stored viewpoint lies closer than the position threshold to its position and closer than the angle threshold to its orientation. Position and orientation must match together, and orientation is one rotation, so a yaw and a pitch each inside the angle threshold can still put a pose outside it. While disabled, isObserved() and getViewpointIndicesNear() find nothing.

Not copyable: the KD-tree refers to the stored viewpoints by address.

Public Functions

void add(const Transform3d &sensor_pose)

Stores the pose, enabled or not.

Parameters:

sensor_pose – Sensor-frame pose of an integrated observation.

void clear()

Drops every stored viewpoint; the thresholds stay.

inline double getAngleThreshold() const

Geodesic orientation threshold [rad].

inline double getPositionThreshold() const

Position threshold [m].

Transform3d getViewpoint(std::size_t index) const

One stored viewpoint, in insertion order.

Parameters:

index – Below size().

Returns:

The viewpoint’s sensor pose.

void getViewpointIndicesNear(const Eigen::Vector3d &position, double radius, std::vector<std::size_t> *indices) const

Indices of the stored viewpoints closer than radius to position, whatever their orientation.

Parameters:

• position – Sensor position to search around.

• radius – Search radius [m].

• indices – Replaced with the matches, ascending, each below size(); empty while disabled.

inline bool isEnabled() const

Whether both thresholds are positive.

bool isObserved(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation) const

Whether a stored viewpoint lies closer than both thresholds to this pose.

Parameters:

• position – Sensor position of the pose to test.

• orientation – Sensor orientation of the pose to test.

Returns:

Always false while disabled.

bool isObserved(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, double angle_threshold) const

isObserved() with angle_threshold in place of getAngleThreshold(); the position threshold stays.

Parameters:

• position – Sensor position of the pose to test.

• orientation – Sensor orientation of the pose to test.

• angle_threshold – Geodesic orientation angle an observed pose lies below [rad].

Returns:

Always false while disabled.

ObservedViewpoints()

Disabled (zero thresholds), with nothing stored.

ObservedViewpoints(const ObservedViewpoints&) = delete

ObservedViewpoints &operator=(const ObservedViewpoints&) = delete

void setThresholds(double position_threshold, double angle_threshold)

Sets both thresholds; the stored viewpoints stay, and the answers over them use the new thresholds.

Zero thresholds disable it: isObserved() and getViewpointIndicesNear() then find nothing.

Parameters:

• position_threshold – Position distance an observed pose lies below [m].

• angle_threshold – Geodesic orientation angle an observed pose lies below [rad].

Throws:

std::invalid_argument – when isValidThresholdPair() rejects the pair; nothing then changes.

inline std::size_t size() const

Number of stored viewpoints.

Append-only between clear() calls, so an index below a count read earlier keeps its viewpoint.

Public Static Functions

static bool isValidThresholdPair(double position_threshold, double angle_threshold, std::string *why = nullptr)

Whether a threshold pair is one setThresholds() accepts.

Valid: both zero (disabled), or a position threshold above 0.01 m and a positive angle threshold. One threshold alone would match on position alone or on orientation alone, and Generator::findNearbyNodesInIndex reads a radius of at most 0.01 as unbounded.

Parameters:

• position_threshold – Position threshold [m].

• angle_threshold – Geodesic orientation threshold [rad].

• why – Optional; set to the reason when the pair is invalid.

Returns:

Whether the pair is valid.

Enumerations

enum class open_apero::Traversability

Node or edge traversability classification.

kUnset: no determination yet, the caller must compute it; raw map checks never return it. kPotentiallyTraversable: an ambiguous map verdict, resolved by safe/clearing-radius optimism.

Values:

enumerator kTraversable

enumerator kPotentiallyTraversable

enumerator kUntraversable

enumerator kUnset

enum class open_apero::SensorMountType

Sensor mounting type relative to the robot base.

Values:

enumerator kFixed

enumerator kFreePitch

enumerator kUnconstrained

Visualization

struct VisualizationMarkers

#include <open_apero_core/visualization/visualization_markers.h>

Container for a collection of visualization markers with auto-incrementing IDs.

Public Functions

void addMarker(const VisualizationMarker &marker)

Appends a single marker with an auto-assigned ID.

Parameters:

marker – The marker to add.

void addMarkers(const std::vector<VisualizationMarker> &markers)

Appends multiple markers, each with an auto-assigned ID.

Parameters:

markers – The markers to add.

std::vector<VisualizationMarker> &getMarkers()

Returns a mutable reference to the list of stored markers.

Returns:

Reference to the markers vector.

const std::vector<VisualizationMarker> &getMarkers() const

Returns a const reference to the list of stored markers.

Returns:

Const reference to the markers vector.

int getNextVisualizationId()

Returns the id after the last stored marker’s (0 when empty); nothing is incremented.

Returns:

Next marker ID.