Added Graph tests

corelib/include/rtabmap/core/Graph.h

@@ -40,39 +40,106 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 class Memory;
 
+/**
+ * @namespace graph
+ * @brief Pose-graph I/O, trajectory metrics, link utilities, and path planning.
+ *
+ * Functions operate on maps of signature ids to @ref Transform poses and
+ * @ref Link constraints (typically stored as `std::multimap<int, Link>` keyed by
+ * the source node id).
+ *
+ * Main groups:
+ * - **I/O:** @ref exportPoses(), @ref importPoses(), @ref exportGPS()
+ * - **Evaluation:** @ref calcKittiSequenceErrors(), @ref calcRelativeErrors(),
+ *   @ref calcRMSE(), @ref computeMaxGraphErrors()
+ * - **Links:** @ref findLink(), @ref findLinks(), @ref filterLinks(),
+ *   @ref filterDuplicateLinks()
+ * - **Spatial queries:** @ref findNearestNode(), @ref findNearestNodes(),
+ *   @ref frustumPosesFiltering(), @ref radiusPosesFiltering()
+ * - **Planning:** @ref computePath(), @ref computePathLength(), @ref getPaths()
+ */
 namespace graph {
 
-////////////////////////////////////////////
-// Graph utilities
-////////////////////////////////////////////
-
+/**
+ * @brief Writes poses (and optional constraints) to disk.
+ * @param filePath Output path; extension may be appended from @p format.
+ * @param 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 @p 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 @p constraints; uses @p parameters)
+ *   - `4` g2o (requires @p constraints; uses @p parameters)
+ * @param poses Node id → pose.
+ * @param constraints Required for formats `3` and `4`.
+ * @param stamps Required for formats `1`, `10`, and `11` (same size as @p poses).
+ * @param parameters Optional optimizer parameters for formats `3` and `4`.
+ * @return False on I/O or validation error.
+ */
 bool RTABMAP_CORE_EXPORT exportPoses(
 		const std::string & filePath,
-		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		int format,
 		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
-		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
-		const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
+		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(),
+		const std::map<int, double> & stamps = std::map<int, double>(),
+		const ParametersMap & parameters = ParametersMap());
 
+/**
+ * @brief Loads poses (and optional constraints) from disk.
+ * @param filePath Input path.
+ * @param 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 @p 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)
+ * @param poses Output node id → pose.
+ * @param constraints Optional output links (format `3` only).
+ * @param stamps Optional output timestamps (formats `1`, `5`–`9`, `10`–`12` when present in file).
+ * @return False on I/O or parse error.
+ */
 bool RTABMAP_CORE_EXPORT importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
+		int format,
 		std::map<int, Transform> & poses,
-		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
-		std::map<int, double> * stamps = 0); // optional for format 1 and 9
+		std::multimap<int, Link> * constraints = 0,
+		std::map<int, double> * stamps = 0);
 
+/**
+ * @brief Exports GPS samples to a PLY point cloud.
+ * @param filePath Output `.ply` path.
+ * @param gpsValues Node id → @ref GPS fix.
+ * @param rgba Point color (default opaque white).
+ */
 bool RTABMAP_CORE_EXPORT exportGPS(
 		const std::string & filePath,
 		const std::map<int, GPS> & gpsValues,
 		unsigned int rgba = 0xFFFFFFFF);
 
 /**
- * Compute translation and rotation errors for KITTI datasets.
- * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
- * @param poses_gt, Ground Truth poses
- * @param poses_result, Estimated poses
- * @param t_err, Output translation error (%)
- * @param r_err, Output rotation error (deg/m)
+ * @brief KITTI odometry benchmark error over fixed trajectory segments.
+ *
+ * For each start pose (every 10 frames) and segment length in
+ * {100, 200, …, 800} m along @p poses_gt, compares the relative transform
+ * GT vs estimate and accumulates normalized errors. The returned values are
+ * the mean over all valid segments.
+ *
+ * @param poses_gt Ground-truth poses in temporal order (one per frame).
+ * @param poses_result Estimated poses (same length and ordering as @p poses_gt).
+ * @param t_err Output mean translation error (%): segment translation error (m)
+ *              divided by segment length, averaged, then × 100.
+ * @param r_err Output mean rotation error (deg/m): segment rotation error (rad)
+ *              divided by segment length, averaged, then converted to deg/m.
+ * @see http://www.cvlibs.net/datasets/kitti/eval_odometry.php
  */
 void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
 		const std::vector<Transform> &poses_gt,
@@ -81,11 +148,21 @@ void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
 		float & r_err);
 
 /**
- * Compute average of translation and rotation errors between each poses.
- * @param poses_gt, Ground Truth poses
- * @param poses_result, Estimated poses
- * @param t_err, Output translation error (m)
- * @param r_err, Output rotation error (deg)
+ * @brief 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 @ref calcKittiSequenceErrors(), there is no fixed segment length and
+ * no path-length normalization.
+ *
+ * @param poses_gt Ground-truth poses in temporal order (one per frame).
+ * @param poses_result Estimated poses (same length and ordering as @p poses_gt).
+ * @param t_err Output mean translation error over consecutive pairs (m).
+ * @param r_err Output mean rotation error over consecutive pairs (deg).
  */
 void RTABMAP_CORE_EXPORT calcRelativeErrors (
 		const std::vector<Transform> &poses_gt,
@@ -94,12 +171,39 @@ void RTABMAP_CORE_EXPORT calcRelativeErrors (
 		float & r_err);
 
 /**
- * Compute root-mean-square error (RMSE) like the TUM RGBD
- * dataset's evaluation tool (absolute trajectory error).
- * See https://vision.in.tum.de/data/datasets/rgbd-dataset
- * @param groundTruth, Ground Truth poses
- * @param poses, Estimated poses
- * @return Gt to Map transform
+ * @brief Absolute trajectory error (ATE) with Sim(3)-style alignment (TUM RGB-D tool).
+ *
+ * Only poses whose id exists in both @p groundTruth and @p poses are compared.
+ * An alignment transform @c t is estimated so that per-pose error is measured after
+ * bringing the estimate into the reference frame:
+ * - If more than five poses match: @c t from SVD on position correspondences
+ *   (estimate positions → ground-truth positions; z ignored when @p align2D is true).
+ * - Otherwise: @c 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.
+ *
+ * @param groundTruth Reference trajectory (node id → pose).
+ * @param poses Estimated trajectory; ids not in @p groundTruth are skipped.
+ * @param translational_rmse Root mean square of translational errors (m).
+ * @param translational_mean Arithmetic mean of translational errors (m).
+ * @param translational_median Middle sample in matched-pose iteration order (m).
+ * @param translational_std Standard deviation of translational errors (m).
+ * @param translational_min Minimum translational error (m).
+ * @param translational_max Maximum translational error (m).
+ * @param rotational_rmse Root mean square of rotational errors (deg).
+ * @param rotational_mean Arithmetic mean of rotational errors (deg).
+ * @param rotational_median Middle sample in matched-pose iteration order (deg).
+ * @param rotational_std Standard deviation of rotational errors (deg).
+ * @param rotational_min Minimum rotational error (deg).
+ * @param rotational_max Maximum rotational error (deg).
+ * @param align2D If true, alignment uses x/y only (z set to 0 for correspondence); 3D if false.
+ * @return Alignment transform @c t applied as `t * poses[id]` before error computation.
+ * @see https://vision.in.tum.de/data/datasets/rgbd-dataset
  */
 Transform RTABMAP_CORE_EXPORT calcRMSE(
 		const std::map<int, Transform> &groundTruth,
@@ -118,94 +222,203 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_max,
 		bool align2D = false);
 
+/**
+ * @brief Largest pose-graph constraint violations after optimization.
+ *
+ * For each non-self-referenced link (`from != to`), compares the relative pose implied by @p poses to the
+ * link measurement and tracks the worst linear/angular error and error/std ratios.
+ */
 struct MaxGraphErrors
 {
-	float linear=-1.0f;         // absolute error (m) of the link with maximum linear error
-	float angular=-1.0f;        // absolute error (rad)  of the link with maximum angular error
-	float linearRatio=-1.0f;    // Ratio = absolute error (m) / linear std (m), of the link with maximum linear error
-	float angularRatio=-1.0f;   // Ratio = absolute error (rad) / angular std (rad), of the link with maximum angular error
-	Link linearLink;  // link with maximum linear error
-	Link angularLink; // link with maximum angular error
+	float linear=-1.0f;         ///< Absolute linear error (m) of the worst link.
+	float angular=-1.0f;        ///< Absolute angular error (rad) of the worst link.
+	float linearRatio=-1.0f;    ///< linear / sqrt(trans variance) of the worst link.
+	float angularRatio=-1.0f;   ///< angular / sqrt(rot variance) of the worst link.
+	Link linearLink;            ///< Link with largest @ref linearRatio.
+	Link angularLink;           ///< Link with largest @ref angularRatio.
 };
+
+/**
+ * @brief Finds the worst pose-graph constraint residuals after optimization.
+ *
+ * Iterates over @p links and, for each non-self-referenced edge (`from != to`):
+ * 1. Looks up `T_from` and `T_to` in @p poses (returns default @ref MaxGraphErrors if
+ *    any endpoint pose is missing, null, or not invertible).
+ * 2. 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
+ * 3. Compares `t` to the link transform:
+ *    - **Linear error:** max |Δx|, |Δy|, and |Δz| (z ignored when @p for3DoF is true).
+ *    - **Angular error:** full 3D angle between `t` and the link, or yaw-only if @p for3DoF;
+ *      skipped for @ref Link::kLandmark when the information matrix does not constrain yaw.
+ * 4. Normalizes by link uncertainty: `error / sqrt(variance)` using the link information
+ *    matrix (largest diagonal variance for translation/rotation).
+ *
+ * The returned @ref MaxGraphErrors holds the link with the highest linear and angular
+ * *ratios* (not necessarily the largest absolute error).
+ *
+ * @param poses Optimized node poses (must contain every `from` and `to` id used).
+ * @param links Graph constraints (typically `std::multimap<int, Link>` keyed by `from`).
+ * @param for3DoF If true, linear error uses x/y only and angular error compares yaw only.
+ * @return @ref MaxGraphErrors; fields stay `-1` when no valid link was checked or on early abort.
+ */
 MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		bool for3DoF = false);
 
