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 
120 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
121  const std::map<int, Transform> & poses,
122  const std::multimap<int, Link> & links,
123  float & maxLinearErrorRatio,
124  float & maxAngularErrorRatio,
125  float & maxLinearError,
126  float & maxAngularError,
127  const Link ** maxLinearErrorLink = 0,
128  const Link ** maxAngularErrorLink = 0,
129  bool for3DoF = false);
130 
131 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
132 
133 std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
134  std::multimap<int, Link> & links,
135  int from,
136  int to,
137  bool checkBothWays = true,
138  Link::Type type = Link::kUndef);
139 std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
140  std::multimap<int, std::pair<int, Link::Type> > & links,
141  int from,
142  int to,
143  bool checkBothWays = true,
144  Link::Type type = Link::kUndef);
145 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
146  std::multimap<int, int> & links,
147  int from,
148  int to,
149  bool checkBothWays = true);
150 std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
151  const std::multimap<int, Link> & links,
152  int from,
153  int to,
154  bool checkBothWays = true,
155  Link::Type type = Link::kUndef);
156 std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
157  const std::multimap<int, std::pair<int, Link::Type> > & links,
158  int from,
159  int to,
160  bool checkBothWays = true,
161  Link::Type type = Link::kUndef);
162 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
163  const std::multimap<int, int> & links,
164  int from,
165  int to,
166  bool checkBothWays = true);
167 std::list<Link> RTABMAP_CORE_EXPORT findLinks(
168  const std::multimap<int, Link> & links,
169  int from);
170 
171 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
172  const std::multimap<int, Link> & links);
176 std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
177  const std::multimap<int, Link> & links,
178  Link::Type filteredType,
179  bool inverted = false);
183 std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
184  const std::map<int, Link> & links,
185  Link::Type filteredType,
186  bool inverted = false);
187 
188 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
189 std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
190  const std::map<int, Transform> & poses,
191  const Transform & cameraPose,
192  float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
193  float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
194  float nearClipPlaneDistance = 0.1f,
195  float farClipPlaneDistance = 100.0f,
196  bool negative = false);
197 
206 std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
207  const std::map<int, Transform> & poses,
208  float radius,
209  float angle,
210  bool keepLatest = true);
211 
219 std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
220  const std::map<int, Transform> & poses,
221  float radius,
222  float angle);
223 
224 void reduceGraph(
225  const std::map<int, Transform> & poses,
226  const std::multimap<int, Link> & links,
227  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
228  std::multimap<int, Link> & hyperLinks);
229 
239 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
240  const std::map<int, rtabmap::Transform> & poses,
241  const std::multimap<int, int> & links,
242  int from,
243  int to,
244  bool updateNewCosts = false);
245 
256 std::list<int> RTABMAP_CORE_EXPORT computePath(
257  const std::multimap<int, Link> & links,
258  int from,
259  int to,
260  bool updateNewCosts = false,
261  bool useSameCostForAllLinks = false);
262 
272 std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
273  int fromId,
274  int toId,
275  const Memory * memory,
276  bool lookInDatabase = true,
277  bool updateNewCosts = false,
278  float linearVelocity = 0.0f, // m/sec
279  float angularVelocity = 0.0f); // rad/sec
280 
288 int RTABMAP_CORE_EXPORT findNearestNode(
289  const std::map<int, rtabmap::Transform> & poses,
290  const rtabmap::Transform & targetPose,
291  float * distance = 0);
292 
301 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
302  int nodeId,
303  const std::map<int, Transform> & poses,
304  float radius,
305  float angle = 0.0f,
306  int k=0);
307 std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
308  const Transform & targetPose,
309  const std::map<int, Transform> & poses,
310  float radius,
311  float angle = 0.0f,
312  int k=0);
313 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
314  int nodeId,
315  const std::map<int, Transform> & poses,
316  float radius,
317  float angle = 0.0f,
318  int k=0);
319 std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
320  const Transform & targetPose,
321  const std::map<int, Transform> & poses,
322  float radius,
323  float angle = 0.0f,
324  int k=0);
325 
326 // Use new findNearestNodes() interface with radius=0, angle=0.
327 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
328 // Renamed to findNearestNodes()
329 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
330 // Renamed to findNearestNodes()
331 RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
332 // Renamed to findNearestNodes()
333 RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
334 // Renamed to findNearestNodes()
335 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);
336 
337 float RTABMAP_CORE_EXPORT computePathLength(
338  const std::vector<std::pair<int, Transform> > & path,
339  unsigned int fromIndex = 0,
340  unsigned int toIndex = 0);
341 
342 // assuming they are all linked in map order
343 float RTABMAP_CORE_EXPORT computePathLength(
344  const std::map<int, Transform> & path);
345 
346 std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
347  std::map<int, Transform> poses,
348  const std::multimap<int, Link> & links);
349 
350 void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
351  cv::Vec3f & min,
352  cv::Vec3f & max);
353 
354 } /* namespace graph */
355 
356 } /* namespace rtabmap */
357 #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:1219
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:717
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:996
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_CORE_EXPORT radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1463
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
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)
Definition: Graph.cpp:759
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:1302
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:2183
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:649
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:2346
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:1238
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:497
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:2395
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:2427
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:1024
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:2202
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:896
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:184
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:2291
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:1252
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:1519
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:2336
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:1351
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:1756
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:2328

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:10