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), 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), 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 
130 std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);
131 
132 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
133  std::multimap<int, Link> & links,
134  int from,
135  int to,
136  bool checkBothWays = true);
137 std::multimap<int, int>::iterator RTABMAP_EXP findLink(
138  std::multimap<int, int> & links,
139  int from,
140  int to,
141  bool checkBothWays = true);
142 std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
143  const std::multimap<int, Link> & links,
144  int from,
145  int to,
146  bool checkBothWays = true,
147  Link::Type type = Link::kUndef);
148 std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
149  const std::multimap<int, int> & links,
150  int from,
151  int to,
152  bool checkBothWays = true);
153 std::list<Link> RTABMAP_EXP findLinks(
154  const std::multimap<int, Link> & links,
155  int from);
156 
157 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
158  const std::multimap<int, Link> & links);
162 std::multimap<int, Link> RTABMAP_EXP filterLinks(
163  const std::multimap<int, Link> & links,
164  Link::Type filteredType,
165  bool inverted = false);
169 std::map<int, Link> RTABMAP_EXP filterLinks(
170  const std::map<int, Link> & links,
171  Link::Type filteredType,
172  bool inverted = false);
173 
174 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
175 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
176  const std::map<int, Transform> & poses,
177  const Transform & cameraPose,
178  float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
179  float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
180  float nearClipPlaneDistance = 0.1f,
181  float farClipPlaneDistance = 100.0f,
182  bool negative = false);
183 
192 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
193  const std::map<int, Transform> & poses,
194  float radius,
195  float angle,
196  bool keepLatest = true);
197 
205 std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
206  const std::map<int, Transform> & poses,
207  float radius,
208  float angle);
209 
210 void reduceGraph(
211  const std::map<int, Transform> & poses,
212  const std::multimap<int, Link> & links,
213  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
214  std::multimap<int, Link> & hyperLinks);
215 
225 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
226  const std::map<int, rtabmap::Transform> & poses,
227  const std::multimap<int, int> & links,
228  int from,
229  int to,
230  bool updateNewCosts = false);
231 
242 std::list<int> RTABMAP_EXP computePath(
243  const std::multimap<int, Link> & links,
244  int from,
245  int to,
246  bool updateNewCosts = false,
247  bool useSameCostForAllLinks = false);
248 
258 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
259  int fromId,
260  int toId,
261  const Memory * memory,
262  bool lookInDatabase = true,
263  bool updateNewCosts = false,
264  float linearVelocity = 0.0f, // m/sec
265  float angularVelocity = 0.0f); // rad/sec
266 
274 int RTABMAP_EXP findNearestNode(
275  const std::map<int, rtabmap::Transform> & nodes,
276  const rtabmap::Transform & targetPose,
277  float * distance = 0);
278 
286 std::map<int, float> RTABMAP_EXP findNearestNodes(
287  const std::map<int, rtabmap::Transform> & nodes,
288  const rtabmap::Transform & targetPose,
289  int k);
290 
298 std::map<int, float> RTABMAP_EXP getNodesInRadius(
299  int nodeId,
300  const std::map<int, Transform> & nodes,
301  float radius);
302 std::map<int, float> RTABMAP_EXP getNodesInRadius(
303  const Transform & targetPose,
304  const std::map<int, Transform> & nodes,
305  float radius);
306 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
307  int nodeId,
308  const std::map<int, Transform> & nodes,
309  float radius,
310  float angle = 0.0f);
311 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
312  const Transform & targetPose,
313  const std::map<int, Transform> & nodes,
314  float radius,
315  float angle = 0.0f);
316 
317 float RTABMAP_EXP computePathLength(
318  const std::vector<std::pair<int, Transform> > & path,
319  unsigned int fromIndex = 0,
320  unsigned int toIndex = 0);
321 
322 // assuming they are all linked in map order
323 float RTABMAP_EXP computePathLength(
324  const std::map<int, Transform> & path);
325 
326 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
327  std::map<int, Transform> poses,
328  const std::multimap<int, Link> & links);
329 
330 
331 } /* namespace graph */
332 
333 } /* namespace rtabmap */
334 #endif /* GRAPH_H_ */
rtabmap::graph::findLinks
std::list< Link > RTABMAP_EXP findLinks(const std::multimap< int, Link > &links, int from)
Definition: Graph.cpp:1097
rtabmap::graph::getPosesInRadius
std::map< int, Transform > RTABMAP_EXP getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)
Definition: Graph.cpp:2173
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:1397
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:1229
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:478
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:1130
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:1116
rtabmap::graph::findNearestNode
int RTABMAP_EXP findNearestNode(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, float *distance=0)
Definition: Graph.cpp:2061
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)
Definition: Graph.cpp:874
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
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:737
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_EXP getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:941
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:1634
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1341
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::getNodesInRadius
std::map< int, float > RTABMAP_EXP getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:2113
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:969
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:695
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:170
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:2299
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:2250
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:1180
rtabmap::graph::findNearestNodes
std::map< int, float > RTABMAP_EXP findNearestNodes(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)
Definition: Graph.cpp:2079
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:627

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58