Pose-graph I/O, trajectory metrics, link utilities, and path planning. More...

Classes
struct	MaxGraphErrors
	Largest pose-graph constraint violations after optimization. More...

Functions
bool RTABMAP_CORE_EXPORT	exportPoses (const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
	Writes poses (and optional constraints) to disk.

bool RTABMAP_CORE_EXPORT	importPoses (const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > constraints=0, std::map< int, double > stamps=0)
	Loads poses (and optional constraints) from disk.

bool RTABMAP_CORE_EXPORT	exportGPS (const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
	Exports GPS samples to a PLY point cloud.

void RTABMAP_CORE_EXPORT	calcKittiSequenceErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
	KITTI odometry benchmark error over fixed trajectory segments.

void RTABMAP_CORE_EXPORT	calcRelativeErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
	Mean frame-to-frame relative pose error (RPE-style, one step).

Transform RTABMAP_CORE_EXPORT	calcRMSE (const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max, bool align2D=false)
	Absolute trajectory error (ATE) with Sim(3)-style alignment (TUM RGB-D tool).

MaxGraphErrors RTABMAP_CORE_EXPORT	computeMaxGraphErrors (const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, bool for3DoF=false)
	Finds the worst pose-graph constraint residuals after optimization.

std::vector< double > RTABMAP_CORE_EXPORT	getMaxOdomInf (const std::multimap< int, Link > &links)
	Maximum information-matrix diagonal over odometry neighbor links.

std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT	findLink (std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
	Finds the first link from `from` to `to` in a multimap keyed by source id.

std::multimap< int, std::pair< int, Link::Type > >::iterator RTABMAP_CORE_EXPORT	findLink (std::multimap< int, std::pair< int, Link::Type > > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)

std::multimap< int, int >::iterator RTABMAP_CORE_EXPORT	findLink (std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)

std::multimap< int, Link >::const_iterator RTABMAP_CORE_EXPORT	findLink (const std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)

std::multimap< int, std::pair< int, Link::Type > >::const_iterator RTABMAP_CORE_EXPORT	findLink (const std::multimap< int, std::pair< int, Link::Type > > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)

std::multimap< int, int >::const_iterator RTABMAP_CORE_EXPORT	findLink (const std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)

std::list< Link > RTABMAP_CORE_EXPORT	findLinks (const std::multimap< int, Link > &links, int from)
	Lists all links incident on node `from`.

std::multimap< int, Link > RTABMAP_CORE_EXPORT	filterDuplicateLinks (const std::multimap< int, Link > &links)
	Removes duplicate undirected links.

std::multimap< int, Link > RTABMAP_CORE_EXPORT	filterLinks (const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
	Filters links by type or self-reference.

std::map< int, Link > RTABMAP_CORE_EXPORT	filterLinks (const std::map< int, Link > &links, Link::Type filteredType, bool inverted=false)

std::map< int, Transform > RTABMAP_CORE_EXPORT	frustumPosesFiltering (const std::map< int, Transform > &poses, const Transform &cameraPose, float horizontalFOV=45.0f, float verticalFOV=45.0f, float nearClipPlaneDistance=0.1f, float farClipPlaneDistance=100.0f, bool negative=false)
	Keeps poses inside (or outside) a camera frustum.

std::map< int, Transform > RTABMAP_CORE_EXPORT	radiusPosesFiltering (const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
	Subsamples poses that are spatially (and optionally angularly) redundant.

std::multimap< int, int > RTABMAP_CORE_EXPORT	radiusPosesClustering (const std::map< int, Transform > &poses, float radius, float angle)
	Radius-neighbor clustering of poses.

void	reduceGraph (const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, std::multimap< int, int > &hyperNodes, std::multimap< int, Link > &hyperLinks)
	Reduces a pose graph into hyper-nodes and hyper-links.

std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT	computePath (const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
	A* shortest path on a pose graph with Euclidean edge costs.

std::list< int > RTABMAP_CORE_EXPORT	computePath (const std::multimap< int, Link > &links, int from, int to, bool updateNewCosts=false, bool useSameCostForAllLinks=false)
	Dijkstra shortest path on link constraints.

std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT	computePath (int fromId, int toId, const Memory *memory, bool lookInDatabase=true, bool updateNewCosts=false, float linearVelocity=0.0f, float angularVelocity=0.0f, bool ignoreDirectLinks=false)
	Dijkstra path through the live Memory pose graph.

int RTABMAP_CORE_EXPORT	findNearestNode (const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
	Id of the nearest pose to `targetPose`.

std::map< int, float > RTABMAP_CORE_EXPORT	findNearestNodes (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
	Spatial neighbors of a node (KD-tree radius or k-NN search).

std::map< int, float > RTABMAP_CORE_EXPORT	findNearestNodes (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
	Spatial neighbors of a pose (KD-tree radius or k-NN search).

std::map< int, Transform > RTABMAP_CORE_EXPORT	findNearestPoses (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
	Like findNearestNodes(int,const std::map<int,Transform>&,float,float,int) but returns full Transform values.

std::map< int, Transform > RTABMAP_CORE_EXPORT	findNearestPoses (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT	findNearestNodes (const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT	getNodesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius)

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT	getNodesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius)

RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT	getPosesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)

RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT	getPosesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)

float RTABMAP_CORE_EXPORT	computePathLength (const std::vector< std::pair< int, Transform > > &path)
	Path length along an ordered list of poses.

float RTABMAP_CORE_EXPORT	computePathLength (const std::map< int, Transform > &path)
	Path length in map iteration order.

std::list< std::map< int, Transform > > RTABMAP_CORE_EXPORT	getPaths (std::map< int, Transform > poses, const std::multimap< int, Link > &links)
	Splits poses into chains connected only by neighbor links.

void RTABMAP_CORE_EXPORT	computeMinMax (const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
	Axis-aligned bounding box of pose positions.

Detailed Description

Pose-graph I/O, trajectory metrics, link utilities, and path planning.

Functions operate on maps of signature ids to Transform poses and Link constraints (typically stored as std::multimap<int, Link> keyed by the source node id).

Main groups:

I/O: exportPoses(), importPoses(), exportGPS()
Evaluation: calcKittiSequenceErrors(), calcRelativeErrors(), calcRMSE(), computeMaxGraphErrors()
Links: findLink(), findLinks(), filterLinks(), filterDuplicateLinks()
Spatial queries: findNearestNode(), findNearestNodes(), frustumPosesFiltering(), radiusPosesFiltering()
Planning: computePath(), computePathLength(), getPaths()

Function Documentation

◆ exportPoses()

bool RTABMAP_CORE_EXPORT rtabmap::graph::exportPoses	(	const std::string &	filePath,
		int	format,
		const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	constraints = `std::multimap< int, Link >()`,
		const std::map< int, double > &	stamps = `std::map< int, double >()`,
		const ParametersMap &	parameters = `ParametersMap()`
	)

Writes poses (and optional constraints) to disk.

Parameters

filePath	Output path; extension may be appended from `format`.
format	Export format: `0` Raw text (`.txt`): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz `1` RGBD-SLAM format, in motion capture frame like the ground truth of RGB-D SLAM Dataset (requires `stamps`) : stamp x y z qx qy qz qw `10` Like `1` without coordinate-frame change (i.e., in base frame) : stamp x y z qx qy qz qw `11` Like `10` with landmark ids after positive ids : stamp x y z qx qy qz qw id `2` KITTI odometry format : r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz `3` TORO graph (requires `constraints`; uses `parameters`) `4` g2o (requires `constraints`; uses `parameters`)
poses	Node id → pose.
constraints	Required for formats `3` and `4`.
stamps	Required for formats `1`, `10`, and `11` (same size as `poses`).
parameters	Optional optimizer parameters for formats `3` and `4`.

Returns: False on I/O or validation error.

◆ importPoses()

bool RTABMAP_CORE_EXPORT rtabmap::graph::importPoses	(	const std::string &	filePath,
		int	format,
		std::map< int, Transform > &	poses,
		std::multimap< int, Link > *	constraints = `0`,
		std::map< int, double > *	stamps = `0`
	)

Loads poses (and optional constraints) from disk.

Parameters

filePath	Input path.
format	Import format: `0` Raw text: 3×4 matrix per line (`Transform::fromString()`) `1` RGBD-SLAM motion capture: stamp x y z qw qx qy qz (applies optical-frame conversion) `2` KITTI odometry: 3×4 matrix per line (applies optical-frame conversion) `3` TORO graph (fills `constraints`) `4` g2o (not supported yet) `5` NewCollege: stamp x y (2D; first pose is origin) `6` Malaga Urban GPS: 25-field `*_GPS.txt` line (local X/Y/Z) `7` St Lucia INS: 12-field log (GPS → local ENU + roll/pitch/yaw) `8` Karlsruhe: timestamp lat lon alt x y z roll pitch yaw (first pose is origin) `9` EuRoC MAV: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az (17 CSV fields) `10` RGBD-SLAM like `1` without coordinate-frame change `11` RGBD-SLAM like `10` with node id as 9th field: stamp x y z qw qx qy qz id `12` RGBD Bonn dynamic dataset format (stamp + pose; Bonn-specific frame conversion)
poses	Output node id → pose.
constraints	Optional output links (format `3` only).
stamps	Optional output timestamps (formats `1`, `5`–`9`, `10`–`12` when present in file).

Returns: False on I/O or parse error.

◆ exportGPS()

bool RTABMAP_CORE_EXPORT rtabmap::graph::exportGPS	(	const std::string &	filePath,
		const std::map< int, GPS > &	gpsValues,
		unsigned int	rgba = `0xFFFFFFFF`
	)

Exports GPS samples to a PLY point cloud.

Parameters

filePath	Output `.ply` path.
gpsValues	Node id → GPS fix.
rgba	Point color (default opaque white).

◆ calcKittiSequenceErrors()

void RTABMAP_CORE_EXPORT rtabmap::graph::calcKittiSequenceErrors	(	const std::vector< Transform > &	poses_gt,
		const std::vector< Transform > &	poses_result,
		float &	t_err,
		float &	r_err
	)

KITTI odometry benchmark error over fixed trajectory segments.

For each start pose (every 10 frames) and segment length in {100, 200, …, 800} m along poses_gt, compares the relative transform GT vs estimate and accumulates normalized errors. The returned values are the mean over all valid segments.

Parameters

poses_gt	Ground-truth poses in temporal order (one per frame).
poses_result	Estimated poses (same length and ordering as `poses_gt`).
t_err	Output mean translation error (%): segment translation error (m) divided by segment length, averaged, then × 100.
r_err	Output mean rotation error (deg/m): segment rotation error (rad) divided by segment length, averaged, then converted to deg/m.

See also: http://www.cvlibs.net/datasets/kitti/eval_odometry.php

◆ calcRelativeErrors()

void RTABMAP_CORE_EXPORT rtabmap::graph::calcRelativeErrors	(	const std::vector< Transform > &	poses_gt,
		const std::vector< Transform > &	poses_result,
		float &	t_err,
		float &	r_err
	)

Mean frame-to-frame relative pose error (RPE-style, one step).

For each consecutive pair (i, i+1), builds the relative motion in ground truth and in the estimate, then measures how much they differ:

translation: Euclidean distance between the two relative transforms (m)
rotation: angle between the two relative transforms (rad → deg)

Returns the arithmetic mean over all N-1 pairs (N = trajectory length). Unlike calcKittiSequenceErrors(), there is no fixed segment length and no path-length normalization.

Parameters

poses_gt	Ground-truth poses in temporal order (one per frame).
poses_result	Estimated poses (same length and ordering as `poses_gt`).
t_err	Output mean translation error over consecutive pairs (m).
r_err	Output mean rotation error over consecutive pairs (deg).

◆ calcRMSE()

Transform RTABMAP_CORE_EXPORT rtabmap::graph::calcRMSE	(	const std::map< int, Transform > &	groundTruth,
		const std::map< int, Transform > &	poses,
		float &	translational_rmse,
		float &	translational_mean,
		float &	translational_median,
		float &	translational_std,
		float &	translational_min,
		float &	translational_max,
		float &	rotational_rmse,
		float &	rotational_mean,
		float &	rotational_median,
		float &	rotational_std,
		float &	rotational_min,
		float &	rotational_max,
		bool	align2D = `false`
	)

Absolute trajectory error (ATE) with Sim(3)-style alignment (TUM RGB-D tool).

Only poses whose id exists in both groundTruth and poses are compared. An alignment transform t is estimated so that per-pose error is measured after bringing the estimate into the reference frame:

If more than five poses match: t from SVD on position correspondences (estimate positions → ground-truth positions; z ignored when align2D is true).
Otherwise: t = groundTruth[firstId] * poses[firstId]⁻¹ using the first matched id.

For each matched pose, after aligned = t * poses[id]:

Translational error: Euclidean distance between aligned and groundTruth[id] (m).
Rotational error: Angle between the poses' +X axes (deg).

The eight @p translational_* and @p rotational_* outputs are statistics over those per-pose errors (all matched poses). They are set to 0 when no id matches.

Parameters

groundTruth	Reference trajectory (node id → pose).
poses	Estimated trajectory; ids not in `groundTruth` are skipped.
translational_rmse	Root mean square of translational errors (m).
translational_mean	Arithmetic mean of translational errors (m).
translational_median	Middle sample in matched-pose iteration order (m).
translational_std	Standard deviation of translational errors (m).
translational_min	Minimum translational error (m).
translational_max	Maximum translational error (m).
rotational_rmse	Root mean square of rotational errors (deg).
rotational_mean	Arithmetic mean of rotational errors (deg).
rotational_median	Middle sample in matched-pose iteration order (deg).
rotational_std	Standard deviation of rotational errors (deg).
rotational_min	Minimum rotational error (deg).
rotational_max	Maximum rotational error (deg).
align2D	If true, alignment uses x/y only (z set to 0 for correspondence); 3D if false.

Returns: Alignment transform t applied as t * poses[id] before error computation.

See also: https://vision.in.tum.de/data/datasets/rgbd-dataset

◆ computeMaxGraphErrors()

MaxGraphErrors RTABMAP_CORE_EXPORT rtabmap::graph::computeMaxGraphErrors	(	const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	links,
		bool	for3DoF = `false`
	)

Finds the worst pose-graph constraint residuals after optimization.

Iterates over links and, for each non-self-referenced edge (from != to):

Looks up T_from and T_to in poses (returns default MaxGraphErrors if any endpoint pose is missing, null, or not invertible).
Builds the relative pose implied by the optimized poses:
- Normal link: t = T_from⁻¹ · T_to
- Landmark (from < 0): t = T_to⁻¹ · T_from, link measurement inverted
Compares t to the link transform:
- Linear error: max |Δx|, |Δy|, and |Δz| (z ignored when for3DoF is true).
- Angular error: full 3D angle between t and the link, or yaw-only if for3DoF; skipped for Link::kLandmark when the information matrix does not constrain yaw.
Normalizes by link uncertainty: error / sqrt(variance) using the link information matrix (largest diagonal variance for translation/rotation).

The returned MaxGraphErrors holds the link with the highest linear and angular ratios (not necessarily the largest absolute error).

Parameters

poses	Optimized node poses (must contain every `from` and `to` id used).
links	Graph constraints (typically `std::multimap<int, Link>` keyed by `from`).
for3DoF	If true, linear error uses x/y only and angular error compares yaw only.

Returns: MaxGraphErrors; fields stay -1 when no valid link was checked or on early abort.

◆ getMaxOdomInf()

std::vector< double > RTABMAP_CORE_EXPORT rtabmap::graph::getMaxOdomInf ( const std::multimap< int, Link > & links )

Maximum information-matrix diagonal over odometry neighbor links.

Scans links of type Link::kNeighbor or Link::kNeighborMerged and, for each dof (x, y, z, roll, pitch, yaw), keeps the largest diagonal entry of the 6×6 information matrix.

Parameters

links Graph constraints (multimap keyed by source id).

Returns: Six maximum information values, or an empty vector if no neighbor links exist.

◆ findLink() [1/6]

std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	std::multimap< int, Link > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`,
		Link::Type	type = `Link::kUndef`
	)

Finds the first link from from to to in a multimap keyed by source id.

Iterates all entries with key from and matches the destination (and optionally type). When checkBothWays is true, also searches key to for a link back to from.

Parameters

links	Link multimap (`key` = source node id).
from	Source node id.
to	Destination node id.
checkBothWays	If true, also match `to → from`.
type	Required link type, or Link::kUndef to accept any type.

Returns: Iterator to the link, or links.end() if not found.

◆ findLink() [2/6]

std::multimap< int, std::pair< int, Link::Type > >::iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	std::multimap< int, std::pair< int, Link::Type > > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`,
		Link::Type	type = `Link::kUndef`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. std::multimap<int, std::pair<int, Link::Type>>.

◆ findLink() [3/6]

std::multimap< int, int >::iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	std::multimap< int, int > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. std::multimap<int, int>.

◆ findLink() [4/6]

std::multimap< int, Link >::const_iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	const std::multimap< int, Link > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`,
		Link::Type	type = `Link::kUndef`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const std::multimap<int, Link>.

◆ findLink() [5/6]

std::multimap< int, std::pair< int, Link::Type > >::const_iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	const std::multimap< int, std::pair< int, Link::Type > > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`,
		Link::Type	type = `Link::kUndef`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const std::multimap<int, std::pair<int, Link::Type>>.

◆ findLink() [6/6]

std::multimap< int, int >::const_iterator RTABMAP_CORE_EXPORT rtabmap::graph::findLink	(	const std::multimap< int, int > &	links,
		int	from,
		int	to,
		bool	checkBothWays = `true`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const std::multimap<int, int>.

◆ findLinks()

std::list< Link > RTABMAP_CORE_EXPORT rtabmap::graph::findLinks	(	const std::multimap< int, Link > &	links,
		int	from
	)

Lists all links incident on node from.

Outgoing links (link.from() == from) are returned as stored; for incoming links (link.to() == from), the inverse link is returned so the pose of from is always the source frame.

Parameters

links	Graph constraints.
from	Node id to query.

Returns: Incident links (may be empty).

◆ filterDuplicateLinks()

std::multimap< int, Link > RTABMAP_CORE_EXPORT rtabmap::graph::filterDuplicateLinks ( const std::multimap< int, Link > & links )

Removes duplicate undirected links.

Keeps the first occurrence of each (from, to) or (to, from) pair with the same Link::Type (see findLink() with checkBothWays).

Parameters

links Input link multimap.

Returns: Copy without duplicates.

◆ filterLinks() [1/2]

std::multimap< int, Link > RTABMAP_CORE_EXPORT rtabmap::graph::filterLinks	(	const std::multimap< int, Link > &	links,
		Link::Type	filteredType,
		bool	inverted = `false`
	)

Filters links by type or self-reference.

If filteredType is kSelfRefLink: exclude self-references (from == to), or include only them when inverted is true.
Otherwise: exclude links of filteredType, or keep only that type when inverted is true.

Parameters

links	Input links.
filteredType	Type to filter, or Link::kSelfRefLink for self-reference filtering.
inverted	If true, keep the filtered set instead of removing it.

Returns: Filtered link container (same structure as input).

◆ filterLinks() [2/2]

std::map< int, Link > RTABMAP_CORE_EXPORT rtabmap::graph::filterLinks	(	const std::map< int, Link > &	links,
		Link::Type	filteredType,
		bool	inverted = `false`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. For std::map<int, Link>.

◆ frustumPosesFiltering()

std::map< int, Transform > RTABMAP_CORE_EXPORT rtabmap::graph::frustumPosesFiltering	(	const std::map< int, Transform > &	poses,
		const Transform &	cameraPose,
		float	horizontalFOV = `45.0f`,
		float	verticalFOV = `45.0f`,
		float	nearClipPlaneDistance = `0.1f`,
		float	farClipPlaneDistance = `100.0f`,
		bool	negative = `false`
	)

Keeps poses inside (or outside) a camera frustum.

Transforms each pose position into the frustum defined by cameraPose using util3d::frustumFiltering() (this assumes the cameraPose includes the optical rotation of the camera (X right, Y down, Z forward).

Parameters

poses	Input poses (null poses are skipped) in base frame (X forward, Y left, Z up),
cameraPose	Frustum origin and orientation including the optical rotation of the camera (X right, Y down, Z forward).
horizontalFOV	Horizontal field of view (deg); see CameraModel::horizontalFOV().
verticalFOV	Vertical field of view (deg); see CameraModel::verticalFOV().
nearClipPlaneDistance	Near clipping distance (m).
farClipPlaneDistance	Far clipping distance (m).
negative	If false, keep poses inside the frustum; if true, keep poses outside.

Returns: Subset of poses passing the filter.

◆ radiusPosesFiltering()

std::map< int, Transform > RTABMAP_CORE_EXPORT rtabmap::graph::radiusPosesFiltering	(	const std::map< int, Transform > &	poses,
		float	radius,
		float	angle,
		bool	keepLatest = `true`
	)

Subsamples poses that are spatially (and optionally angularly) redundant.

For each pose not yet processed, finds all poses within radius (KD-tree). When angle > 0, only poses whose +X axis differs by at most angle (rad) are grouped. From each group, keeps one pose: the latest in map order if keepLatest, otherwise the earliest. The first and last poses of the input map are always kept.

Parameters

poses	Input trajectory (map iteration order defines “latest/oldest”).
radius	Clustering radius (m); if `≤ 0` or fewer than three poses, returns `poses` unchanged.
angle	Max heading difference within a cluster (rad); `0` ignores orientation.
keepLatest	If true, keep the latest pose per cluster; otherwise the earliest.

Returns: Subsampled poses.

◆ radiusPosesClustering()

std::multimap< int, int > RTABMAP_CORE_EXPORT rtabmap::graph::radiusPosesClustering	(	const std::map< int, Transform > &	poses,
		float	radius,
		float	angle
	)

Radius-neighbor clustering of poses.

For each pose, inserts (queryId, neighborId) into the output for every other pose within radius (and within angle of the query heading when angle > 0).

Parameters

poses	Input poses.
radius	Search radius (m); no pairs if `≤ 0` or fewer than two poses.
angle	Max heading difference (rad); `0` ignores orientation.

Returns: Multimap of pose id → neighbor id (both ids from poses).

◆ reduceGraph()

void rtabmap::graph::reduceGraph	(	const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	links,
		std::multimap< int, int > &	hyperNodes,
		std::multimap< int, Link > &	hyperLinks
	)

Reduces a pose graph into hyper-nodes and hyper-links.

Hyper-nodes: clusters poses connected by non-neighbor loop-closure links. Clustering starts from the largest id downward; each cluster is keyed by its parent (hyper-node) id.

Hyper-links: for each Link::kNeighbor or Link::kNeighborMerged link between different clusters, builds one merged Link along the shortest path through intra-cluster closure links (Dijkstra with unit cost).

Parameters

poses	Input optimized poses.
links	Input constraints (should be unique per directed edge for closure links).
hyperNodes	Output `hyperNodeId → childPoseId` membership.
hyperLinks	Output links between hyper-nodes (one per hyper-edge, most recent kept).

◆ computePath() [1/3]

std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT rtabmap::graph::computePath	(	const std::map< int, rtabmap::Transform > &	poses,
		const std::multimap< int, int > &	links,
		int	from,
		int	to,
		bool	updateNewCosts = `false`
	)

A* shortest path on a pose graph with Euclidean edge costs.

Edge cost between adjacent nodes is the Euclidean distance between their poses in poses. Uses costSoFar + distToEnd where distToEnd is the distance to the goal pose.

Parameters

poses	Node id → pose (must contain every node reached by `links`).
links	Directed edges (`from` → `to`) keyed by source id.
from	Start node id.
to	Goal node id.
updateNewCosts	If true, use a multimap queue that can decrease keys when a shorter path is found.

Returns: Ordered path from from to to (inclusive) with poses; empty if unreachable.

◆ computePath() [2/3]

std::list< int > RTABMAP_CORE_EXPORT rtabmap::graph::computePath	(	const std::multimap< int, Link > &	links,
		int	from,
		int	to,
		bool	updateNewCosts = `false`,
		bool	useSameCostForAllLinks = `false`
	)

Dijkstra shortest path on link constraints.

Explores outgoing links keyed by link.from(). Edge cost is 1 when useSameCostForAllLinks is true, otherwise the translation norm of the link transform.

Parameters

links	Constraints keyed by source node id.
from	Start node id.
to	Goal node id.
updateNewCosts	If true, allow cost improvements on open nodes.
useSameCostForAllLinks	If true, unit edge cost; else use `link.transform().getNorm()`.

Returns: Node ids from from to to (inclusive); empty if unreachable.

◆ computePath() [3/3]

std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT rtabmap::graph::computePath	(	int	fromId,
		int	toId,
		const Memory *	memory,
		bool	lookInDatabase = `true`,
		bool	updateNewCosts = `false`,
		float	linearVelocity = `0.0f`,
		float	angularVelocity = `0.0f`,
		bool	ignoreDirectLinks = `false`
	)

Dijkstra path through the live Memory pose graph.

Loads links from Memory (optionally from the database), chains transforms along the chosen path, and returns the accumulated poses. Self-referenced links are skipped.

By default (linearVelocity and angularVelocity ≤ 0), edge cost is translation distance (m) only. When set > 0, costs are expressed in seconds of motion:

linearVelocity adds linkTranslation / linearVelocity (time to drive the edge at that speed). Used alone it scales every edge by the same factor, so the shortest path is unchanged; set it to your robot’s typical forward speed (e.g. 0.5 m/s) when you also use angularVelocity so translation and rotation costs are comparable.
angularVelocity adds headingMismatch / angularVelocity, where heading mismatch is the angle between the displacement to the next node and that node’s forward (+X) axis. This is what changes which path is chosen: a chain followed mostly forward (small mismatch) can beat a shorter route through loop closures that require large reorientations (e.g. angularVelocity = 1.0 rad/s with linearVelocity = 0.5 m/s). With angularVelocity > 0 and linearVelocity ≤ 0, translation is ignored and the path minimizes heading mismatch only (forward-following paths, regardless of distance). This can help loop-closure detection when the map was built with a forward-facing camera: the path stays aligned with how places were observed while driving forward.

Parameters

fromId	Start signature id (`≥ 0`).
toId	Goal signature id (`≠ 0`).
memory	Graph memory (must not be null).
lookInDatabase	If true, load links from the database when not already in RAM.
updateNewCosts	If true, allow cost improvements on open nodes.
linearVelocity	If > 0, add `translationNorm / linearVelocity` to edge cost (m/s).
angularVelocity	If > 0, add rotation time from motion direction change (rad/s).
ignoreDirectLinks	If true, skip the direct edge between `fromId` and `toId`.

Returns: Path as (nodeId, pose) pairs; first pose is identity at fromId. Empty if unreachable.

◆ findNearestNode()

int RTABMAP_CORE_EXPORT rtabmap::graph::findNearestNode	(	const std::map< int, rtabmap::Transform > &	poses,
		const rtabmap::Transform &	targetPose,
		float *	distance = `0`
	)

Id of the nearest pose to targetPose.

Wrapper around findNearestNodes() with radius=0, k=1 (1-NN in 3D).

Parameters

poses	Nodes to search.
targetPose	Query position (x, y, z only; orientation is not used).
distance	If not null, set to the squared Euclidean distance of the match.

Returns: Closest node id, or 0 if poses is empty.

◆ findNearestNodes() [1/3]

std::map< int, float > RTABMAP_CORE_EXPORT rtabmap::graph::findNearestNodes	(	int	nodeId,
		const std::map< int, Transform > &	poses,
		float	radius,
		float	angle = `0.0f`,
		int	k = `0`
	)

Spatial neighbors of a node (KD-tree radius or k-NN search).

nodeId is removed from the search set. Requires radius > 0 or k > 0. When radius > 0, returns all poses within radius (up to k if k > 0). When radius == 0, returns the k nearest neighbors.

Parameters

nodeId	Query node (must exist in `poses`); excluded from results.
poses	Candidate poses.
radius	Search radius (m).
angle	Max +X axis angle difference (rad); `0` ignores heading.
k	Max neighbors (`0` = all within radius).

Returns: Neighbor id → squared Euclidean distance.

◆ findNearestNodes() [2/3]

std::map< int, float > RTABMAP_CORE_EXPORT rtabmap::graph::findNearestNodes	(	const Transform &	targetPose,
		const std::map< int, Transform > &	poses,
		float	radius,
		float	angle = `0.0f`,
		int	k = `0`
	)

Spatial neighbors of a pose (KD-tree radius or k-NN search).

Parameters

targetPose	Query pose (position used; orientation used when `angle` > 0).
poses	Candidate poses (not modified).
radius	Search radius (m).
angle	Max +X axis angle difference (rad); `0` ignores heading.
k	Max neighbors (`0` = all within radius).

Returns: Neighbor id → squared Euclidean distance.

◆ findNearestPoses()

std::map< int, Transform > RTABMAP_CORE_EXPORT rtabmap::graph::findNearestPoses	(	const Transform &	targetPose,
		const std::map< int, Transform > &	poses,
		float	radius,
		float	angle = `0.0f`,
		int	k = `0`
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Query by Transform instead of node id.

◆ findNearestNodes() [3/3]

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT rtabmap::graph::findNearestNodes	(	const std::map< int, rtabmap::Transform > &	nodes,
		const rtabmap::Transform &	targetPose,
		int	k
	)

Deprecated:: Use findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int) with radius=0, k set.

◆ getNodesInRadius() [1/2]

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT rtabmap::graph::getNodesInRadius	(	int	nodeId,
		const std::map< int, Transform > &	nodes,
		float	radius
	)

Deprecated:: Use findNearestNodes(int,const std::map<int,Transform>&,float,float,int).

◆ getNodesInRadius() [2/2]

RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT rtabmap::graph::getNodesInRadius	(	const Transform &	targetPose,
		const std::map< int, Transform > &	nodes,
		float	radius
	)

Deprecated:: Use findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int).

◆ getPosesInRadius() [1/2]

RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT rtabmap::graph::getPosesInRadius	(	int	nodeId,
		const std::map< int, Transform > &	nodes,
		float	radius,
		float	angle = `0.0f`
	)

Deprecated:: Use findNearestPoses(int,const std::map<int,Transform>&,float,float,int).

◆ getPosesInRadius() [2/2]

RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT rtabmap::graph::getPosesInRadius	(	const Transform &	targetPose,
		const std::map< int, Transform > &	nodes,
		float	radius,
		float	angle = `0.0f`
	)

Deprecated:: Use findNearestPoses(const Transform&,const std::map<int,Transform>&,float,float,int).

◆ computePathLength() [1/2]

float RTABMAP_CORE_EXPORT rtabmap::graph::computePathLength ( const std::vector< std::pair< int, Transform > > & path )

Path length along an ordered list of poses.

Sums path[i].second.getDistance(path[i+1].second) for consecutive entries.

Parameters

path	Ordered `(nodeId, pose)` pairs.

Returns: Total length (m), or 0 if fewer than two poses.

◆ computePathLength() [2/2]

float RTABMAP_CORE_EXPORT rtabmap::graph::computePathLength ( const std::map< int, Transform > & path )

Path length in map iteration order.

Sums distances between consecutive poses in ascending map key order (does not verify that entries form a connected path in the graph).

Parameters

path	Poses keyed by node id (sorted by key).

Returns: Total length (m), or 0 if fewer than two poses.

◆ getPaths()

std::list< std::map< int, Transform > > RTABMAP_CORE_EXPORT rtabmap::graph::getPaths	(	std::map< int, Transform >	poses,
		const std::multimap< int, Link > &	links
	)

Splits poses into chains connected only by neighbor links.

Repeatedly builds a path starting from the lowest remaining id: adds the next pose in map order only if a Link::kNeighbor or Link::kNeighborMerged link exists from the previous pose to it. Stops at the first gap, pushes the chain, and continues until poses is empty.

Parameters

poses	Input poses (cleared as segments are extracted).
links	Graph constraints keyed by source id.

Returns: List of pose maps, each a contiguous neighbor chain.

◆ computeMinMax()

void RTABMAP_CORE_EXPORT rtabmap::graph::computeMinMax	(	const std::map< int, Transform > &	poses,
		cv::Vec3f &	min,
		cv::Vec3f &	max
	)

Axis-aligned bounding box of pose positions.

Parameters

poses	Input poses (no effect if empty).
min	Output minimum (x, y, z) in meters.
max	Output maximum (x, y, z) in meters.

Classes

Functions

Detailed Description

Function Documentation

◆ exportPoses()

◆ importPoses()

◆ exportGPS()

◆ calcKittiSequenceErrors()

◆ calcRelativeErrors()

◆ calcRMSE()

◆ computeMaxGraphErrors()

◆ getMaxOdomInf()

◆ findLink() [1/6]

◆ findLink() [2/6]

◆ findLink() [3/6]

◆ findLink() [4/6]

◆ findLink() [5/6]

◆ findLink() [6/6]

◆ findLinks()

◆ filterDuplicateLinks()

◆ filterLinks() [1/2]

◆ filterLinks() [2/2]

◆ frustumPosesFiltering()

◆ radiusPosesFiltering()

◆ radiusPosesClustering()

◆ reduceGraph()

◆ computePath() [1/3]

◆ computePath() [2/3]

◆ computePath() [3/3]

◆ findNearestNode()

◆ findNearestNodes() [1/3]

◆ findNearestNodes() [2/3]

◆ findNearestPoses()

◆ findNearestNodes() [3/3]

◆ getNodesInRadius() [1/2]

◆ getNodesInRadius() [2/2]

◆ getPosesInRadius() [1/2]

◆ getPosesInRadius() [2/2]

◆ computePathLength() [1/2]

◆ computePathLength() [2/2]

◆ getPaths()

◆ computeMinMax()