+/**
+ * @brief Maximum information-matrix diagonal over odometry neighbor links.
+ *
+ * Scans @p links of type @ref Link::kNeighbor or @ref Link::kNeighborMerged and,
+ * for each dof (x, y, z, roll, pitch, yaw), keeps the largest diagonal entry of
+ * the 6×6 information matrix.
+ *
+ * @param links Graph constraints (multimap keyed by source id).
+ * @return Six maximum information values, or an empty vector if no neighbor links exist.
+ */
 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
 
+/**
+ * @brief Finds the first link from @p from to @p to in a multimap keyed by source id.
+ *
+ * Iterates all entries with key @p from and matches the destination (and optionally
+ * @p type). When @p checkBothWays is true, also searches key @p to for a link back
+ * to @p from.
+ *
+ * @param links Link multimap (`key` = source node id).
+ * @param from Source node id.
+ * @param to Destination node id.
+ * @param checkBothWays If true, also match `to → from`.
+ * @param type Required link type, or @ref Link::kUndef to accept any type.
+ * @return Iterator to the link, or `links.end()` if not found.
+ */
 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);
+/** @overload `std::multimap<int, std::pair<int, Link::Type>>`. */
 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);
+/** @overload `std::multimap<int, int>`. */
 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
+/** @overload const `std::multimap<int, Link>`. */
 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);
+/** @overload const `std::multimap<int, std::pair<int, Link::Type>>`. */
 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);
+/** @overload const `std::multimap<int, int>`. */
 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
+
+/**
+ * @brief Lists all links incident on node @p 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 @p from is always
+ * the source frame.
+ *
+ * @param links Graph constraints.
+ * @param from Node id to query.
+ * @return Incident links (may be empty).
+ */
 std::list<Link> RTABMAP_CORE_EXPORT findLinks(
 		const std::multimap<int, Link> & links,
 		int from);
 
+/**
+ * @brief Removes duplicate undirected links.
+ *
+ * Keeps the first occurrence of each `(from, to)` or `(to, from)` pair with the same
+ * @ref Link::Type (see @ref findLink() with @p checkBothWays).
+ *
+ * @param links Input link multimap.
+ * @return Copy without duplicates.
+ */
 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+
 /**
- * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
+ * @brief Filters links by type or self-reference.
+ *
+ * - If @p filteredType is @ref Link::kSelfRefLink: exclude self-references (`from == to`),
+ *   or include only them when @p inverted is true.
+ * - Otherwise: exclude links of @p filteredType, or keep only that type when @p inverted is true.
+ *
+ * @param links Input links.
+ * @param filteredType Type to filter, or @ref Link::kSelfRefLink for self-reference filtering.
+ * @param inverted If true, keep the filtered set instead of removing it.
+ * @return Filtered link container (same structure as input).
  */
 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
-/**
- * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
- */
+/** @overload for `std::map<int, Link>`. */
 std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::map<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 
-//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
+/**
+ * @brief Keeps poses inside (or outside) a camera frustum.
+ *
+ * Transforms each pose position into the frustum defined by @p cameraPose using
+ * @ref util3d::frustumFiltering() (this assumes the cameraPose includes the optical rotation of the camera (X right, Y down, Z forward).
+ *
+ * @param poses Input poses (null poses are skipped) in base frame (X forward, Y left, Z up), 
+ * @param cameraPose Frustum origin and orientation including the optical rotation of the camera (X right, Y down, Z forward).
+ * @param horizontalFOV Horizontal field of view (deg); see @ref CameraModel::horizontalFOV().
+ * @param verticalFOV Vertical field of view (deg); see @ref CameraModel::verticalFOV().
+ * @param nearClipPlaneDistance Near clipping distance (m).
+ * @param farClipPlaneDistance Far clipping distance (m).
+ * @param negative If false, keep poses inside the frustum; if true, keep poses outside.
+ * @return Subset of @p poses passing the filter.
+ */
 std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,
-		float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
-		float verticalFOV = 45.0f,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float horizontalFOV = 45.0f,
+		float verticalFOV = 45.0f,
 		float nearClipPlaneDistance = 0.1f,
 		float farClipPlaneDistance = 100.0f,
 		bool negative = false);
 
 /**
- * Get only the the most recent or older poses in the defined radius.
- * @param poses The poses
- * @param radius Radius (m) of the search for near neighbors
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
- * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
- * @return A map containing only most recent or older poses in the the defined radius
+ * @brief Subsamples poses that are spatially (and optionally angularly) redundant.
+ *
+ * For each pose not yet processed, finds all poses within @p radius (KD-tree). When
+ * @p angle &gt; 0, only poses whose +X axis differs by at most @p angle (rad) are grouped.
+ * From each group, keeps one pose: the latest in map order if @p keepLatest, otherwise
+ * the earliest. The first and last poses of the input map are always kept.
+ *
+ * @param poses Input trajectory (map iteration order defines “latest/oldest”).
+ * @param radius Clustering radius (m); if `≤ 0` or fewer than three poses, returns @p poses unchanged.
+ * @param angle Max heading difference within a cluster (rad); `0` ignores orientation.
+ * @param keepLatest If true, keep the latest pose per cluster; otherwise the earliest.
+ * @return Subsampled poses.
  */
 std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
@@ -214,31 +427,55 @@ std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
 		bool keepLatest = true);
 
 /**
- * Get all neighbor nodes in a fixed radius around each pose.
- * @param poses The poses
- * @param radius Radius (m) of the search for near neighbors
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
- * @return A map between each pose id and its neighbors found in the radius
+ * @brief Radius-neighbor clustering of poses.
+ *
+ * For each pose, inserts `(queryId, neighborId)` into the output for every other pose
+ * within @p radius (and within @p angle of the query heading when @p angle &gt; 0).
+ *
+ * @param poses Input poses.
+ * @param radius Search radius (m); no pairs if `≤ 0` or fewer than two poses.
+ * @param angle Max heading difference (rad); `0` ignores orientation.
+ * @return Multimap of pose id → neighbor id (both ids from @p poses).
  */
 std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle);
 
