<trclass="memdesc:a96a6c2e8348e3d9cfd8b1770d17da9b2"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Exports <aclass="el"href="classrtabmap_1_1GPS.html"title="WGS84 GPS fix attached to a sensor sample or graph node.">GPS</a> samples to a PLY point cloud. <br/></td></tr>
<trclass="memdesc:a4691b760dfafe6d93de514c96fa55e0e"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Finds the worst pose-graph constraint residuals after optimization. <br/></td></tr>
<trclass="memitem:aaa0b06795dea514ad1d5fdd4626d32ec"id="r_aaa0b06795dea514ad1d5fdd4626d32ec"><tdclass="memItemLeft"align="right"valign="top">std::multimap< int, <aclass="el"href="classrtabmap_1_1Link.html">Link</a>>::iterator RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#aaa0b06795dea514ad1d5fdd4626d32ec">findLink</a> (std::multimap< int, <aclass="el"href="classrtabmap_1_1Link.html">Link</a>>&links, int from, int to, bool checkBothWays=true, <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91e">Link::Type</a> type=<aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8a720d632258ba1b468af0319ae8e7d4">Link::kUndef</a>)</td></tr>
<trclass="memdesc:aaa0b06795dea514ad1d5fdd4626d32ec"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Finds the first link from <code>from</code> to <code>to</code> in a multimap keyed by source id. <br/></td></tr>
<trclass="memitem:a06ed3112247c13dcf494665c1d9df327"id="r_a06ed3112247c13dcf494665c1d9df327"><tdclass="memItemLeft"align="right"valign="top">std::multimap< int, int >::iterator RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#a06ed3112247c13dcf494665c1d9df327">findLink</a> (std::multimap< int, int >&links, int from, int to, bool checkBothWays=true)</td></tr>
<trclass="memitem:aecdcc01682092f4c89257d070aa2a437"id="r_aecdcc01682092f4c89257d070aa2a437"><tdclass="memItemLeft"align="right"valign="top">std::multimap< int, int >::const_iterator RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#aecdcc01682092f4c89257d070aa2a437">findLink</a> (const std::multimap< int, int >&links, int from, int to, bool checkBothWays=true)</td></tr>
<trclass="memitem:a308c9331cd3dd36a776517e64b33bd82"id="r_a308c9331cd3dd36a776517e64b33bd82"><tdclass="memItemLeft"align="right"valign="top">std::list<<aclass="el"href="classrtabmap_1_1Link.html">Link</a>> RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#a308c9331cd3dd36a776517e64b33bd82">findLinks</a> (const std::multimap< int, <aclass="el"href="classrtabmap_1_1Link.html">Link</a>>&links, int from)</td></tr>
<trclass="memdesc:a308c9331cd3dd36a776517e64b33bd82"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Lists all links incident on node <code>from</code>. <br/></td></tr>
<trclass="memdesc:a592918d54b32d6f13236de85958da300"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Filters links by type or self-reference. <br/></td></tr>
<trclass="memdesc:a8213a5876c86611ee4c832981eb1e7de"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Keeps poses inside (or outside) a camera frustum. <br/></td></tr>
<trclass="memdesc:a1906b8b4ad4b7ffc955d22c4996b1506"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Subsamples poses that are spatially (and optionally angularly) redundant. <br/></td></tr>
<trclass="memdesc:acaf3e4ffcfc830747c3c3cb02a8ea155"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Radius-neighbor clustering of poses. <br/></td></tr>
<trclass="memdesc:aabda884597e6b8df053d04ee56cfaf6f"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Reduces a pose graph into hyper-nodes and hyper-links. <br/></td></tr>
<trclass="memitem:a5ec6c3883f73181d9458c5905c0aa0ea"id="r_a5ec6c3883f73181d9458c5905c0aa0ea"><tdclass="memItemLeft"align="right"valign="top">std::list< std::pair< int, <aclass="el"href="classrtabmap_1_1Transform.html">Transform</a>>> RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#a5ec6c3883f73181d9458c5905c0aa0ea">computePath</a> (const std::map< int, <aclass="el"href="classrtabmap_1_1Transform.html">rtabmap::Transform</a>>&poses, const std::multimap< int, int >&links, int from, int to, bool updateNewCosts=false)</td></tr>
<trclass="memdesc:a5ec6c3883f73181d9458c5905c0aa0ea"><tdclass="mdescLeft"> </td><tdclass="mdescRight">A* shortest path on a pose graph with Euclidean edge costs. <br/></td></tr>
<trclass="memitem:ae21d02d613df461d6163f9d9a3db7ad2"id="r_ae21d02d613df461d6163f9d9a3db7ad2"><tdclass="memItemLeft"align="right"valign="top">std::list< int > RTABMAP_CORE_EXPORT </td><tdclass="memItemRight"valign="bottom"><aclass="el"href="namespacertabmap_1_1graph.html#ae21d02d613df461d6163f9d9a3db7ad2">computePath</a> (const std::multimap< int, <aclass="el"href="classrtabmap_1_1Link.html">Link</a>>&links, int from, int to, bool updateNewCosts=false, bool useSameCostForAllLinks=false)</td></tr>
<trclass="memdesc:ae21d02d613df461d6163f9d9a3db7ad2"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Dijkstra shortest path on link constraints. <br/></td></tr>
<trclass="memdesc:a1ec6b9506985624f31fb2091219cb142"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Dijkstra path through the live <aclass="el"href="classrtabmap_1_1Memory.html">Memory</a> pose graph. <br/></td></tr>
<trclass="memdesc:a3d6e74a018b07cd52151e3e168f6384c"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Id of the nearest pose to <code>targetPose</code>. <br/></td></tr>
<trclass="memdesc:a92dfe14b22f897e11bf5bf3f7a7c9157"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Spatial neighbors of a node (KD-tree radius or k-NN search). <br/></td></tr>
<trclass="memdesc:a70a5f4b17aa3bb54cad151c9f290ff7f"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Spatial neighbors of a pose (KD-tree radius or k-NN search). <br/></td></tr>
<trclass="memdesc:a8cd823f440a4512407ef78d76ff1dbbf"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Like <aclass="el"href="namespacertabmap_1_1graph.html#a92dfe14b22f897e11bf5bf3f7a7c9157">findNearestNodes(int,const std::map<int,Transform>&,float,float,int)</a> but returns full <aclass="el"href="classrtabmap_1_1Transform.html">Transform</a> values. <br/></td></tr>
<trclass="memdesc:a3cd2009752bc8e39ec00f7f08cfd4a39"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Path length along an ordered list of poses. <br/></td></tr>
<trclass="memdesc:a0a04ab5289c258725555d6c18a076094"><tdclass="mdescLeft"> </td><tdclass="mdescRight">Splits poses into chains connected only by neighbor links. <br/></td></tr>
<divclass="textblock"><p>Pose-graph I/O, trajectory metrics, link utilities, and path planning. </p>
<p>Functions operate on maps of signature ids to <aclass="el"href="classrtabmap_1_1Transform.html">Transform</a> poses and <aclass="el"href="classrtabmap_1_1Link.html">Link</a> constraints (typically stored as <code>std::multimap<int, <aclass="el"href="classrtabmap_1_1Link.html"title="Directed constraint between two nodes in RTAB-Map's pose graph.">Link</a>></code> keyed by the source node id).</p>
<li><code>0</code> Raw text (<code>.txt</code>): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</li>
<li><code>1</code> RGBD-SLAM format, in motion capture frame like the ground truth of RGB-D SLAM Dataset (requires <code>stamps</code>) : stamp x y z qx qy qz qw</li>
<li><code>10</code> Like <code>1</code> without coordinate-frame change (i.e., in base frame) : stamp x y z qx qy qz qw</li>
<li><code>11</code> Like <code>10</code> with landmark ids after positive ids : stamp x y z qx qy qz qw id</li>
<li><code>2</code> KITTI odometry format : r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</li>
<tr><tdclass="paramname">poses</td><td>Node id → pose. </td></tr>
<tr><tdclass="paramname">constraints</td><td>Required for formats <code>3</code> and <code>4</code>. </td></tr>
<tr><tdclass="paramname">stamps</td><td>Required for formats <code>1</code>, <code>10</code>, and <code>11</code> (same size as <code>poses</code>). </td></tr>
<tr><tdclass="paramname">parameters</td><td>Optional optimizer parameters for formats <code>3</code> and <code>4</code>. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>False on I/O or validation error. </dd></dl>
<li><code>0</code> Raw text: 3×4 matrix per line (<code><aclass="el"href="classrtabmap_1_1Transform.html#a8358aacfcddec4ab82c43c758cad0b75"title="Parses a transform from a string representation. Supported formats:">Transform::fromString()</a></code>)</li>
<li><code>1</code> RGBD-SLAM motion capture: stamp x y z qw qx qy qz (applies optical-frame conversion)</li>
<li><code>2</code> KITTI odometry: 3×4 matrix per line (applies optical-frame conversion)</li>
<li><code>5</code> NewCollege: stamp x y (2D; first pose is origin)</li>
<li><code>6</code> Malaga Urban <aclass="el"href="classrtabmap_1_1GPS.html"title="WGS84 GPS fix attached to a sensor sample or graph node.">GPS</a>: 25-field <code>*_GPS.txt</code> line (local X/Y/Z)</li>
<li><code>7</code> St Lucia INS: 12-field log (<aclass="el"href="classrtabmap_1_1GPS.html"title="WGS84 GPS fix attached to a sensor sample or graph node.">GPS</a> → local ENU + roll/pitch/yaw)</li>
<li><code>8</code> Karlsruhe: timestamp lat lon alt x y z roll pitch yaw (first pose is origin)</li>
<li><code>9</code> EuRoC MAV: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az (17 CSV fields)</li>
<li><code>10</code> RGBD-SLAM like <code>1</code> without coordinate-frame change</li>
<li><code>11</code> RGBD-SLAM like <code>10</code> with node id as 9th field: stamp x y z qw qx qy qz id</li>
<p>Exports <aclass="el"href="classrtabmap_1_1GPS.html"title="WGS84 GPS fix attached to a sensor sample or graph node.">GPS</a> samples to a PLY point cloud. </p>
<p>KITTI odometry benchmark error over fixed trajectory segments. </p>
<p>For each start pose (every 10 frames) and segment length in {100, 200, …, 800} m along <code>poses_gt</code>, compares the relative transform GT vs estimate and accumulates normalized errors. The returned values are the mean over all valid segments.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses_gt</td><td>Ground-truth poses in temporal order (one per frame). </td></tr>
<tr><tdclass="paramname">poses_result</td><td>Estimated poses (same length and ordering as <code>poses_gt</code>). </td></tr>
<tr><tdclass="paramname">t_err</td><td>Output mean translation error (%): segment translation error (m) divided by segment length, averaged, then × 100. </td></tr>
<tr><tdclass="paramname">r_err</td><td>Output mean rotation error (deg/m): segment rotation error (rad) divided by segment length, averaged, then converted to deg/m. </td></tr>
<p>Mean frame-to-frame relative pose error (RPE-style, one step). </p>
<p>For each consecutive pair <code>(i, i+1)</code>, builds the relative motion in ground truth and in the estimate, then measures how much they differ:</p><ul>
<li>translation: Euclidean distance between the two relative transforms (m)</li>
<li>rotation: angle between the two relative transforms (rad → deg)</li>
</ul>
<p>Returns the arithmetic mean over all <code>N-1</code> pairs (<code>N</code> = trajectory length). Unlike <aclass="el"href="namespacertabmap_1_1graph.html#a3c9effdb0d7c235dc1b8d7e3f8487c67">calcKittiSequenceErrors()</a>, there is no fixed segment length and no path-length normalization.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses_gt</td><td>Ground-truth poses in temporal order (one per frame). </td></tr>
<tr><tdclass="paramname">poses_result</td><td>Estimated poses (same length and ordering as <code>poses_gt</code>). </td></tr>
<tr><tdclass="paramname">t_err</td><td>Output mean translation error over consecutive pairs (m). </td></tr>
<tr><tdclass="paramname">r_err</td><td>Output mean rotation error over consecutive pairs (deg). </td></tr>
<p>Only poses whose id exists in both <code>groundTruth</code> and <code>poses</code> are compared. An alignment transform <code>t</code> is estimated so that per-pose error is measured after bringing the estimate into the reference frame:</p><ul>
<li>If more than five poses match: <code>t</code> from SVD on position correspondences (estimate positions → ground-truth positions; z ignored when <code>align2D</code> is true).</li>
<li>Otherwise: <code>t</code> = groundTruth[firstId] * poses[firstId]⁻¹ using the first matched id.</li>
</ul>
<p>For each matched pose, after <code>aligned = t * poses[id]</code>:</p><ul>
<li><b>Translational error:</b> Euclidean distance between <code>aligned</code> and <code>groundTruth[id]</code> (m).</li>
<li><b>Rotational error:</b> Angle between the poses' +X axes (deg).</li>
</ul>
<p>The eight <code>@p translational_*</code> and <code>@p rotational_*</code> outputs are statistics over those per-pose errors (all matched poses). They are set to <code>0</code> when no id matches.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">groundTruth</td><td>Reference trajectory (node id → pose). </td></tr>
<tr><tdclass="paramname">poses</td><td>Estimated trajectory; ids not in <code>groundTruth</code> are skipped. </td></tr>
<tr><tdclass="paramname">translational_rmse</td><td>Root mean square of translational errors (m). </td></tr>
<tr><tdclass="paramname">translational_mean</td><td>Arithmetic mean of translational errors (m). </td></tr>
<tr><tdclass="paramname">translational_median</td><td>Middle sample in matched-pose iteration order (m). </td></tr>
<tr><tdclass="paramname">translational_std</td><td>Standard deviation of translational errors (m). </td></tr>
<p>Finds the worst pose-graph constraint residuals after optimization. </p>
<p>Iterates over <code>links</code> and, for each non-self-referenced edge (<code>from != to</code>):</p><oltype="1">
<li>Looks up <code>T_from</code> and <code>T_to</code> in <code>poses</code> (returns default <aclass="el"href="structrtabmap_1_1graph_1_1MaxGraphErrors.html">MaxGraphErrors</a> if any endpoint pose is missing, null, or not invertible).</li>
<li>Builds the relative pose implied by the optimized poses:<ul>
<li><aclass="el"href="classrtabmap_1_1Landmark.html"title="Optimized pose of a visual landmark (e.g. ArUco/AprilTag) in the map.">Landmark</a> (<code>from < 0</code>): <code>t = T_to⁻¹ · T_from</code>, link measurement inverted</li>
</ul>
</li>
<li>Compares <code>t</code> to the link transform:<ul>
<li><b>Linear error:</b> max |Δx|, |Δy|, and |Δz| (z ignored when <code>for3DoF</code> is true).</li>
<li><b>Angular error:</b> full 3D angle between <code>t</code> and the link, or yaw-only if <code>for3DoF</code>; skipped for <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea22912bbbcacce5ee39e61da7ec4640b9">Link::kLandmark</a> when the information matrix does not constrain yaw.</li>
</ul>
</li>
<li>Normalizes by link uncertainty: <code>error / sqrt(variance)</code> using the link information matrix (largest diagonal variance for translation/rotation).</li>
</ol>
<p>The returned <aclass="el"href="structrtabmap_1_1graph_1_1MaxGraphErrors.html">MaxGraphErrors</a> holds the link with the highest linear and angular <em>ratios</em> (not necessarily the largest absolute error).</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Optimized node poses (must contain every <code>from</code> and <code>to</code> id used). </td></tr>
<tr><tdclass="paramname">links</td><td>Graph constraints (typically <code>std::multimap<int, <aclass="el"href="classrtabmap_1_1Link.html"title="Directed constraint between two nodes in RTAB-Map's pose graph.">Link</a>></code> keyed by <code>from</code>). </td></tr>
<tr><tdclass="paramname">for3DoF</td><td>If true, linear error uses x/y only and angular error compares yaw only. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd><aclass="el"href="structrtabmap_1_1graph_1_1MaxGraphErrors.html">MaxGraphErrors</a>; fields stay <code>-1</code> when no valid link was checked or on early abort. </dd></dl>
<p>Maximum information-matrix diagonal over odometry neighbor links. </p>
<p>Scans <code>links</code> of type <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8f60efd83d5fad3493573bef3d4a8adf">Link::kNeighbor</a> or <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8fbb6dbe2d16e19ad8fecb6bac65e9db">Link::kNeighborMerged</a> and, for each dof (x, y, z, roll, pitch, yaw), keeps the largest diagonal entry of the 6×6 information matrix.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">links</td><td>Graph constraints (multimap keyed by source id). </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Six maximum information values, or an empty vector if no neighbor links exist. </dd></dl>
<p>Finds the first link from <code>from</code> to <code>to</code> in a multimap keyed by source id. </p>
<p>Iterates all entries with key <code>from</code> and matches the destination (and optionally <code>type</code>). When <code>checkBothWays</code> is true, also searches key <code>to</code> for a link back to <code>from</code>.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">links</td><td><aclass="el"href="classrtabmap_1_1Link.html"title="Directed constraint between two nodes in RTAB-Map's pose graph.">Link</a> multimap (<code>key</code> = source node id). </td></tr>
<tr><tdclass="paramname">checkBothWays</td><td>If true, also match <code>to → from</code>. </td></tr>
<tr><tdclass="paramname">type</td><td>Required link type, or <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8a720d632258ba1b468af0319ae8e7d4">Link::kUndef</a> to accept any type. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Iterator to the link, or <code>links.end()</code> if not found. </dd></dl>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. <code>std::multimap<int, std::pair<int, <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91e"title="Link category and filter sentinels.">Link::Type</a>>></code>. </p>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. <code>std::multimap<int, int></code>. </p>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const <code>std::multimap<int, <aclass="el"href="classrtabmap_1_1Link.html"title="Directed constraint between two nodes in RTAB-Map's pose graph.">Link</a>></code>. </p>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const <code>std::multimap<int, std::pair<int, <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91e"title="Link category and filter sentinels.">Link::Type</a>>></code>. </p>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Const <code>std::multimap<int, int></code>. </p>
<p>Lists all links incident on node <code>from</code>. </p>
<p>Outgoing links (<code>link.from() == from</code>) are returned as stored; for incoming links (<code>link.to() == from</code>), the inverse link is returned so the pose of <code>from</code> is always the source frame.</p>
<p>Keeps the first occurrence of each <code>(from, to)</code> or <code>(to, from)</code> pair with the same <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91e">Link::Type</a> (see <aclass="el"href="namespacertabmap_1_1graph.html#aaa0b06795dea514ad1d5fdd4626d32ec">findLink()</a> with <code>checkBothWays</code>).</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">links</td><td>Input link multimap. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Copy without duplicates. </dd></dl>
<li>If <code>filteredType</code> is <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ead52adb0176a83a02833e89111f66497e">kSelfRefLink</a>: exclude self-references (<code>from == to</code>), or include only them when <code>inverted</code> is true.</li>
<li>Otherwise: exclude links of <code>filteredType</code>, or keep only that type when <code>inverted</code> is true.</li>
<tr><tdclass="paramname">filteredType</td><td>Type to filter, or <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ead52adb0176a83a02833e89111f66497e">Link::kSelfRefLink</a> for self-reference filtering. </td></tr>
<tr><tdclass="paramname">inverted</td><td>If true, keep the filtered set instead of removing it. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Filtered link container (same structure as input). </dd></dl>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. For <code>std::map<int, <aclass="el"href="classrtabmap_1_1Link.html"title="Directed constraint between two nodes in RTAB-Map's pose graph.">Link</a>></code>. </p>
<p>Keeps poses inside (or outside) a camera frustum. </p>
<p>Transforms each pose position into the frustum defined by <code>cameraPose</code> using <aclass="el"href="group__FrustumFiltering.html#gad05921639a3371d2ee0f684b77542330">util3d::frustumFiltering()</a> (this assumes the cameraPose includes the optical rotation of the camera (X right, Y down, Z forward).</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Input poses (null poses are skipped) in base frame (X forward, Y left, Z up), </td></tr>
<tr><tdclass="paramname">cameraPose</td><td>Frustum origin and orientation including the optical rotation of the camera (X right, Y down, Z forward). </td></tr>
<tr><tdclass="paramname">horizontalFOV</td><td>Horizontal field of view (deg); see <aclass="el"href="classrtabmap_1_1CameraModel.html#a12abd4cac897f48bf862a96f0eba1d65">CameraModel::horizontalFOV()</a>. </td></tr>
<tr><tdclass="paramname">verticalFOV</td><td>Vertical field of view (deg); see <aclass="el"href="classrtabmap_1_1CameraModel.html#af6226e43afcb69259b137bbe50099537">CameraModel::verticalFOV()</a>. </td></tr>
<p>Subsamples poses that are spatially (and optionally angularly) redundant. </p>
<p>For each pose not yet processed, finds all poses within <code>radius</code> (KD-tree). When <code>angle</code>> 0, only poses whose +X axis differs by at most <code>angle</code> (rad) are grouped. From each group, keeps one pose: the latest in map order if <code>keepLatest</code>, otherwise the earliest. The first and last poses of the input map are always kept.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Input trajectory (map iteration order defines “latest/oldest”). </td></tr>
<tr><tdclass="paramname">radius</td><td>Clustering radius (m); if <code>≤ 0</code> or fewer than three poses, returns <code>poses</code> unchanged. </td></tr>
<tr><tdclass="paramname">angle</td><td>Max heading difference within a cluster (rad); <code>0</code> ignores orientation. </td></tr>
<tr><tdclass="paramname">keepLatest</td><td>If true, keep the latest pose per cluster; otherwise the earliest. </td></tr>
<p>For each pose, inserts <code>(queryId, neighborId)</code> into the output for every other pose within <code>radius</code> (and within <code>angle</code> of the query heading when <code>angle</code>> 0).</p>
<p>Reduces a pose graph into hyper-nodes and hyper-links. </p>
<p><b>Hyper-nodes:</b> 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.</p>
<p><b>Hyper-links:</b> for each <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8f60efd83d5fad3493573bef3d4a8adf">Link::kNeighbor</a> or <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8fbb6dbe2d16e19ad8fecb6bac65e9db">Link::kNeighborMerged</a> link between different clusters, builds one merged <aclass="el"href="classrtabmap_1_1Link.html">Link</a> along the shortest path through intra-cluster closure links (Dijkstra with unit cost).</p>
<p>A* shortest path on a pose graph with Euclidean edge costs. </p>
<p>Edge cost between adjacent nodes is the Euclidean distance between their poses in <code>poses</code>. Uses <code>costSoFar + distToEnd</code> where <code>distToEnd</code> is the distance to the goal pose.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Node id → pose (must contain every node reached by <code>links</code>). </td></tr>
<tr><tdclass="paramname">links</td><td>Directed edges (<code>from</code> → <code>to</code>) keyed by source id. </td></tr>
<tr><tdclass="paramname">updateNewCosts</td><td>If true, use a multimap queue that can decrease keys when a shorter path is found. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Ordered path from <code>from</code> to <code>to</code> (inclusive) with poses; empty if unreachable. </dd></dl>
<p>Dijkstra shortest path on link constraints. </p>
<p>Explores outgoing links keyed by <code>link.from()</code>. Edge cost is <code>1</code> when <code>useSameCostForAllLinks</code> is true, otherwise the translation norm of the link transform.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">links</td><td>Constraints keyed by source node id. </td></tr>
<p>Dijkstra path through the live <aclass="el"href="classrtabmap_1_1Memory.html">Memory</a> pose graph. </p>
<p>Loads links from <aclass="el"href="classrtabmap_1_1Memory.html">Memory</a> (optionally from the database), chains transforms along the chosen path, and returns the accumulated poses. Self-referenced links are skipped.</p>
<p>By default (<code>linearVelocity</code> and <code>angularVelocity</code> ≤ 0), edge cost is translation distance (m) only. When set > 0, costs are expressed in seconds of motion:</p><ul>
<li><code>linearVelocity</code> adds <code>linkTranslation / linearVelocity</code> (time to drive the edge at that speed). Used alone it scales every edge by the same factor, so the <b>shortest path is unchanged</b>; set it to your robot’s typical forward speed (e.g. <code>0.5</code> m/s) when you also use <code>angularVelocity</code> so translation and rotation costs are comparable.</li>
<li><code>angularVelocity</code> adds <code>headingMismatch / angularVelocity</code>, 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 <b>mostly forward</b> (small mismatch) can beat a shorter route through loop closures that require large reorientations (e.g. <code>angularVelocity = 1.0</code> rad/s with <code>linearVelocity = 0.5</code> m/s). With <code>angularVelocity</code>> 0 and <code>linearVelocity</code> ≤ 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.</li>
</ul>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">fromId</td><td>Start signature id (<code>≥ 0</code>). </td></tr>
<tr><tdclass="paramname">toId</td><td>Goal signature id (<code>≠ 0</code>). </td></tr>
<tr><tdclass="paramname">memory</td><td>Graph memory (must not be null). </td></tr>
<tr><tdclass="paramname">lookInDatabase</td><td>If true, load links from the database when not already in RAM. </td></tr>
<tr><tdclass="paramname">updateNewCosts</td><td>If true, allow cost improvements on open nodes. </td></tr>
<tr><tdclass="paramname">angularVelocity</td><td>If > 0, add rotation time from motion direction change (rad/s). </td></tr>
<tr><tdclass="paramname">ignoreDirectLinks</td><td>If true, skip the direct edge between <code>fromId</code> and <code>toId</code>. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Path as <code>(nodeId, pose)</code> pairs; first pose is identity at <code>fromId</code>. Empty if unreachable. </dd></dl>
<p>Id of the nearest pose to <code>targetPose</code>. </p>
<p>Wrapper around <aclass="el"href="namespacertabmap_1_1graph.html#a92dfe14b22f897e11bf5bf3f7a7c9157">findNearestNodes()</a> with <code>radius=0</code>, <code>k=1</code> (1-NN in 3D).</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Nodes to search. </td></tr>
<tr><tdclass="paramname">targetPose</td><td>Query position (x, y, z only; orientation is not used). </td></tr>
<tr><tdclass="paramname">distance</td><td>If not null, set to the squared Euclidean distance of the match. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>Closest node id, or <code>0</code> if <code>poses</code> is empty. </dd></dl>
<p>Spatial neighbors of a node (KD-tree radius or k-NN search). </p>
<p><code>nodeId</code> is removed from the search set. Requires <code>radius &gt; 0</code> or <code>k &gt; 0</code>. When <code>radius &gt; 0</code>, returns all poses within <code>radius</code> (up to <code>k</code> if <code>k &gt; 0</code>). When <code>radius == 0</code>, returns the <code>k</code> nearest neighbors.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">nodeId</td><td>Query node (must exist in <code>poses</code>); excluded from results. </td></tr>
<p>This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. Query by <aclass="el"href="classrtabmap_1_1Transform.html">Transform</a> instead of node id. </p>
<p>Splits poses into chains connected only by neighbor links. </p>
<p>Repeatedly builds a path starting from the lowest remaining id: adds the next pose in map order only if a <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8f60efd83d5fad3493573bef3d4a8adf">Link::kNeighbor</a> or <aclass="el"href="classrtabmap_1_1Link.html#a925bb7ca93eb95a3a873b0a9e8d5e91ea8fbb6dbe2d16e19ad8fecb6bac65e9db">Link::kNeighborMerged</a> link exists from the previous pose to it. Stops at the first gap, pushes the chain, and continues until <code>poses</code> is empty.</p>
<dlclass="params"><dt>Parameters</dt><dd>
<tableclass="params">
<tr><tdclass="paramname">poses</td><td>Input poses (cleared as segments are extracted). </td></tr>
<tr><tdclass="paramname">links</td><td>Graph constraints keyed by source id. </td></tr>
</table>
</dd>
</dl>
<dlclass="section return"><dt>Returns</dt><dd>List of pose maps, each a contiguous neighbor chain. </dd></dl>
<liclass="footer">Generated by <ahref="https://www.doxygen.org/index.html"><imgclass="footer"src="doxygen.svg"width="104"height="31"alt="doxygen"/></a> 1.9.8 </li>
