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/RtabmapExp.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_EXP 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_EXP 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_EXP exportGPS(
65  const std::string & filePath,
66  const std::map<int, GPS> & gpsValues,
67  unsigned int rgba = 0xFFFFFFFF);
68 
77 void RTABMAP_EXP 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_EXP 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_EXP 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_EXP 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_EXP getMaxOdomInf(const std::multimap<int, Link> & links);
132 
133 std::multimap<int, Link>::iterator RTABMAP_EXP 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, int>::iterator RTABMAP_EXP findLink(
140  std::multimap<int, int> & links,
141  int from,
142  int to,
143  bool checkBothWays = true);
144 std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
145  const std::multimap<int, Link> & links,
146  int from,
147  int to,
148  bool checkBothWays = true,
149  Link::Type type = Link::kUndef);
150 std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
151  const std::multimap<int, int> & links,
152  int from,
153  int to,
154  bool checkBothWays = true);
155 std::list<Link> RTABMAP_EXP findLinks(
156  const std::multimap<int, Link> & links,
157  int from);
158 
159 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
160  const std::multimap<int, Link> & links);
164 std::multimap<int, Link> RTABMAP_EXP filterLinks(
165  const std::multimap<int, Link> & links,
166  Link::Type filteredType,
167  bool inverted = false);
171 std::map<int, Link> RTABMAP_EXP filterLinks(
172  const std::map<int, Link> & links,
173  Link::Type filteredType,
174  bool inverted = false);
175 
176 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
177 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
178  const std::map<int, Transform> & poses,
179  const Transform & cameraPose,
180  float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
181  float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
182  float nearClipPlaneDistance = 0.1f,
183  float farClipPlaneDistance = 100.0f,
184  bool negative = false);
185 
194 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
195  const std::map<int, Transform> & poses,
196  float radius,
197  float angle,
198  bool keepLatest = true);
199 
207 std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
208  const std::map<int, Transform> & poses,
209  float radius,
210  float angle);
211 
212 void reduceGraph(
213  const std::map<int, Transform> & poses,
214  const std::multimap<int, Link> & links,
215  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
216  std::multimap<int, Link> & hyperLinks);
217 
227 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
228  const std::map<int, rtabmap::Transform> & poses,
229  const std::multimap<int, int> & links,
230  int from,
231  int to,
232  bool updateNewCosts = false);
233 
244 std::list<int> RTABMAP_EXP computePath(
245  const std::multimap<int, Link> & links,
246  int from,
247  int to,
248  bool updateNewCosts = false,
249  bool useSameCostForAllLinks = false);
250 
260 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
261  int fromId,
262  int toId,
263  const Memory * memory,
264  bool lookInDatabase = true,
265  bool updateNewCosts = false,
266  float linearVelocity = 0.0f, // m/sec
267  float angularVelocity = 0.0f); // rad/sec
268 
276 int RTABMAP_EXP findNearestNode(
277  const std::map<int, rtabmap::Transform> & poses,
278  const rtabmap::Transform & targetPose,
279  float * distance = 0);
280 
289 std::map<int, float> RTABMAP_EXP findNearestNodes(
290  int nodeId,
291  const std::map<int, Transform> & poses,
292  float radius,
293  float angle = 0.0f,
294  int k=0);
295 std::map<int, float> RTABMAP_EXP findNearestNodes(
296  const Transform & targetPose,
297  const std::map<int, Transform> & poses,
298  float radius,
299  float angle = 0.0f,
300  int k=0);
301 std::map<int, Transform> RTABMAP_EXP findNearestPoses(
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, Transform> RTABMAP_EXP findNearestPoses(
308  const Transform & targetPose,
309  const std::map<int, Transform> & poses,
310  float radius,
311  float angle = 0.0f,
312  int k=0);
313 
314 // typedef hack to avoid error with RTABMAP_DEPRECATED
315 typedef std::map<int, float> _mapIntFloat;
316 typedef std::map<int, Transform> _mapIntTransform;
317 RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
318 RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
319 RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
320 RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
321 RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
322 
323 float RTABMAP_EXP computePathLength(
324  const std::vector<std::pair<int, Transform> > & path,
325  unsigned int fromIndex = 0,
326  unsigned int toIndex = 0);
327 
328 // assuming they are all linked in map order
329 float RTABMAP_EXP computePathLength(
330  const std::map<int, Transform> & path);
331 
332 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
333  std::map<int, Transform> poses,
334  const std::multimap<int, Link> & links);
335 
336 void RTABMAP_EXP computeMinMax(const std::map<int, Transform> & poses,
337  cv::Vec3f & min,
338  cv::Vec3f & max);
339 
340 } /* namespace graph */
341 
342 } /* namespace rtabmap */
343 #endif /* GRAPH_H_ */
rtabmap::graph::findLinks
std::list< Link > RTABMAP_EXP findLinks(const std::multimap< int, Link > &links, int from)
Definition: Graph.cpp:1121
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:1421
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1253
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:992
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_EXP 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
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
f
f
rtabmap::graph::exportGPS
bool RTABMAP_EXP exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:497
rtabmap::graph::findNearestNode
int RTABMAP_EXP findNearestNode(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
Definition: Graph.cpp:2085
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_EXP filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
Definition: Graph.cpp:1154
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::graph::filterDuplicateLinks
std::multimap< int, Link > RTABMAP_EXP filterDuplicateLinks(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1140
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
rtabmap::graph::_mapIntFloat
std::map< int, float > _mapIntFloat
Definition: Graph.h:315
rtabmap::graph::getNodesInRadius
std::map< int, float > getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:2230
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP 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
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_EXP getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:964
rtabmap::graph::getPosesInRadius
std::map< int, Transform > getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle)
Definition: Graph.cpp:2238
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_EXP computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1658
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1365
rtabmap::graph::exportPoses
bool RTABMAP_EXP 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::graph::computeMinMax
void RTABMAP_EXP computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2329
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::graph::calcRelativeErrors
void RTABMAP_EXP calcRelativeErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:717
rtabmap::graph::findNearestPoses
std::map< int, Transform > RTABMAP_EXP findNearestPoses(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2193
rtabmap::graph::importPoses
bool RTABMAP_EXP 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::getPaths
std::list< std::map< int, Transform > > RTABMAP_EXP getPaths(std::map< int, Transform > poses, const std::multimap< int, Link > &links)
Definition: Graph.cpp:2297
rtabmap::graph::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.")
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2248
rtabmap::graph::findNearestNodes
std::map< int, float > RTABMAP_EXP findNearestNodes(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2104
rtabmap::graph::frustumPosesFiltering
std::map< int, Transform > RTABMAP_EXP 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:1204
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP 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::_mapIntTransform
std::map< int, Transform > _mapIntTransform
Definition: Graph.h:316

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28