+/**
+ * @brief 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 @ref Link::kNeighbor or @ref Link::kNeighborMerged link between
+ * different clusters, builds one merged @ref Link along the shortest path through
+ * intra-cluster closure links (Dijkstra with unit cost).
+ *
+ * @param poses Input optimized poses.
+ * @param links Input constraints (should be unique per directed edge for closure links).
+ * @param hyperNodes Output `hyperNodeId → childPoseId` membership.
+ * @param hyperLinks Output links between hyper-nodes (one per hyper-edge, most recent kept).
+ */
 void reduceGraph(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
+		std::multimap<int, int> & hyperNodes,
 		std::multimap<int, Link> & hyperLinks);
 
 /**
- * Perform A* path planning in the graph.
- * @param poses The graph's poses
- * @param links The graph's links (from node id -> to node id)
- * @param from initial node
- * @param to final node
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
- * @return the path ids from id "from" to id "to" including initial and final nodes.
+ * @brief A* shortest path on a pose graph with Euclidean edge costs.
+ *
+ * Edge cost between adjacent nodes is the Euclidean distance between their poses in
+ * @p poses. Uses `costSoFar + distToEnd` where `distToEnd` is the distance to the goal pose.
+ *
+ * @param poses Node id → pose (must contain every node reached by @p links).
+ * @param links Directed edges (`from` → `to`) keyed by source id.
+ * @param from Start node id.
+ * @param to Goal node id.
+ * @param updateNewCosts If true, use a multimap queue that can decrease keys when a shorter path is found.
+ * @return Ordered path from @p from to @p to (inclusive) with poses; empty if unreachable.
  */
 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			const std::map<int, rtabmap::Transform> & poses,
@@ -248,14 +485,17 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			bool updateNewCosts = false);
 
 /**
- * Perform Dijkstra path planning in the graph.
- * @param poses The graph's poses
- * @param links The graph's links (from node id -> to node id)
- * @param from initial node
- * @param to final node
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
- * @param useSameCostForAllLinks Ignore distance between nodes
- * @return the path ids from id "from" to id "to" including initial and final nodes.
+ * @brief Dijkstra shortest path on link constraints.
+ *
+ * Explores outgoing links keyed by `link.from()`. Edge cost is `1` when
+ * @p useSameCostForAllLinks is true, otherwise the translation norm of the link transform.
+ *
+ * @param links Constraints keyed by source node id.
+ * @param from Start node id.
+ * @param to Goal node id.
+ * @param updateNewCosts If true, allow cost improvements on open nodes.
+ * @param useSameCostForAllLinks If true, unit edge cost; else use `link.transform().getNorm()`.
+ * @return Node ids from @p from to @p to (inclusive); empty if unreachable.
  */
 std::list<int> RTABMAP_CORE_EXPORT computePath(
 			const std::multimap<int, Link> & links,
@@ -265,13 +505,36 @@ std::list<int> RTABMAP_CORE_EXPORT computePath(
 			bool useSameCostForAllLinks = false);
 
 /**
- * Perform Dijkstra path planning in the graph.
- * @param fromId initial node
- * @param toId final node
- * @param memory The graph's memory
- * @param lookInDatabase check links in database
- * @param updateNewCosts Keep up-to-date costs while traversing the graph.
- * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
+ * @brief Dijkstra path through the live @ref Memory pose graph.
+ *
+ * Loads links from @ref 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 &gt; 0, costs are expressed in seconds of motion:
+ * - @p 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 @p angularVelocity so translation and rotation costs are comparable.
+ * - @p 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 @p angularVelocity &gt; 0 and @p 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.
+ *
+ * @param fromId Start signature id (`≥ 0`).
+ * @param toId Goal signature id (`≠ 0`).
+ * @param memory Graph memory (must not be null).
+ * @param lookInDatabase If true, load links from the database when not already in RAM.
+ * @param updateNewCosts If true, allow cost improvements on open nodes.
+ * @param linearVelocity If &gt; 0, add `translationNorm / linearVelocity` to edge cost (m/s).
+ * @param angularVelocity If &gt; 0, add rotation time from motion direction change (rad/s).
+ * @param ignoreDirectLinks If true, skip the direct edge between @p fromId and @p toId.
+ * @return Path as `(nodeId, pose)` pairs; first pose is identity at @p fromId. Empty if unreachable.
  */
 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		int fromId,
@@ -279,16 +542,19 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		const Memory * memory,
 		bool lookInDatabase = true,
 		bool updateNewCosts = false,
-		float linearVelocity = 0.0f,   // m/sec
-		float angularVelocity = 0.0f,  // rad/sec
-		bool ignoreDirectLinks = false); 
+		float linearVelocity = 0.0f,
+		float angularVelocity = 0.0f,
+		bool ignoreDirectLinks = false);
 
 /**
- * Find the nearest node of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param distance squared distance of the nearest node found (optional)
- * @return the node id.
+ * @brief Id of the nearest pose to @p targetPose.
+ *
+ * Wrapper around @ref findNearestNodes() with `radius=0`, `k=1` (1-NN in 3D).
+ *
+ * @param poses Nodes to search.
+ * @param targetPose Query position (x, y, z only; orientation is not used).
+ * @param distance If not null, set to the squared Euclidean distance of the match.
+ * @return Closest node id, or `0` if @p poses is empty.
  */
 int RTABMAP_CORE_EXPORT findNearestNode(
 		const std::map<int, rtabmap::Transform> & poses,
@@ -296,12 +562,18 @@ int RTABMAP_CORE_EXPORT findNearestNode(
 		float * distance = 0);
 
 /**
- * Find the nearest nodes of the query pose or node
- * @param nodeId the query id
- * @param nodes the nodes to search for
- * @param radius radius to search for (m), if 0, k should be > 0.
- * @param k max nearest neighbors (0=all inside the radius)
- * @return the nodes with squared distance to query node.
+ * @brief Spatial neighbors of a node (KD-tree radius or k-NN search).
+ *
+ * @p nodeId is removed from the search set. Requires `radius &gt; 0` or `k &gt; 0`.
+ * When `radius &gt; 0`, returns all poses within @p radius (up to @p k if `k &gt; 0`).
+ * When `radius == 0`, returns the @p k nearest neighbors.
+ *
+ * @param nodeId Query node (must exist in @p poses); excluded from results.
+ * @param poses Candidate poses.
+ * @param radius Search radius (m).
+ * @param angle Max +X axis angle difference (rad); `0` ignores heading.
+ * @param k Max neighbors (`0` = all within radius).
+ * @return Neighbor id → squared Euclidean distance.
  */
 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		int nodeId,
@@ -309,18 +581,32 @@ std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		float radius,
 		float angle = 0.0f,
 		int k=0);
+/**
+ * @brief Spatial neighbors of a pose (KD-tree radius or k-NN search).
+ * @param targetPose Query pose (position used; orientation used when @p angle &gt; 0).
+ * @param poses Candidate poses (not modified).
+ * @param radius Search radius (m).
+ * @param angle Max +X axis angle difference (rad); `0` ignores heading.
+ * @param k Max neighbors (`0` = all within radius).
+ * @return Neighbor id → squared Euclidean distance.
+ */
 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);
+/**
+ * @brief Like @ref findNearestNodes(int,const std::map<int,Transform>&,float,float,int)
+ * but returns full @ref Transform values.
+ */
 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);
+/** @overload query by @ref Transform instead of node id. */
 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		const Transform & targetPose,
 		const std::map<int, Transform> & poses,
@@ -328,28 +614,63 @@ std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		float angle = 0.0f,
 		int k=0);
 
-// Use new findNearestNodes() interface with radius=0, angle=0.
+/** @deprecated Use @ref findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int) with `radius=0`, `k` set. */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestNodes(int,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestNodes(const Transform&,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestPoses(int,const std::map<int,Transform>&,float,float,int). */
 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
-// Renamed to findNearestNodes()
+/** @deprecated Use @ref findNearestPoses(const Transform&,const std::map<int,Transform>&,float,float,int). */
 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);
 
+/**
+ * @brief Path length along an ordered list of poses.
+ *
+ * Sums `path[i].second.getDistance(path[i+1].second)` for consecutive entries.
+ *
+ * @param path Ordered `(nodeId, pose)` pairs.
+ * @return Total length (m), or `0` if fewer than two poses.
+ */
 float RTABMAP_CORE_EXPORT computePathLength(
 		const std::vector<std::pair<int, Transform> > & path);
 
-// assuming they are all linked in map order
+/**
+ * @brief 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).
+ *
+ * @param path Poses keyed by node id (sorted by key).
+ * @return Total length (m), or `0` if fewer than two poses.
+ */
 float RTABMAP_CORE_EXPORT computePathLength(
 		const std::map<int, Transform> & path);
 
+/**
+ * @brief 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 @ref Link::kNeighbor or @ref Link::kNeighborMerged link exists
+ * from the previous pose to it. Stops at the first gap, pushes the chain, and continues
+ * until @p poses is empty.
+ *
+ * @param poses Input poses (cleared as segments are extracted).
+ * @param links Graph constraints keyed by source id.
+ * @return List of pose maps, each a contiguous neighbor chain.
+ */
 std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
 
+/**
+ * @brief Axis-aligned bounding box of pose positions.
+ *
+ * @param poses Input poses (no effect if empty).
+ * @param min Output minimum (x, y, z) in meters.
+ * @param max Output maximum (x, y, z) in meters.
+ */
 void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
 		cv::Vec3f & min,
 		cv::Vec3f & max);

