Graph.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef GRAPH_H_
29 #define GRAPH_H_
30 
31 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
32 
33 #include <map>
34 #include <list>
35 #include <rtabmap/core/Parameters.h>
36 #include <rtabmap/core/Link.h>
37 #include <rtabmap/core/GPS.h>
38 #include <rtabmap/core/CameraModel.h>
39 
40 namespace rtabmap {
41 class Memory;
42 
43 namespace graph {
44 
46 // Graph utilities
48 
49 bool RTABMAP_CORE_EXPORT exportPoses(
50  const std::string & filePath,
51  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)
52  const std::map<int, Transform> & poses,
53  const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
54  const std::map<int, double> & stamps = std::map<int, double>(), // required for format 1
55  const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
56 
57 bool RTABMAP_CORE_EXPORT importPoses(
58  const std::string & filePath,
59  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
60  std::map<int, Transform> & poses,
61  std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
62  std::map<int, double> * stamps = 0); // optional for format 1 and 9
63 
64 bool RTABMAP_CORE_EXPORT exportGPS(
65  const std::string & filePath,
66  const std::map<int, GPS> & gpsValues,
67  unsigned int rgba = 0xFFFFFFFF);
68 
77 void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
78  const std::vector<Transform> &poses_gt,
79  const std::vector<Transform> &poses_result,
80  float & t_err,
81  float & r_err);
82 
90 void RTABMAP_CORE_EXPORT calcRelativeErrors (
91  const std::vector<Transform> &poses_gt,
92  const std::vector<Transform> &poses_result,
93  float & t_err,
94  float & r_err);
95 
104 Transform RTABMAP_CORE_EXPORT calcRMSE(
105  const std::map<int, Transform> &groundTruth,
106  const std::map<int, Transform> &poses,
107  float & translational_rmse,
108  float & translational_mean,
109  float & translational_median,
110  float & translational_std,
111  float & translational_min,
112  float & translational_max,
113  float & rotational_rmse,
114  float & rotational_mean,
115  float & rotational_median,
116  float & rotational_std,
117  float & rotational_min,
118  float & rotational_max,
119  bool align2D = false);
120 
121 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
122  const std::map<int, Transform> & poses,
123  const std::multimap<int, Link> & links,
124  float & maxLinearErrorRatio,
125  float & maxAngularErrorRatio,
126  float & maxLinearError,
127  float & maxAngularError,
128  const Link ** maxLinearErrorLink = 0,
129  const Link ** maxAngularErrorLink = 0,
130  bool for3DoF = false);
131 
132 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
133 
134 std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
135  std::multimap<int, Link> & links,
136  int from,
137  int to,
138  bool checkBothWays = true,
139  Link::Type type = Link::kUndef);
140 std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
141  std::multimap<int, std::pair<int, Link::Type> > & links,
142  int from,
143  int to,
144  bool checkBothWays = true,
145  Link::Type type = Link::kUndef);
146 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
147  std::multimap<int, int> & links,
148  int from,
149  int to,
150  bool checkBothWays = true);
151 std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
152  const std::multimap<int, Link> & links,
153  int from,
154  int to,
155  bool checkBothWays = true,
156  Link::Type type = Link::kUndef);
157 std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
158  const std::multimap<int, std::pair<int, Link::Type> > & links,
159  int from,
160  int to,
161  bool checkBothWays = true,
162  Link::Type type = Link::kUndef);
163 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
164  const std::multimap<int, int> & links,
165  int from,
166  int to,
167  bool checkBothWays = true);
168 std::list<Link> RTABMAP_CORE_EXPORT findLinks(
169  const std::multimap<int, Link> & links,
170  int from);
171 
172 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
173  const std::multimap<int, Link> & links);
177 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
178  const std::multimap<int, Link> & links,
179  Link::Type filteredType,
180  bool inverted = false);
184 std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
185  const std::map<int, Link> & links,
186  Link::Type filteredType,
187  bool inverted = false);
188 
189 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
190 std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
191  const std::map<int, Transform> & poses,
192  const Transform & cameraPose,
193  float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
194  float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
195  float nearClipPlaneDistance = 0.1f,
196  float farClipPlaneDistance = 100.0f,
197  bool negative = false);
198 
207 std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
208  const std::map<int, Transform> & poses,
209  float radius,
210  float angle,
211  bool keepLatest = true);
212 
220 std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
221  const std::map<int, Transform> & poses,
222  float radius,
223  float angle);
224 
225 void reduceGraph(
226  const std::map<int, Transform> & poses,
227  const std::multimap<int, Link> & links,
228  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
229  std::multimap<int, Link> & hyperLinks);
230 
240 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
241  const std::map<int, rtabmap::Transform> & poses,
242  const std::multimap<int, int> & links,
243  int from,
244  int to,
245  bool updateNewCosts = false);
246 
257 std::list<int> RTABMAP_CORE_EXPORT computePath(
258  const std::multimap<int, Link> & links,
259  int from,
260  int to,
261  bool updateNewCosts = false,
262  bool useSameCostForAllLinks = false);
263 
273 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
274  int fromId,
275  int toId,
276  const Memory * memory,
277  bool lookInDatabase = true,
278  bool updateNewCosts = false,
279  float linearVelocity = 0.0f, // m/sec
280  float angularVelocity = 0.0f); // rad/sec
281 
289 int RTABMAP_CORE_EXPORT findNearestNode(
290  const std::map<int, rtabmap::Transform> & poses,
291  const rtabmap::Transform & targetPose,
292  float * distance = 0);
293 
302 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
303  int nodeId,
304  const std::map<int, Transform> & poses,
305  float radius,
306  float angle = 0.0f,
307  int k=0);
308 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
309  const Transform & targetPose,
310  const std::map<int, Transform> & poses,
311  float radius,
312  float angle = 0.0f,
313  int k=0);
314 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
315  int nodeId,
316  const std::map<int, Transform> & poses,
317  float radius,
318  float angle = 0.0f,
319  int k=0);
320 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
321  const Transform & targetPose,
322  const std::map<int, Transform> & poses,
323  float radius,
324  float angle = 0.0f,
325  int k=0);
326 
327 // Use new findNearestNodes() interface with radius=0, angle=0.
328 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
329 // Renamed to findNearestNodes()
330 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
331 // Renamed to findNearestNodes()
332 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
333 // Renamed to findNearestNodes()
334 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
335 // Renamed to findNearestNodes()
336 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);
337 
338 float RTABMAP_CORE_EXPORT computePathLength(
339  const std::vector<std::pair<int, Transform> > & path,
340  unsigned int fromIndex = 0,
341  unsigned int toIndex = 0);
342 
343 // assuming they are all linked in map order
344 float RTABMAP_CORE_EXPORT computePathLength(
345  const std::map<int, Transform> & path);
346 
347 std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
348  std::map<int, Transform> poses,
349  const std::multimap<int, Link> & links);
350 
351 void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
352  cv::Vec3f & min,
353  cv::Vec3f & max);
354 
355 } /* namespace graph */
356 
357 } /* namespace rtabmap */
358 #endif /* GRAPH_H_ */
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::graph::findLinks
std::list< Link > RTABMAP_CORE_EXPORT findLinks(const std::multimap< int, Link > &links, int from)
Definition: Graph.cpp:1233
rtabmap::graph::calcRelativeErrors
void RTABMAP_CORE_EXPORT calcRelativeErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:730
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1010
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_CORE_EXPORT radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1477
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
type
iterator
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Parameters.h
rtabmap::graph::frustumPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT frustumPosesFiltering(const std::map< int, Transform > &poses, const Transform &cameraPose, float horizontalFOV=45.0f, float verticalFOV=45.0f, float nearClipPlaneDistance=0.1f, float farClipPlaneDistance=100.0f, bool negative=false)
Definition: Graph.cpp:1316
rtabmap::graph::findNearestNode
int RTABMAP_CORE_EXPORT findNearestNode(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
Definition: Graph.cpp:2197
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:662
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max, bool align2D=false)
Definition: Graph.cpp:772
rtabmap::graph::computePathLength
float RTABMAP_CORE_EXPORT computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2360
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::graph::filterDuplicateLinks
std::multimap< int, Link > RTABMAP_CORE_EXPORT filterDuplicateLinks(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1252
rtabmap::graph::exportGPS
bool RTABMAP_CORE_EXPORT exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:510
rtabmap::graph::getPaths
std::list< std::map< int, Transform > > RTABMAP_CORE_EXPORT getPaths(std::map< int, Transform > poses, const std::multimap< int, Link > &links)
Definition: Graph.cpp:2409
GPS.h
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::graph::computeMinMax
void RTABMAP_CORE_EXPORT computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2441
rtabmap::graph::findLink
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)
Definition: Graph.cpp:1038
graph
FactorGraph< FACTOR > * graph
rtabmap::graph::findNearestNodes
std::map< int, float > RTABMAP_CORE_EXPORT findNearestNodes(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2216
path
path
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
nodes
KeyVector nodes
rtabmap::Transform
Definition: Transform.h:41
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_CORE_EXPORT computeMaxGraphErrors(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, float &maxLinearErrorRatio, float &maxAngularErrorRatio, float &maxLinearError, float &maxAngularError, const Link **maxLinearErrorLink=0, const Link **maxAngularErrorLink=0, bool for3DoF=false)
Definition: Graph.cpp:910
rtabmap::graph::importPoses
bool RTABMAP_CORE_EXPORT importPoses(const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > *constraints=0, std::map< int, double > *stamps=0)
Definition: Graph.cpp:197
rtabmap::graph::findNearestPoses
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)
Definition: Graph.cpp:2305
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_CORE_EXPORT filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
Definition: Graph.cpp:1266
rtabmap::Memory
Definition: Memory.h:64
distance
Double_ distance(const OrientedPlane3_ &p)
CameraModel.h
rtabmap::graph::reduceGraph
void reduceGraph(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, std::multimap< int, int > &hyperNodes, std::multimap< int, Link > &hyperLinks)
Definition: Graph.cpp:1533
rtabmap::graph::getPosesInRadius
RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)
Definition: Graph.cpp:2350
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1365
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1770
rtabmap::graph::getNodesInRadius
RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:2342

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:54