corelib/include/rtabmap/core/IMU.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -130,7 +130,7 @@ public:
 	 * Applies @ref localTransform() rotation to vectors and covariances, then sets
 	 * rotational part of @ref localTransform() to identity (translation unchanged).
 	 * No-op if @ref localTransform() is null or rotation is identity.
-	 * Orientation is updated only when quaternion x, y, z are not all zero.
+	 * Orientation is updated only when quaternion qx, qy, qz, qw are not all zero.
 	 */
 	void convertToBaseFrame();
 

corelib/src/Graph.cpp

@@ -666,18 +666,11 @@ int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,
 }
 
 inline float rotationError(const Transform &pose_error) {
-	float a = pose_error(0,0);
-	float b = pose_error(1,1);
-	float c = pose_error(2,2);
-	float d = 0.5*(a+b+c-1.0);
-	return std::acos(std::max(std::min(d,1.0f),-1.0f));
+	return pose_error.getAngle(Transform::getIdentity());
 }
 
 inline float translationError(const Transform &pose_error) {
-	float dx = pose_error.x();
-	float dy = pose_error.y();
-	float dz = pose_error.z();
-	return sqrt(dx*dx+dy*dy+dz*dz);
+	return pose_error.getNorm();
 }
 
 void calcKittiSequenceErrors (
@@ -688,6 +681,14 @@ void calcKittiSequenceErrors (
 
 	UASSERT(poses_gt.size() == poses_result.size());
 
+	t_err = 0.0f;
+	r_err = 0.0f;
+
+	if(poses_gt.size() < 2)
+	{
+		return;
+	}
+
 	// error vector
 	std::vector<errors> err;
 
@@ -713,24 +714,35 @@ void calcKittiSequenceErrors (
 			if (last_frame==-1)
 				continue;
 
+			const Transform & gtFirst = poses_gt[first_frame];
+			const Transform & gtLast = poses_gt[last_frame];
+			const Transform & estFirst = poses_result[first_frame];
+			const Transform & estLast = poses_result[last_frame];
+			UASSERT_MSG(gtFirst.isInvertible() && gtLast.isInvertible() &&
+					estFirst.isInvertible() && estLast.isInvertible(),
+					uFormat("Non-invertible poses at frames %d and %d (segment length %f m)",
+							first_frame, last_frame, len).c_str());
+
 			// compute rotational and translational errors
-			Transform pose_delta_gt     = poses_gt[first_frame].inverse()*poses_gt[last_frame];
-			Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
-			Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
-			float r_err = rotationError(pose_error);
-			float t_err = translationError(pose_error);
+			Transform pose_delta_gt     = gtFirst.inverse()*gtLast;
+			Transform pose_delta_result = estFirst.inverse()*estLast;
+			Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
+			const float rotErr = rotationError(pose_error);
+			const float transErr = translationError(pose_error);
 
 			// compute speed
 			float num_frames = (float)(last_frame-first_frame+1);
-			float speed = len/(0.1*num_frames);
+			float speed = len/(0.1f*num_frames);
 
 			// write to file
-			err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
+			err.push_back(errors(first_frame, rotErr/len, transErr/len, len, speed));
 		}
 	}
 
-	t_err = 0;
-	r_err = 0;
+	if(err.empty())
+	{
+		return;
+	}
 
 	// for all errors do => compute sum of t_err, r_err
 	for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
@@ -740,11 +752,11 @@ void calcKittiSequenceErrors (
 	}
 
 	// save errors
-	float num = err.size();
+	const float num = float(err.size());
 	t_err /= num;
 	r_err /= num;
 	t_err *= 100.0f;    // Translation error (%)
-	r_err *= 180/CV_PI; // Rotation error (deg/m)
+	r_err *= 180.0f/CV_PI; // Rotation error (deg/m)
 }
 // KITTI evaluation end
 
@@ -1872,6 +1884,7 @@ std::list<std::pair<int, Transform> > computePath(
 						if(mapIter->second == nodeIter->first)
 						{
 							pqmap.erase(mapIter);
+							nodeIter->second.setFromId(currentNode->id());
 							nodeIter->second.setCostSoFar(newCostSoFar);
 							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 							break;
@@ -1962,7 +1975,7 @@ std::list<int> computePath(
 					pq.push(Pair(n.id(), n.totalCost()));
 				}
 			}
-			else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
+			else if(updateNewCosts && nodeIter->second.isOpened())
 			{
 				float newCostSoFar = currentNode->costSoFar() + cost;
 				if(nodeIter->second.costSoFar() > newCostSoFar)
@@ -1973,6 +1986,7 @@ std::list<int> computePath(
 						if(mapIter->second == nodeIter->first)
 						{
 							pqmap.erase(mapIter);
+							nodeIter->second.setFromId(currentNode->id());
 							nodeIter->second.setCostSoFar(newCostSoFar);
 							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 							break;
@@ -2142,6 +2156,7 @@ std::list<std::pair<int, Transform> > computePath(
 							if(mapIter->second == nodeIter->first)
 							{
 								pqmap.erase(mapIter);
+								nodeIter->second.setFromId(currentNode->id());
 								nodeIter->second.setCostSoFar(newCostSoFar);
 								pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
 								break;
@@ -2420,8 +2435,18 @@ std::list<std::map<int, Transform> > getPaths(
 			std::map<int, Transform> path;
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
 			{
-				std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
-				if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
+				bool addPose = false;
+				if(path.empty())
+				{
+					addPose = true;
+				}
+				else
+				{
+					std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
+					addPose = jter != links.end() &&
+							(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged);
+				}
+				if(addPose)
 				{
 					path.insert(*iter);
 					poses.erase(iter++);

corelib/test/CMakeLists.txt

@@ -111,6 +111,11 @@ add_executable(test_imu test_imu.cpp)
 target_link_libraries(test_imu gtest_main rtabmap_core)
 add_test(NAME test_imu COMMAND test_imu)
 
+#Graph.h
+add_executable(test_graph test_graph.cpp)
+target_link_libraries(test_graph gtest_main rtabmap_core)
+add_test(NAME test_graph COMMAND test_graph)
+
 #GeodeticCoords.h
 add_executable(test_geodeticcoords test_geodeticcoords.cpp)
 target_link_libraries(test_geodeticcoords gtest_main rtabmap_core)

corelib/test/test_graph.cpp

@@ -0,0 +1,860 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Link.h>
+#include <cmath>
+#include <string>
+#include <vector>
+
+using namespace rtabmap;
+
+namespace {
+
+static cv::Mat infMatrixDiagonal(double x, double y, double z, double roll, double pitch, double yaw)
+{
+	cv::Mat inf = cv::Mat::zeros(6, 6, CV_64FC1);
+	inf.at<double>(0, 0) = x;
+	inf.at<double>(1, 1) = y;
+	inf.at<double>(2, 2) = z;
+	inf.at<double>(3, 3) = roll;
+	inf.at<double>(4, 4) = pitch;
+	inf.at<double>(5, 5) = yaw;
+	return inf;
+}
+
+static Link neighborLink(int from, int to, float dx = 1.0f)
+{
+	return Link(from, to, Link::kNeighbor, Transform(dx, 0, 0, 0, 0, 0));
+}
+
+static void insertLink(std::multimap<int, Link> & links, const Link & link)
+{
+	links.insert(std::make_pair(link.from(), link));
+}
+
+static std::map<int, Transform> linePoses(unsigned int count, float step = 1.0f)
+{
+	std::map<int, Transform> poses;
+	for(unsigned int i = 0; i < count; ++i)
+	{
+		poses.insert(std::make_pair(static_cast<int>(i + 1), Transform(step * i, 0, 0, 0, 0, 0)));
+	}
+	return poses;
+}
+
+// KITTI metrics use 100–800 m segments; need ~800 m of trajectory at 1 m/frame.
+static std::vector<Transform> lineTrajectory(unsigned int count, float step = 1.0f)
+{
+	std::vector<Transform> traj;
+	traj.reserve(count);
+	for(unsigned int i = 0; i < count; ++i)
+	{
+		traj.push_back(Transform(step * i, 0, 0, 0, 0, 0));
+	}
+	return traj;
+}
+
+static std::map<int, Transform> transformPoses(
+		const std::map<int, Transform> & poses,
+		const Transform & t)
+{
+	std::map<int, Transform> out;
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter != poses.end(); ++iter)
+	{
+		out.insert(std::make_pair(iter->first, t * iter->second));
+	}
+	return out;
+}
+
+static float calcTranslationalRmse(
+		const std::map<int, Transform> & groundTruth,
+		const std::map<int, Transform> & poses,
+		bool align2D = true)
+{
+	float tRmse = 0.0f;
+	float tMean = 0.0f;
+	float tMedian = 0.0f;
+	float tStd = 0.0f;
+	float tMin = 0.0f;
+	float tMax = 0.0f;
+	float rRmse = 0.0f;
+	float rMean = 0.0f;
+	float rMedian = 0.0f;
+	float rStd = 0.0f;
+	float rMin = 0.0f;
+	float rMax = 0.0f;
+	graph::calcRMSE(
+			groundTruth,
+			poses,
+			tRmse,
+			tMean,
+			tMedian,
+			tStd,
+			tMin,
+			tMax,
+			rRmse,
+			rMean,
+			rMedian,
+			rStd,
+			rMin,
+			rMax,
+			align2D);
+	return tRmse;
+}
+
+} // namespace
+
+TEST(GraphTest, FindLinkForwardAndReverse)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+
+	EXPECT_NE(graph::findLink(links, 1, 2), links.end());
+	EXPECT_EQ(graph::findLink(links, 1, 2)->second.to(), 2);
+
+	EXPECT_EQ(graph::findLink(links, 2, 1, false), links.end());
+	EXPECT_NE(graph::findLink(links, 2, 1, true), links.end());
+
+	EXPECT_EQ(graph::findLink(links, 1, 2, true, Link::kGlobalClosure), links.end());
+	EXPECT_NE(graph::findLink(links, 1, 2, true, Link::kNeighbor), links.end());
+}
+
+TEST(GraphTest, FindLinkIntMultimap)
+{
+	std::multimap<int, int> links;
+	links.insert(std::make_pair(1, 2));
+	links.insert(std::make_pair(2, 3));
+
+	EXPECT_NE(graph::findLink(links, 1, 2), links.end());
+	EXPECT_NE(graph::findLink(links, 3, 2, true), links.end());
+	EXPECT_EQ(graph::findLink(links, 1, 3), links.end());
+}
+
+TEST(GraphTest, FindLinksIncludesIncomingAsInverse)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+
+	const std::list<Link> from1 = graph::findLinks(links, 1);
+	ASSERT_EQ(from1.size(), 1u);
+	EXPECT_EQ(from1.front().from(), 1);
+	EXPECT_EQ(from1.front().to(), 2);
+
+	const std::list<Link> from2 = graph::findLinks(links, 2);
+	ASSERT_EQ(from2.size(), 1u);
+	EXPECT_EQ(from2.front().from(), 2);
+	EXPECT_EQ(from2.front().to(), 1);
+}
+
+TEST(GraphTest, FilterDuplicateLinks)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, neighborLink(2, 1));
+
+	const std::multimap<int, Link> filtered = graph::filterDuplicateLinks(links);
+	EXPECT_EQ(filtered.size(), 1u);
+}
+
+TEST(GraphTest, FilterLinksByType)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, Link(2, 3, Link::kGlobalClosure, Transform::getIdentity()));
+
+	const std::multimap<int, Link> noClosure = graph::filterLinks(links, Link::kGlobalClosure, false);
+	EXPECT_EQ(noClosure.size(), 1u);
+	EXPECT_EQ(noClosure.begin()->second.type(), Link::kNeighbor);
+
+	const std::multimap<int, Link> onlyClosure = graph::filterLinks(links, Link::kGlobalClosure, true);
+	ASSERT_EQ(onlyClosure.size(), 1u);
+	EXPECT_EQ(onlyClosure.begin()->second.type(), Link::kGlobalClosure);
+}
+
+TEST(GraphTest, FilterSelfReferenceLinks)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, Link(3, 3, Link::kPosePrior, Transform::getIdentity()));
+
+	const std::multimap<int, Link> nonSelf = graph::filterLinks(links, Link::kSelfRefLink, false);
+	EXPECT_EQ(nonSelf.size(), 1u);
+	EXPECT_NE(nonSelf.begin()->second.from(), nonSelf.begin()->second.to());
+
+	const std::multimap<int, Link> selfOnly = graph::filterLinks(links, Link::kSelfRefLink, true);
+	ASSERT_EQ(selfOnly.size(), 1u);
+	EXPECT_EQ(selfOnly.begin()->second.from(), selfOnly.begin()->second.to());
+}
+
+static std::list<int> computeDijkstraPath(bool updateNewCosts, bool useSameCostForAllLinks)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, neighborLink(2, 3));
+	insertLink(links, neighborLink(1, 3, 5.0f));
+	return graph::computePath(links, 1, 3, updateNewCosts, useSameCostForAllLinks);
+}
+
+static std::string dijkstraPathToString(const std::list<int> & path)
+{
+	std::string s;
+	for(std::list<int>::const_iterator it = path.begin(); it != path.end(); ++it)
+	{
+		if(!s.empty())
+		{
+			s += "->";
+		}
+		s += std::to_string(*it);
+	}
+	return s;
+}
+
+static void expectDijkstraPath(
+		const std::list<int> & path,
+		const std::initializer_list<int> expected)
+{
+	ASSERT_EQ(path.size(), expected.size());
+	auto it = path.begin();
+	for(int id : expected)
+	{
+		ASSERT_NE(it, path.end());
+		EXPECT_EQ(*it, id);
+		++it;
+	}
+}
+
+TEST(GraphTest, ComputePathDijkstraWeighted)
+{
+	// Same graph as computeDijkstraPath(); edge cost = link length (m).
+	//
+	//   1 -------- 5 m -------- 3   cost 5  ->  {1, 3} when updateNewCosts=false
+	//   |                       ^
+	//   +-- 1 m -- 2 -- 1 m ----+   cost 2  ->  {1, 2, 3} when updateNewCosts=true
+	//
+	expectDijkstraPath(computeDijkstraPath(false, false), {1, 3}); // updateNewCosts=false: 3 stays on direct link
+	expectDijkstraPath(computeDijkstraPath(true, false), {1, 2, 3});
+}
+
+TEST(GraphTest, ComputePathDijkstraUnitCostUpdateNewCostsEquivalent)
+{
+	// Same topology; useSameCostForAllLinks=true (1 hop per edge).
+	//
+	//   1 --------------------- 3   1 hop  ->  {1, 3} (both updateNewCosts values)
+	//   |
+	//   +-- 1 hop -- 2 -- 1 hop -- 3   2 hops (never chosen)
+	//
+	// Relaxation only applies on a strictly lower hop count, so updateNewCosts cannot
+	// change the result.
+	expectDijkstraPath(computeDijkstraPath(false, true), {1, 3});
+	expectDijkstraPath(computeDijkstraPath(true, true), {1, 3});
+	EXPECT_EQ(
+			dijkstraPathToString(computeDijkstraPath(false, true)),
+			dijkstraPathToString(computeDijkstraPath(true, true)));
+}
+
+TEST(GraphTest, ComputePathAStar)
+{
+	std::map<int, Transform> poses = linePoses(3);
+	std::multimap<int, int> links;
+	links.insert(std::make_pair(1, 2));
+	links.insert(std::make_pair(2, 3));
+
+	const std::list<std::pair<int, Transform> > path = graph::computePath(poses, links, 1, 3, false);
+	ASSERT_EQ(path.size(), 3u);
+	EXPECT_EQ(path.front().first, 1);
+	EXPECT_EQ(path.back().first, 3);
+}
+
+static std::string aStarPathToString(const std::list<std::pair<int, Transform> > & path)
+{
+	std::string s;
+	for(std::list<std::pair<int, Transform> >::const_iterator it = path.begin(); it != path.end(); ++it)
+	{
+		if(it != path.begin())
+		{
+			s += "->";
+		}
+		s += std::to_string(it->first);
+	}
+	return s;
+}
+
+static void expectAStarPath(
+		const std::list<std::pair<int, Transform> > & path,
+		const std::initializer_list<int> expectedIds)
+{
+	ASSERT_EQ(path.size(), expectedIds.size()) << "path=" << aStarPathToString(path);
+	auto it = path.begin();
+	for(int id : expectedIds)
+	{
+		ASSERT_NE(it, path.end());
+		EXPECT_EQ(it->first, id);
+		++it;
+	}
+}
+
+static std::list<std::pair<int, Transform> > computeAStarPath(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, int> & links,
+		bool updateNewCosts)
+{
+	return graph::computePath(poses, links, 1, 3, updateNewCosts);
+}
+
+TEST(GraphTest, ComputePathAStarUpdateNewCostsChangesPath)
+{
+	// Detour 1→2→5→6→3 vs shortcut 1→2→4→6→3. No 5→3 edge so h(5,3) < cost(5→6→3).
+	// Node 5 is expanded before 4 (lower f-score). Node 6 is first reached from 5;
+	// expanding 4 relaxes the parent of 6 when updateNewCosts=true.
+	//
+	//        3  goal (10, 0)
+	//        |
+	//        6  (2, -5)
+	//       / \
+	//      5   4   (1,-2)  (1.5,-4)
+	//       \ /
+	//        2  (1, 0)
+	//        |
+	//        1  start (0, 0)
+	//
+	// Links: 1—2, 2—5, 2—4, 5—6, 4—6, 6—3  (no 5—3)
+	const std::map<int, Transform> poses = {
+		{1, Transform(0, 0, 0, 0, 0, 0)},
+		{2, Transform(1, 0, 0, 0, 0, 0)},
+		{3, Transform(10, 0, 0, 0, 0, 0)},
+		{4, Transform(1.5f, -4, 0, 0, 0, 0)},
+		{5, Transform(1, -2, 0, 0, 0, 0)},
+		{6, Transform(2, -5, 0, 0, 0, 0)}};
+	std::multimap<int, int> links;
+	links.insert(std::make_pair(1, 2));
+	links.insert(std::make_pair(2, 5)); // before 2→4
+	links.insert(std::make_pair(2, 4));
+	links.insert(std::make_pair(5, 6));
+	links.insert(std::make_pair(4, 6));
+	links.insert(std::make_pair(6, 3));
+
+	const std::list<std::pair<int, Transform> > pathNoUpdate =
+			computeAStarPath(poses, links, false);
+	const std::list<std::pair<int, Transform> > pathUpdate =
+			computeAStarPath(poses, links, true);
+
+	expectAStarPath(pathNoUpdate, {1, 2, 5, 6, 3});
+	expectAStarPath(pathUpdate, {1, 2, 4, 6, 3});
+	EXPECT_NE(aStarPathToString(pathNoUpdate), aStarPathToString(pathUpdate));
+}
+
+TEST(GraphTest, FindNearestNode)
+{
+	const std::map<int, Transform> poses = linePoses(3, 2.0f);
+	const Transform query(2.1f, 0.1f, 0, 0, 0, 0);
+
+	float sqDist = -1.0f;
+	const int id = graph::findNearestNode(poses, query, &sqDist);
+	EXPECT_EQ(id, 2);
+	EXPECT_NEAR(sqDist, 0.1f * 0.1f + 0.1f * 0.1f, 1e-4f);
+}
+
+TEST(GraphTest, FindNearestNodesKnn)
+{
+	const std::map<int, Transform> poses = linePoses(4);
+	const std::map<int, float> nearest = graph::findNearestNodes(poses.at(2), poses, 0.0f, 0.0f, 2);
+	ASSERT_EQ(nearest.size(), 2u);
+	ASSERT_TRUE(nearest.find(2) != nearest.end());
+	EXPECT_NEAR(nearest.at(2), 0.0f, 1e-6f);
+
+	// nodeId overload excludes the query node from results
+	const std::map<int, float> excludingSelf = graph::findNearestNodes(2, poses, 0.0f, 0.0f, 2);
+	ASSERT_EQ(excludingSelf.size(), 2u);
+	EXPECT_TRUE(excludingSelf.find(2) == excludingSelf.end());
+	EXPECT_NEAR(excludingSelf.at(1), 1.0f, 1e-6f);
+	EXPECT_NEAR(excludingSelf.at(3), 1.0f, 1e-6f);
+}
+
+TEST(GraphTest, FindNearestNodesRadius)
+{
+	const std::map<int, Transform> poses = linePoses(4);
+	const std::map<int, float> inRadius = graph::findNearestNodes(poses.at(2), poses, 1.5f);
+	EXPECT_EQ(inRadius.size(), 3u);
+	EXPECT_TRUE(inRadius.find(2) != inRadius.end());
+	EXPECT_NEAR(inRadius.at(2), 0.0f, 1e-6f);
+	EXPECT_TRUE(inRadius.find(4) == inRadius.end());
+}
+
+TEST(GraphTest, ComputePathLength)
+{
+	const std::vector<std::pair<int, Transform> > vecPath = {
+		{1, Transform(0, 0, 0, 0, 0, 0)},
+		{2, Transform(3, 4, 0, 0, 0, 0)},
+		{3, Transform(3, 9, 0, 0, 0, 0)}};
+	EXPECT_NEAR(graph::computePathLength(vecPath), 10.0f, 1e-4f);
+
+	const std::map<int, Transform> mapPath = linePoses(3);
+	EXPECT_NEAR(graph::computePathLength(mapPath), 2.0f, 1e-4f);
+}
+
+TEST(GraphTest, ComputeMinMax)
+{
+	const std::map<int, Transform> poses = {
+		{1, Transform(-1, 2, 3, 0, 0, 0)},
+		{2, Transform(4, -5, 0, 0, 0, 0)}};
+
+	cv::Vec3f min, max;
+	graph::computeMinMax(poses, min, max);
+	EXPECT_FLOAT_EQ(min[0], -1.0f);
+	EXPECT_FLOAT_EQ(min[1], -5.0f);
+	EXPECT_FLOAT_EQ(min[2], 0.0f);
+	EXPECT_FLOAT_EQ(max[0], 4.0f);
+	EXPECT_FLOAT_EQ(max[1], 2.0f);
+	EXPECT_FLOAT_EQ(max[2], 3.0f);
+}
+
+TEST(GraphTest, CalcRelativeErrorsIdenticalTrajectories)
+{
+	const std::vector<Transform> traj = {
+		Transform(0, 0, 0, 0, 0, 0),
+		Transform(1, 0, 0, 0, 0, 0),
+		Transform(2, 0, 0, 0, 0, 0)};
+
+	float tErr = -1.0f;
+	float rErr = -1.0f;
+	graph::calcRelativeErrors(traj, traj, tErr, rErr);
+	EXPECT_NEAR(tErr, 0.0f, 1e-5f);
+	EXPECT_NEAR(rErr, 0.0f, 1e-5f);
+}
+
+TEST(GraphTest, CalcRelativeErrorsWithNoise)
+{
+	const std::vector<Transform> gt = {
+		Transform(0, 0, 0, 0, 0, 0),
+		Transform(1, 0, 0, 0, 0, 0),
+		Transform(2, 0, 0, 0, 0, 0),
+		Transform(3, 0, 0, 0, 0, 0),
+		Transform(4, 0, 0, 0, 0, 0)};
+
+	// Small position and orientation noise on the estimate.
+	const std::vector<Transform> est = {
+		Transform(0.01f, -0.02f, 0.005f, 0, 0, 0.01f),
+		Transform(1.03f, 0.01f, -0.01f, 0, 0, -0.02f),
+		Transform(2.02f, -0.03f, 0.02f, 0, 0, 0.015f),
+		Transform(2.98f, 0.02f, 0.01f, 0, 0, -0.01f),
+		Transform(4.01f, -0.01f, -0.02f, 0, 0, 0.005f)};
+
+	float tErr = 0.0f;
+	float rErr = 0.0f;
+	graph::calcRelativeErrors(gt, est, tErr, rErr);
+	EXPECT_GT(tErr, 0.0f);
+	EXPECT_LT(tErr, 0.1f);
+	EXPECT_GT(rErr, 0.0f);
+	EXPECT_LT(rErr, 2.0f);
+}
+
+TEST(GraphTest, CalcKittiSequenceErrorsIdenticalTrajectories)
+{
+	const std::vector<Transform> traj = lineTrajectory(901, 1.0f);
+
+	float tErr = -1.0f;
+	float rErr = -1.0f;
+	graph::calcKittiSequenceErrors(traj, traj, tErr, rErr);
+	EXPECT_NEAR(tErr, 0.0f, 1e-5f);
+	EXPECT_NEAR(rErr, 0.0f, 1e-5f);
+}
+
+TEST(GraphTest, CalcKittiSequenceErrorsWithNoise)
+{
+	const std::vector<Transform> gt = lineTrajectory(901, 1.0f);
+	ASSERT_EQ(gt.size(), 901u);
+	ASSERT_NEAR(gt[0].x(), 0.0f, 1e-5f);
+
+	std::vector<Transform> est;
+	est.reserve(gt.size());
+	for(unsigned int i = 0; i < gt.size(); ++i)
+	{
+		const int ii = static_cast<int>(i);
+		const float dx = 0.02f * static_cast<float>((ii % 3) - 1);
+		const float dy = 0.01f * static_cast<float>((ii % 5) - 2);
+		est.push_back(Transform(
+				gt.at(i).x() + dx,
+				gt.at(i).y() + dy,
+				gt.at(i).z(),
+				0.0f,
+				0.0f,
+				0.0f));
+	}
+	ASSERT_EQ(est.size(), 901u);
+	ASSERT_NEAR(est.at(0).x(), -0.02f, 1e-3f);
+	ASSERT_NEAR(est.at(800).x(), 800.02f, 1e-1f);
+
+	const Transform poseDeltaEst = est.at(0).inverse() * est.at(800);
+	ASSERT_NEAR(poseDeltaEst.getNorm(), 800.0f, 5.0f);
+
+	float tErr = 0.0f;
+	float rErr = 0.0f;
+	graph::calcKittiSequenceErrors(gt, est, tErr, rErr);
+	EXPECT_TRUE(std::isfinite(tErr)) << "tErr=" << tErr;
+	EXPECT_TRUE(std::isfinite(rErr)) << "rErr=" << rErr;
+	EXPECT_GT(tErr, 0.0f);
+	EXPECT_LT(tErr, 2.0f);   // translation error (%)
+	EXPECT_NEAR(rErr, 0.0f, 0.5f); // no orientation noise on the trajectory
+}
+
+TEST(GraphTest, CalcRMSEIdenticalMaps)
+{
+	const std::map<int, Transform> gt = linePoses(3);
+	float tRmse = -1.0f;
+	float tMean = -1.0f;
+	float tMedian = -1.0f;
+	float tStd = -1.0f;
+	float tMin = -1.0f;
+	float tMax = -1.0f;
+	float rRmse = -1.0f;
+	float rMean = -1.0f;
+	float rMedian = -1.0f;
+	float rStd = -1.0f;
+	float rMin = -1.0f;
+	float rMax = -1.0f;
+
+	const Transform align = graph::calcRMSE(
+			gt, gt,
+			tRmse, tMean, tMedian, tStd, tMin, tMax,
+			rRmse, rMean, rMedian, rStd, rMin, rMax,
+			true);
+	EXPECT_TRUE(align.isIdentity());
+	EXPECT_NEAR(tRmse, 0.0f, 1e-4f);
+	EXPECT_NEAR(rRmse, 0.0f, 1e-4f);
+}
+
+TEST(GraphTest, CalcRMSEWithNoise)
+{
+	const std::map<int, Transform> gt = linePoses(8, 1.0f);
+	std::map<int, Transform> est = gt;
+	est[2] = Transform(1.05f, 0.02f, 0, 0, 0, 0.01f);
+	est[4] = Transform(3.02f, -0.03f, 0.01f, 0, 0, -0.02f);
+	est[6] = Transform(5.01f, 0.01f, -0.02f, 0, 0, 0.015f);
+	est[8] = Transform(7.0f, -0.01f, 0.02f, 0, 0, -0.005f);
+
+	const float tRmse = calcTranslationalRmse(gt, est, true);
+	EXPECT_GT(tRmse, 0.0f);
+	EXPECT_LT(tRmse, 0.1f);
+}
+
+TEST(GraphTest, CalcRMSEAlignsRotatedTrajectory)
+{
+	// Eight poses so calcRMSE uses SVD alignment (more than five matched poses).
+	const std::map<int, Transform> gt = linePoses(8, 1.0f);
+
+	std::map<int, Transform> noisy = gt;
+	noisy[2] = Transform(1.05f, 0.02f, 0, 0, 0, 0.01f);
+	noisy[5] = Transform(4.02f, -0.02f, 0, 0, 0, -0.01f);
+	noisy[7] = Transform(6.01f, 0.01f, 0, 0, 0, 0.02f);
+
+	const Transform yaw90(0, 0, 0, 0, 0, static_cast<float>(CV_PI / 2.0));
+	const std::map<int, Transform> rotated = transformPoses(gt, yaw90);
+
+	// Without alignment, positions would differ a lot (x vs y).
+	const Transform p = gt.at(4);
+	const Transform r = rotated.at(4);
+	EXPECT_GT(p.getDistance(r), 1.0f);
+
+	const float rmseNoisy = calcTranslationalRmse(gt, noisy, true);
+
+	float tRmse = 0.0f;
+	float tMean = 0.0f;
+	float tMedian = 0.0f;
+	float tStd = 0.0f;
+	float tMin = 0.0f;
+	float tMax = 0.0f;
+	float rRmse = 0.0f;
+	float rMean = 0.0f;
+	float rMedian = 0.0f;
+	float rStd = 0.0f;
+	float rMin = 0.0f;
+	float rMax = 0.0f;
+	const Transform align = graph::calcRMSE(
+			gt,
+			rotated,
+			tRmse,
+			tMean,
+			tMedian,
+			tStd,
+			tMin,
+			tMax,
+			rRmse,
+			rMean,
+			rMedian,
+			rStd,
+			rMin,
+			rMax,
+			true);
+
+	EXPECT_LT(rmseNoisy, 0.1f);
+	EXPECT_LT(tRmse, 0.1f);
+	EXPECT_NEAR(tRmse, rmseNoisy, 0.08f);
+
+	// est = yaw90 * gt  =>  align * est ≈ gt  =>  align ≈ yaw90⁻¹
+	const Transform expectedAlign = yaw90.inverse();
+	EXPECT_NEAR(align.getAngle(expectedAlign), 0.0f, 0.05f);
+	EXPECT_NEAR(align.x(), expectedAlign.x(), 1e-2f);
+	EXPECT_NEAR(align.y(), expectedAlign.y(), 1e-2f);
+	EXPECT_NEAR(align.theta(), expectedAlign.theta(), 1e-2f);
+	EXPECT_LT((align * rotated.at(4)).getDistance(gt.at(4)), 1e-2f);
+}
+
+static graph::MaxGraphErrors maxGraphErrors(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		bool for3DoF = false)
+{
+	return graph::computeMaxGraphErrors(poses, links, for3DoF);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsZeroResidual)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_NEAR(errors.linear, 0.0f, 1e-4f);
+	EXPECT_NEAR(errors.angular, 0.0f, 1e-4f);
+	EXPECT_NEAR(errors.linearRatio, 0.0f, 1e-4f);
+	EXPECT_NEAR(errors.angularRatio, 0.0f, 1e-4f);
+	EXPECT_TRUE(errors.linearLink.isValid());
+	EXPECT_EQ(errors.linearLink.from(), 1);
+	EXPECT_EQ(errors.linearLink.to(), 2);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsKnownLinearResidual)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(2, 0, 0, 0, 0, 0))); // 2 m apart
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f)); // link says 1 m
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_NEAR(errors.linear, 1.0f, 1e-4f);
+	EXPECT_NEAR(errors.linearRatio, 1.0f, 1e-4f); // default inf: variance 1, stddev 1
+	EXPECT_TRUE(errors.linearLink.isValid());
+	EXPECT_EQ(errors.linearLink.from(), 1);
+	EXPECT_EQ(errors.linearLink.to(), 2);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsPicksHighestLinearRatio)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(0.5f, 0, 0, 0, 0, 0))); // 0.5 m error vs link
+	poses.insert(std::make_pair(3, Transform(2.5f, 0, 0, 0, 0, 0))); // 1 m error vs link
+
+	std::multimap<int, Link> links;
+	insertLink(links, Link(
+			1,
+			2,
+			Link::kNeighbor,
+			Transform(0, 0, 0, 0, 0, 0),
+			infMatrixDiagonal(100, 100, 100, 1, 1, 1))); // ratio ≈ 0.5 / 0.1 = 5
+	insertLink(links, neighborLink(2, 3, 1.0f)); // ratio ≈ 1 / 1 = 1
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_NEAR(errors.linear, 0.5f, 1e-4f);
+	EXPECT_NEAR(errors.linearRatio, 5.0f, 1e-3f);
+	EXPECT_EQ(errors.linearLink.from(), 1);
+	EXPECT_EQ(errors.linearLink.to(), 2);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsAngularResidual)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0.5f)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_NEAR(errors.linear, 0.0f, 1e-4f);
+	EXPECT_NEAR(errors.angular, 0.5f, 1e-4f);
+	EXPECT_NEAR(errors.angularRatio, 0.5f, 1e-4f);
+	EXPECT_EQ(errors.angularLink.from(), 1);
+	EXPECT_EQ(errors.angularLink.to(), 2);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsDifferentWorstLinearAndAngularLinks)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(0.6f, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(3, Transform(1.6f, 0, 0, 0, 0, 0.5f)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, Link(
+			1,
+			2,
+			Link::kNeighbor,
+			Transform(1, 0, 0, 0, 0, 0),
+			infMatrixDiagonal(100, 100, 100, 1, 1, 1)));
+	insertLink(links, Link(
+			2,
+			3,
+			Link::kNeighbor,
+			Transform(1, 0, 0, 0, 0, 0),
+			infMatrixDiagonal(1, 1, 1, 100, 100, 100)));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_EQ(errors.linearLink.from(), 1);
+	EXPECT_EQ(errors.linearLink.to(), 2);
+	EXPECT_EQ(errors.angularLink.from(), 2);
+	EXPECT_EQ(errors.angularLink.to(), 3);
+	EXPECT_NE(errors.linearLink.from(), errors.angularLink.from());
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsFor3DoF)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(1, 0, 0.3f, 0, 0, 0)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f));
+
+	const graph::MaxGraphErrors errors6 = maxGraphErrors(poses, links, false);
+	const graph::MaxGraphErrors errors3 = maxGraphErrors(poses, links, true);
+	EXPECT_NEAR(errors6.linear, 0.3f, 1e-4f);
+	EXPECT_NEAR(errors3.linear, 0.0f, 1e-4f);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsSkipsSelfLinks)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, Link(1, 1, Link::kPosePrior, Transform(1, 2, 3, 0, 0, 0)));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_FLOAT_EQ(errors.linear, -1.0f);
+	EXPECT_FLOAT_EQ(errors.angular, -1.0f);
+	EXPECT_FALSE(errors.linearLink.isValid());
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsAbortsOnMissingPose)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_FLOAT_EQ(errors.linear, -1.0f);
+	EXPECT_FLOAT_EQ(errors.angular, -1.0f);
+	EXPECT_FALSE(errors.linearLink.isValid());
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsLandmarkSkipsUnconstrainedYaw)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0.1f))); // 0.1 rad yaw vs neighbor link
+	poses.insert(std::make_pair(-10, Transform(2, 1, 0, 0, 0, 0.5f))); // 1 m y and 0.5 rad yaw vs landmark link
+
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2, 1.0f));
+	// Landmark yaw is not constrained; angular error is skipped even though poses disagree by 0.5 rad.
+	insertLink(links, Link(
+			1,
+			-10,
+			Link::kLandmark,
+			Transform(2, 0, 0, 0, 0, 0),
+			infMatrixDiagonal(1, 1, 1, 1, 1, 0.00001)));
+
+	const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
+	EXPECT_NEAR(errors.linear, 1.0f, 1e-4f);
+	EXPECT_EQ(errors.linearLink.from(), 1);
+	EXPECT_EQ(errors.linearLink.to(), -10);
+	EXPECT_NEAR(errors.angular, 0.1f, 1e-4f);
+	EXPECT_EQ(errors.angularLink.from(), 1);
+	EXPECT_EQ(errors.angularLink.to(), 2);
+	EXPECT_NE(errors.angularLink.type(), Link::kLandmark);
+}
+
+TEST(GraphTest, ComputeMaxGraphErrorsLandmarkTwoPoseObservations)
+{
+	// Same landmark -10 observed from poses 1 and 2 (two links sharing the landmark id).
+	//
+	//     -10  (1, 1)
+	//    /   \
+	//   1     2
+	// (0,0) (2,0)
+	//
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, Transform(2, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(-10, Transform(1, 1, 0, 0, 0, 0)));
+
+	std::multimap<int, Link> links;
+	insertLink(links, Link(1, -10, Link::kLandmark, Transform(1, 1, 0, 0, 0, 0)));
+	insertLink(links, Link(2, -10, Link::kLandmark, Transform(-1, 1, 0, 0, 0, 0)));
+
+	const graph::MaxGraphErrors consistent = maxGraphErrors(poses, links);
+	EXPECT_NEAR(consistent.linear, 0.0f, 1e-4f);
+	EXPECT_NEAR(consistent.angular, 0.0f, 1e-4f);
+
+	// Poses still agree with observation from 1; link 2→-10 is wrong by 1 m in y.
+	std::multimap<int, Link> linksOneInconsistent;
+	insertLink(linksOneInconsistent, Link(1, -10, Link::kLandmark, Transform(1, 1, 0, 0, 0, 0)));
+	insertLink(linksOneInconsistent, Link(2, -10, Link::kLandmark, Transform(-1, 0, 0, 0, 0, 0)));
+	const graph::MaxGraphErrors oneInconsistent = maxGraphErrors(poses, linksOneInconsistent);
+	EXPECT_NEAR(oneInconsistent.linear, 1.0f, 1e-4f);
+	EXPECT_EQ(oneInconsistent.linearLink.from(), 2);
+	EXPECT_EQ(oneInconsistent.linearLink.to(), -10);
+
+	// Same mismatch with landmark as link.from (from < 0): measurement is inverted.
+	std::multimap<int, Link> linksOneInconsistentLandmarkFrom;
+	insertLink(linksOneInconsistentLandmarkFrom, Link(-10, 1, Link::kLandmark, Transform(-1, -1, 0, 0, 0, 0)));
+	insertLink(linksOneInconsistentLandmarkFrom, Link(-10, 2, Link::kLandmark, Transform(1, 0, 0, 0, 0, 0)));
+	const graph::MaxGraphErrors oneInconsistentLandmarkFrom =
+			maxGraphErrors(poses, linksOneInconsistentLandmarkFrom);
+	EXPECT_NEAR(oneInconsistentLandmarkFrom.linear, oneInconsistent.linear, 1e-4f);
+	EXPECT_NEAR(oneInconsistentLandmarkFrom.linearRatio, oneInconsistent.linearRatio, 1e-4f);
+	EXPECT_EQ(oneInconsistentLandmarkFrom.linearLink.from(), -10);
+	EXPECT_EQ(oneInconsistentLandmarkFrom.linearLink.to(), 2);
+
+	// Move the optimized landmark; both observations are now inconsistent by 1 m in y.
+	poses[-10] = Transform(1, 0, 0, 0, 0, 0);
+	const graph::MaxGraphErrors inconsistent = maxGraphErrors(poses, links);
+	EXPECT_NEAR(inconsistent.linear, 1.0f, 1e-4f);
+	EXPECT_TRUE(inconsistent.linearLink.isValid());
+	EXPECT_EQ(inconsistent.linearLink.to(), -10);
+	EXPECT_TRUE(inconsistent.linearLink.from() == 1 || inconsistent.linearLink.from() == 2);
+}
+
+TEST(GraphTest, GetMaxOdomInf)
+{
+	std::multimap<int, Link> links;
+	insertLink(links, Link(1, 2, Link::kNeighbor, Transform::getIdentity(), infMatrixDiagonal(1, 2, 3, 4, 5, 6)));
+	insertLink(links, Link(2, 3, Link::kNeighbor, Transform::getIdentity(), infMatrixDiagonal(6, 5, 4, 3, 2, 1)));
+	insertLink(links, Link(3, 4, Link::kGlobalClosure, Transform::getIdentity(), infMatrixDiagonal(99, 99, 99, 99, 99, 99)));
+
+	const std::vector<double> maxInf = graph::getMaxOdomInf(links);
+	ASSERT_EQ(maxInf.size(), 6u);
+	EXPECT_DOUBLE_EQ(maxInf[0], 6.0);
+	EXPECT_DOUBLE_EQ(maxInf[5], 6.0);
+}
+
+TEST(GraphTest, GetPathsNeighborChain)
+{
+	std::map<int, Transform> poses = linePoses(3);
+	std::multimap<int, Link> links;
+	insertLink(links, neighborLink(1, 2));
+	insertLink(links, neighborLink(2, 3));
+
+	const std::list<std::map<int, Transform> > paths = graph::getPaths(poses, links);
+	ASSERT_EQ(paths.size(), 1u);
+	EXPECT_EQ(paths.front().size(), 3u);
+	EXPECT_EQ(poses.size(), 3u); // poses passed by value, caller's map is unchanged
+}